#include "control.h"
//#include "usart.h"
#include "delay.h"
typedef struct
{
int set_angle;//设定角度
int actual_angle;//实际角度
int error;//偏差
int error_next;//上一个偏差
int error_last;//上上一个偏差
float kp,ki,kd;//定义比例，积分，微分参数
}PID;
u8 hw0_state = 0,hw22_5_state = 0,hw45_state = 0,hw67_5_state = 0,hw90_state = 0;
u8 hw112_5_state = 0,hw135_state = 0,hw157_5_state = 0,hw180_state = 0;
extern u8 TIM2CH1_CAPTURE_STA; //输入捕获状态
extern u16 TIM2CH1_CAPTURE_VAL; //输入捕获值
extern u8 TIM2CH2_CAPTURE_STA; //输入捕获状态
extern u16 TIM2CH2_CAPTURE_VAL; //输入捕获值
extern u8 TIM2CH3_CAPTURE_STA; //输入捕获状态
extern u16 TIM2CH3_CAPTURE_VAL; //输入捕获值
extern u8 TIM3CH1_CAPTURE_STA; //输入捕获状态
extern u16 TIM3CH1_CAPTURE_VAL; //输入捕获值
extern u8 TIM3CH2_CAPTURE_STA; //输入捕获状态
extern u16 TIM3CH2_CAPTURE_VAL; //输入捕获值
extern u8 TIM3CH3_CAPTURE_STA; //输入捕获状态
extern u16 TIM3CH3_CAPTURE_VAL; //输入捕获值
extern u8 TIM3CH4_CAPTURE_STA; //输入捕获状态
extern u16 TIM3CH4_CAPTURE_VAL; //输入捕获值
extern u8 TIM4CH3_CAPTURE_STA; //输入捕获状态
extern u16 TIM4CH3_CAPTURE_VAL; //输入捕获值
extern u8 TIM4CH4_CAPTURE_STA; //输入捕获状态
extern u16 TIM4CH4_CAPTURE_VAL; //输入捕获值
u16 N1=0,N2=0;//N1左马达 N2右马达
signed int turn_angle = 0;
PID altpid;
void driver_init(void)
{
// GPIO_InitTypeDef GPIO_InitStructure2;
// GPIO_InitTypeDef GPIO_InitStructure3;
//
//
// GPIO_InitStructure1.GPIO_Mode=GPIO_Mode_IPD;//设置上拉输入
// GPIO_InitStructure1.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7;
// GPIO_InitStructure1.GPIO_Speed=GPIO_Speed_50MHz;
// GPIO_Init(GPIOA,&GPIO_InitStructure1);
// GPIO_SetBits(GPIOA,GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7); //初始化GPIOA1
//
// GPIO_InitStructure2.GPIO_Mode=GPIO_Mode_IPD;//设置上拉输入
// GPIO_InitStructure2.GPIO_Pin=GPIO_Pin_15;
// GPIO_InitStructure2.GPIO_Speed=GPIO_Speed_50MHz;
// GPIO_Init(GPIOC,&GPIO_InitStructure2);
// GPIO_SetBits(GPIOC,GPIO_Pin_15);
//
// GPIO_InitStructure2.GPIO_Mode=GPIO_Mode_Out_PP;//设置上拉输入
// GPIO_InitStructure2.GPIO_Pin=GPIO_Pin_14;
// GPIO_InitStructure2.GPIO_Speed=GPIO_Speed_50MHz;
// GPIO_Init(GPIOC,&GPIO_InitStructure2);
// GPIO_SetBits(GPIOC,GPIO_Pin_14);
/*
PA 2345
*/
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;//设置推挽输出
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStructure);
//GPIO_ResetBits(GPIOA,GPIO_Pin_7);//xia拉led
}
float Fabs(float j)
{
float a;
if(j<0)
a = -j;
else
a = j;
return a;
}
float limit(float value,float min,float max)
{
float value_new;
if(value > max)
value_new = max;
else if(value < min)
value_new = min;
else
value_new = value;
return value_new;
}
void sys_init()
{
// hw_high_GPIOA;
// hw_high_GPIOC;//红外上电默认不接收到信号
ENA_ENABLE;
ENB_ENABLE;//默认使能
LEFT_MOTOR_1_HIGH;
LEFT_MOTOR_2_LOW;
RIGHT_MOTOR_1_HIGH;
RIGHT_MOTOR_2_LOW;
}
void left_right_pwm(u16 n1,u16 n2)
{
LEFT_MOTOR_1_HIGH;
LEFT_MOTOR_2_LOW;
RIGHT_MOTOR_1_HIGH;
RIGHT_MOTOR_2_LOW;
TIM_SetCompare1(TIM4,n1);//left
TIM_SetCompare2(TIM4,n2);
}
void pid_realise_init(float k_p,float k_i,float k_d)//增量式PID初始化
{
altpid.set_angle = 0;
altpid.actual_angle = 0;
altpid.error = 0;
altpid.error_next = 0;
altpid.error_last = 0;
altpid.kp = k_p;
altpid.ki = k_i;
altpid.kd = k_d;
}
/*
PA 0/1 PB 0/1/4/5/8/9/10
*/
void Catch_hw_state()
{
if(TIM2CH1_CAPTURE_STA&0x80)//捕获到一个下降沿、、
{
hw0_state = 1;//说明0度红外收到信号，便于给权重
TIM2CH1_CAPTURE_STA=0;//开启下一次捕获
}
else hw0_state = 0;
if(TIM2CH2_CAPTURE_STA&0x80)//捕获到一个下降沿、、
{
hw22_5_state = 1;//说明0度红外收到信号，便于给权重
TIM2CH2_CAPTURE_STA=0;//开启下一次捕获
}
else hw22_5_state = 0;
if(TIM3CH3_CAPTURE_STA&0x80)//捕获到一个下降沿
{
hw45_state = 1;//说明0度红外收到信号，便于给权重
TIM3CH3_CAPTURE_STA=0;//开启下一次捕获
}
else hw45_state = 0;
if(TIM3CH4_CAPTURE_STA&0x80)//捕获到一个下降沿
{
hw67_5_state = 1;//说明0度红外收到信号，便于给权重
TIM3CH4_CAPTURE_STA=0;//开启下一次捕获
}
else hw67_5_state = 0;
if(TIM3CH1_CAPTURE_STA&0x80)//捕获到一个下降沿
{
hw90_state = 1;//说明0度红外收到信号，便于给权重
TIM3CH1_CAPTURE_STA=0;//开启下一次捕获
}
else hw90_state = 0;
if(TIM3CH2_CAPTURE_STA&0x80)//捕获到一个下降沿 、、
{
hw112_5_state = 1;//说明0度红外收到信号，便于给权重
TIM3CH2_CAPTURE_STA=0;//开启下一次捕获
}
else hw112_5_state = 0;
if(TIM4CH3_CAPTURE_STA&0x80)//捕获到一个下降沿、、
{
hw135_state = 1;//说明0度红外收到信号，便于给权重
TIM4CH3_CAPTURE_STA=0;//开启下一次捕获
}
else hw135_state = 0;
if(TIM4CH4_CAPTURE_STA&0x80)//捕获到一个下降沿
{
hw157_5_state = 1;//说明0度红外收到信号，便于给权重
TIM4CH4_CAPTURE_STA=0;//开启下一次捕获
}
else hw157_5_state = 0;
if(TIM2CH3_CAPTURE_STA&0x80)//捕获到一个下降沿、、
{
hw180_state = 1;//说明0度红外收到信号，便于给权重
TIM2CH3_CAPTURE_STA=0;//开启下一次捕获
}
else hw180_state = 0;
}
int Calculate(void)
{
unsigned int weight_num = 0;//记录红外接收到的个数
static int times = 0,turn_angle_num;//滤波
//Catch_hw_state();// CATCH AGAIN
weight_num = hw0_state
+ hw22_5_state
+ hw45_state
+ hw67_5_state
+ hw90_state
+ hw112_5_state
+ hw135_state
+ hw157_5_state
+ hw180_state;
if(weight_num==0)//全没接收到
{
times = 0;
turn_angle_num = 0;
}
else
{
turn_angle = ( hw0_weight*hw0_state //50
+ hw22_5_weight*hw22_5_state//20*
+ hw45_weight*hw45_state//30*
+ hw67_5_weight*hw67_5_state//40*
+ hw90_weight*hw90_state//50*
+ hw112_5_weight*hw112_5_state//60*
+ hw135_weight*hw135_state//70*
+ hw157_5_weight*hw157_5_state//80*
+ hw180_weight*hw180_state
)/weight_num-50;
turn_angle_num+=turn_angle;//50
times++;
if(times==5)
{
times = 0;
turn_angle = turn_angle_num/5;
turn_angle_num=0;
}
}
return turn_angle;
}
void pid_realise_control(int angle) // 增量式
{
float increment_speed = 0.0;//increment_speed_abs;//增量
u16 increment_speed_fab;
//Catch_hw_state();// CATCH AGAIN
altpid.actual_angle = Calculate();//反馈值
altpid.set_angle = angle;//设置目标速度
altpid.error = altpid.set_angle - altpid.actual_angle;//set 0 error>0 left
// 0.2 0.01 0.2
increment_speed = altpid.kp*(altpid.error-altpid.error_next)+altpid.ki*altpid.error+altpid.kd*(altpid.error-2*altpid.error_next+altpid.error_last);//增量计算公式
altpid.error_last = altpid.error_next;//上上次误差
altpid.error_next = altpid.error;//上一次误差
increment_speed_fab = 100*Fabs(limit(increment_speed,-10,10));//限幅
if(altpid.error > 0)//turn left
{
N1 = angle_turn_N1 + increment_speed_fab;
N2 = angle_turn_N2;
//left_right_pwm(N1,N2);
printf("N1=%d\r\n",N1);
printf("N2=%d\r\n",N2);
}
if(altpid.error < 0)//turn right
{
N1 = angle_turn_N1;
N2 = angle_turn_N2 + increment_speed_fab;
//left_right_pwm(N1,N2);
printf("N1=%d\r\n",N1);
printf("N2=%d\r\n",N2);
}
else
{
N1 = angle_turn_N1;
N2 = angle_turn_N2;
}
//
printf("altpid.actual_angle=%d\r\n",altpid.actual_angle);
printf("altpid.error=%d\r\n",altpid.error);
printf("increment_speed=%f\r\n",increment_speed);
printf("altpid.error_last=%d\r\n",altpid.error_last);
printf("altpid.error_next=%d\r\n",altpid.error_next);
printf("increment_speed_fab=%d\r\n",increment_speed_fab);
}
//void pid_control()
//{
// pid_realise_control(0);
// left_right_pwm(N1,N2);
// delay_ms(200);
//
//}
全部程序51hei下载地址(缺少原理图)：
32_boat.7z
(200.45 KB, 下载次数: 12)
|
