基于FreeRTOS的四麦_循迹_视觉_夹取小车

目录

​思维导图

1.材料

2.PID算法控制电机闭环

1).调用函数让TIM读取CNT寄存器的数值。

2).从APP接收到的字符串进行XYZ三轴分解。

3).PID算法得出PWM,后赋值给结构体。

3.循迹(PID控制,卡尔曼滤波)

4.对树莓派3B 传输的数据进行处理

5.APP对舵机控制


基于FreeRTOS的四麦_循迹_视觉_夹取小车_第1张图片

1.材料

stm32vet6主控板,树莓派3b八路循迹模块,TB6612电机驱动 x 2,降压模块(建议焊急停开关)注:不要用杜邦线接12v

2.PID算法控制电机闭环

PID(proportion integration differentiation)其实就是指比例,积分,微分控制。(图片源于网络)

基于FreeRTOS的四麦_循迹_视觉_夹取小车_第2张图片

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_count

5.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;
	 }

你可能感兴趣的:(单片机,stm32,嵌入式硬件)