基于FreeRTOS的四麦_循迹_视觉_夹取小车
目录
思维导图
1.材料
2.PID算法控制电机闭环
1).调用函数让TIM读取CNT寄存器的数值。
2).从APP接收到的字符串进行XYZ三轴分解。
3).PID算法得出PWM，后赋值给结构体。
3.循迹（PID控制，卡尔曼滤波）
4.对树莓派3B 传输的数据进行处理
5.APP对舵机控制
1.材料
stm32vet6主控板，树莓派3b，八路循迹模块，TB6612电机驱动 x 2，降压模块（建议焊急停开关）注：不要用杜邦线接12v
2.PID算法控制电机闭环
PID(proportion integration differentiation)其实就是指比例，积分，微分控制。(图片源于网络)
1).调用函数让TIM读取CNT寄存器的数值。
//读取编码器计数
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0; break;
case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0; break;
case 4: Encoder_TIM= (short)TIM4 -> CNT; TIM4 -> CNT=0; break;
case 5: Encoder_TIM= (short)TIM5 -> CNT; TIM5 -> CNT=0; break;
default: Encoder_TIM=0;
}
return Encoder_TIM;//速度
}将读取到的数值，赋值给结构体（结构体可提高封装效果）
//编码器数据转换为车轮速度，单位m/s
void Get_Velocity_Form_Encoder(void)
{
float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr;
OriginalEncoder.A=Read_Encoder(2);
OriginalEncoder.B=Read_Encoder(3);
OriginalEncoder.C=Read_Encoder(4);
OriginalEncoder.D=Read_Encoder(5);
switch(Car_Mode)
{
case Mec_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C; Encoder_D_pr=-OriginalEncoder.D; break;
}
MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
}
typedef struct
{
float Encoder;
float Motor_Pwm;
float Target;
float Velocity_KP;
float Velocity_KI;
}Motor_parameter;2).从APP接收到的字符串进行XYZ三轴分解。
在运动学分析中，将目标速度赋值给结构体。
void Drive_Motor(float Vx,float Vy,float Vz)
{
float amplitude=1.3; //目标速度
if (Car_Mode==Mec_Car)
{
//运动学逆解
MOTOR_A.Target = +Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
MOTOR_B.Target = -Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
MOTOR_C.Target = +Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
MOTOR_D.Target = -Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
//电机目标速度限幅
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-amplitude,amplitude);
}
}3).PID算法得出PWM，后赋值给结构体。
//pid pwm+=Kp[e（k）-e(k-1)]+Ki*e(k) pwm=pwm+Kp[e（k）-e(k-1)]+Ki*e(k)
int Incremental_PI_A (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //计算偏差
MOTOR_A.Velocity_KP=300;
MOTOR_A.Velocity_KI=300;
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //保存上一次偏差
return Pwm;
}
int Incremental_PI_B (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder;
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias;
return Pwm;
}
int Incremental_PI_C (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder;
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias;
return Pwm;
}
int Incremental_PI_D (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder;
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias;
return Pwm;
} MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);
赋值给CRR寄存器，封装TIM寄存器。
#define PWM_PORTA1 GPIOB //PWMA
#define PWM_PIN_A1 GPIO_Pin_8
#define PWMA1 TIM10->CCR1
#define PWM_PORTA2 GPIOB //PWMA
#define PWM_PIN_A2 GPIO_Pin_9
#define PWMA2 TIM11->CCR1
#define PWM_PORTB1 GPIOE //PWMB
#define PWM_PIN_B1 GPIO_Pin_5
#define PWMB1 TIM9->CCR1
#define PWM_PORTB2 GPIOE //PWMB
#define PWM_PIN_B2 GPIO_Pin_6
#define PWMB2 TIM9->CCR2
#define PWM_PORTC1 GPIOE //PWMC
#define PWM_PIN_C1 GPIO_Pin_11
#define PWMC1 TIM1->CCR2
#define PWM_PORTC2 GPIOE //PWMC
#define PWM_PIN_C2 GPIO_Pin_9
#define PWMC2 TIM1->CCR1
#define PWM_PORTD1 GPIOE //PWMD
#define PWM_PIN_D1 GPIO_Pin_14
#define PWMD1 TIM1->CCR4
#define PWM_PORTD2 GPIOE //PWMD
#define PWM_PIN_D2 GPIO_Pin_13
#define PWMD2 TIM1->CCR3
Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm );
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d)
{
if(motor_a<0) PWMA1=16799,PWMA2=16799+motor_a;
else PWMA2=16799,PWMA1=16799-motor_a;
if(motor_b<0) PWMB1=16799,PWMB2=16799+motor_b;
else PWMB2=16799,PWMB1=16799-motor_b;
if(motor_c<0) PWMC1=16799,PWMC2=16799+motor_c;
else PWMC2=16799,PWMC1=16799-motor_c;
if(motor_d<0) PWMD1=16799,PWMD2=16799+motor_d;
else PWMD2=16799,PWMD1=16799-motor_d;
}电机成功闭环！
3.循迹（PID控制，卡尔曼滤波）
初始化一个数组更方便调试，读取循迹电平。
void check_LED()
{
xunji1[0]=L4;
xunji1[1]=L3;
xunji1[2]=L2;
xunji1[3]=L1;
xunji1[4]=R1;
xunji1[5]=R2;
xunji1[6]=R3;
xunji1[7]=R4;
}最简单的直接，条件语句判断赋值。
void xunji()
{
check_LED();
if(L2==0&&L1==0&&R1==0&&R2==0) // --00 00--
{
Drive_Motor(0, 0, 0);
}
else if(L4==1&&L3==1&&L2==0&&L1==0&&R1==0&&R2==0&&R3==1&&R4==1) //停车线 1100 0011
{
Drive_Motor(0, 0, 0);
}
//--------------------------------------------【直线】---------------------------------------------------------------------------------
else if(L4==1&&L3==1&&L2==1&&L1==0&&R1==0&&R2==1&&R3==1&&R4==1)// 直线 1110 0111
{
Drive_Motor(-0.4, 0, 0);
}
else if(L4==1&&L3==1&&L2==0&&L1==0&&R1==0&&R2==1&&R3==1&&R4==1)// 直线 1100 0111
{
Drive_Motor(-0.35, 0, 0);
}
else if(L4==1&&L3==1&&L2==1&&L1==0&&R1==0&&R2==0&&R3==1&&R4==1)// 直线 1110 0011
{
Drive_Motor(-0.35, 0, 0);
}
else if(L1==0)// 直线 ---0 ----
{
Drive_Motor(-0.475, 0, 0);
}
else if(R1==0 )// 直线 ---- 0---
{
Drive_Motor(-0.475, 0, 0);
}
else if(L4==1&&L3==1&&L2==1&&L1==0&&R1==1&&R2==1&&R3==1&&R4==1)// 直线 1100 0111
{
Drive_Motor(-0.3, 0, 0);
}
else if(L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==1&&R3==1&&R4==1)// 直线 1110 0011
{
Drive_Motor(-0.3, 0, 0);
}
else if(L4==1&&L3==1&&L2==0&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1)// 小左偏2 （直线） Load(-1000,-0);
{
Drive_Motor(-0.025, -0.475, 0);
}
else if(L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==0&&R3==1&&R4==1)// 中左偏2 （直线） Load(-1500,-0);
{
Drive_Motor(-0.025, 0.475, 0);
}
else if(L4==1&&L3==1&&L2==0&&L1==0&&R1==1&&R2==1&&R3==1&&R4==1)// （直线） Load(-1000,-0); 1100 1111
{
Drive_Motor(-0.4, 0, 0);
}
else if(L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==0&&R3==1&&R4==1)// （直线） Load(-1000,-0); 1111 0011
{
Drive_Motor(-0.4, 0, 0);
}
//--------------------------------------------【左偏】---------------------------------------------------------------------------------
else if(L4==1&&L3==0&&L2==0&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1)// 中左偏 1001 1111
{
Drive_Motor(-0.02, 0, 1.05);
}
else if(L4==0&&L3==0&&L2==1&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1)// 大左偏 0011 1111
{
Drive_Motor(-0.02, 0, 1.3);
}
else if(L4==0&&L3==0&&L2==0&&L1==1&&R1==1&&R2==1&&R3==1&&R4==1)//左边很转弯（直角弯） 0001 1111
{
Drive_Motor(-0.02, 0, 1.3);
}
else if(L4==0&&L3==0&&L2==0&&L1==0&&R1==1&&R2==1&&R3==1&&R4==1)//左边急转弯（直角弯） 0000 1111
{
Drive_Motor(-0.5, 0, 1.35);
}
else if(L4==1&&L3==0&&L2==0&&L1==0&&R1==1&&R2==1&&R3==1&&R4==1)//右边超急转弯 1000 1111
{
Drive_Motor(-0.1, 0, 1.35);
}
else if(L4==1&&L3==0&&L2==0&&L1==0&&R1==0&&R2==1&&R3==1&&R4==1)//左边超急转弯 1000 0111
{
Drive_Motor(-0.1, 0, 1.4);
}
//
// else if(L3==0)// 大左偏2 Load(-1500,-0); -0-- ----
// {
// Drive_Motor(-0.1, 0, 1.325);
//
// }
// else if(L4==0)// 大左偏2 Load(-1500,-0); 0--- ----
// {
// Drive_Motor(-0.1, 0, 1.375);
//
// }
// ----------------------------------------【右偏】---------------------------------------------------------------------------------------
else if(L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==0&&R3==0&&R4==1)// 中右偏 1111 1001
{
Drive_Motor(-0.02, 0, -1.05);
}
else if(L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==1&&R3==0&&R4==0)// 大右偏 1111 1100
{
Drive_Motor(-0.02, 0, -1.3);
}
else if(L4==1&&L3==1&&L2==1&&L1==1&&R1==1&&R2==0&&R3==0&&R4==0)//右边很转弯 1111 1000
{
Drive_Motor(-0.02, 0, -1.325);
}
else if(L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==0&&R3==0&&R4==0)//右边急转弯 1111 0000
{
Drive_Motor(-0.05, 0, -1.35);
}
else if(L4==1&&L3==1&&L2==1&&L1==1&&R1==0&&R2==0&&R3==0&&R4==1)//右边超急转弯 1111 0001
{
Drive_Motor(-0.1, 0, -1.375);
}
else if(L4==1&&L3==1&&L2==1&&L1==0&&R1==0&&R2==0&&R3==0&&R4==1)//右边超急转弯 1110 0001
{
Drive_Motor(-0.1, 0, -1.4 );
}
// else if(R3==0)// 大右偏2 ---- --0-
// {
// Drive_Motor(-0.1, 0, -1.325);
// }
// else if(R4==0)// 大右偏2 ---- ---0
// {
// Drive_Motor(-0.1, 0, -1.375);
// }
} 4.对树莓派3B 传输的数据进行处理
if(ShiJue=='s')
{
// if(shijue1<=10)
// {
// Drive_Motor(-0.3,0,0);
// }
// if(10开启串口中断，赋值给变量，对应变量进行控制
int USART3_IRQHandler(void)//串口三与视觉通信的中断
{
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
{
ShiJue = USART_ReceiveData(USART3); //读取数据
if(Time_count5.APP对舵机控制
switch(Flag_Direction)
{
case 1: Move_X=RC_Velocity; Move_Y=0; break;
case 2: Move_X=RC_Velocity; Move_Y=-RC_Velocity; break;
case 17: Move_Z= -PI/2*(RC_Velocity/500); break;
case 3: Move_X=0; Move_Y=-RC_Velocity; break;
case 5: Move_X=-RC_Velocity; Move_Y=0; break;
case 7: Move_X=0; Move_Y=RC_Velocity; break;
case 18: Move_Z= PI/2*(RC_Velocity/500); break;
case 8: Move_X=RC_Velocity; Move_Y=RC_Velocity; break;
case 9: mark1=1; APP_ON_Flag=0; break;
case 10: TIM_SetCompare3(TIM8, 2100); break;
case 11: TIM_SetCompare3(TIM8,1150); break;
case 12: TIM_SetCompare1(TIM12, 2400); break;
case 19: TIM_SetCompare1(TIM12, 1500); break;
case 13: TIM_SetCompare1(TIM12, 600); break;
case 14: TIM_SetCompare4(TIM8, 1450); break;
case 15: TIM_SetCompare4(TIM8, 1550); break;
case 16: TIM_SetCompare4(TIM8, 1500); break;
case 4: Move_X=-RC_Velocity; Move_Y=-RC_Velocity; break;
case 6: Move_X=-RC_Velocity; Move_Y=RC_Velocity; break;
default: Move_X=0; Move_Y=0; Move_Z=0; break;
}