devbox
Monodyee
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

基于stm32的平衡小车_stm32平衡车

作者：Monodyee | 2024-04-08 16:25:03

赞

踩

stm32平衡车

目录

前言

一、电机驱动部分

1、TB6612FNG电机驱动模块接线方式：

2、代码使用定时器2的4路输出pwm

3、gpio引脚初始化，以及前进，后退引脚设置

二、MPU6050陀螺仪部分

三、编码器捕获部分

四、pid部分

1、直立环KD

2、速度环KI

3、转向环（PD）

五、蓝牙通信部分

总结

前言

经过几天对平衡车的理论学习和动手实践，终于完成了平衡车的基本功能，实现前进，后退，直立，转向。本项目用到了两个电机，一个两块亚力克板，一个mpu6050陀螺仪，TB6612FNG电机驱动模块，通信方式使用蓝牙模块进行简单的通信，主控芯片使用stm32f103c8t6。

一、电机驱动部分

1、TB6612FNG电机驱动模块接线方式：

VM 接12V电源，给电机供电

STBY 置1使能AIN1，AIN2，BIN1，BIN2

VCC 接5V电源

GND 接地

AO1，AO2 输出控制电机1正反转

BO1，BO2 输入控制电机2正反转

PWMA 控制AO1，AO2使能

PWMB 控制BO1，BO2使能

AIN1，AIN2 输入控制控制电机1正反转

BIN1，BIN2 输入控制控制电机2正反转

2、代码使用定时器2的4路输出pwm

具体代码如下

  1. void time2_pwm_init(uint16_t arr,uint16_t pre)
  2. {
  3.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);//定时器2使能
  4.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//gpio引脚使能
  5.     
  6.     GPIO_InitTypeDef gpio_init={0};
  7.     gpio_init.GPIO_Pin=GPIO_Pin_2 | GPIO_Pin_3;
  8.     gpio_init.GPIO_Mode=GPIO_Mode_AF_PP;
  9.     gpio_init.GPIO_Speed=GPIO_Speed_50MHz;
  10.     GPIO_Init(GPIOA,&gpio_init);
  11.     
  12.     TIM_TimeBaseInitTypeDef tim2_base={0};
  13.     tim2_base.TIM_ClockDivision=TIM_CKD_DIV1;
  14.     tim2_base.TIM_CounterMode=TIM_CounterMode_Up;
  15.     tim2_base.TIM_Period=arr;
  16.     tim2_base.TIM_Prescaler=pre;
  17.     tim2_base.TIM_RepetitionCounter=DISABLE;
  18.     TIM_TimeBaseInit(TIM2,&tim2_base);
  19.     
  20.     TIM_OCInitTypeDef time2_oc={0};
  21.     time2_oc.TIM_OCMode=TIM_OCMode_PWM1;
  22.     time2_oc.TIM_OCPolarity=TIM_OCPolarity_High;
  23.     time2_oc.TIM_Pulse=0;
  24.     time2_oc.TIM_OutputState=TIM_OutputState_Enable;
  25.     TIM_OC3Init(TIM2,&time2_oc);
  26.     TIM_OC4Init(TIM2,&time2_oc);
  27.     
  28.     TIM_Cmd(TIM2,ENABLE);//启动定时器
  29. }

3、gpio引脚初始化，以及前进，后退引脚设置

代码

  1. void mator_gpio_init(void)
  2. {
  3.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
  4.     GPIO_InitTypeDef gpio_init={0};
  5.     gpio_init.GPIO_Pin=GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
  6.     gpio_init.GPIO_Mode=GPIO_Mode_Out_PP;
  7.     gpio_init.GPIO_Speed=GPIO_Speed_50MHz;
  8.     GPIO_Init(GPIOB,&gpio_init);
  9.     
  10.     GPIO_WriteBit(GPIOB,GPIO_Pin_12,Bit_RESET);
  11.     GPIO_WriteBit(GPIOB,GPIO_Pin_13,Bit_RESET);
  12.     GPIO_WriteBit(GPIOB,GPIO_Pin_14,Bit_RESET);
  13.     GPIO_WriteBit(GPIOB,GPIO_Pin_15,Bit_RESET);
  14. }
  15.  
  16. void mator_forward(void)
  17. {
  18.     GPIO_WriteBit(GPIOB,GPIO_Pin_12,Bit_SET);
  19.     GPIO_WriteBit(GPIOB,GPIO_Pin_13,Bit_RESET);
  20.     GPIO_WriteBit(GPIOB,GPIO_Pin_14,Bit_RESET);
  21.     GPIO_WriteBit(GPIOB,GPIO_Pin_15,Bit_SET);
  22. }
  23.  
  24. void mator_back(void)
  25. {
  26.     GPIO_WriteBit(GPIOB,GPIO_Pin_12,Bit_RESET);
  27.     GPIO_WriteBit(GPIOB,GPIO_Pin_13,Bit_SET);
  28.     GPIO_WriteBit(GPIOB,GPIO_Pin_14,Bit_SET);
  29.     GPIO_WriteBit(GPIOB,GPIO_Pin_15,Bit_RESET);
  30. }

二、MPU6050陀螺仪部分

主要使用的是mpu6050官方的dmp库

代码如下

  1. //采集俯仰角，翻滚角，偏航角
  2. void MPU6050_Pose(void)
  3. {
  4. 	
  5. 	dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);	 
  6. 	
  7. 	if(sensors & INV_WXYZ_QUAT )
  8. 	{
  9. 		q0 = quat[0] / q30;	
  10. 		q1 = quat[1] / q30;
  11. 		q2 = quat[2] / q30;
  12. 		q3 = quat[3] / q30;
  13.  
  14. 		Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
  15. 		Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)*57.3;	// roll
  16. 		Yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
  17. 		
  18. 	}
  19. }
  20.  
  21.  
  22. //mpu6050初始化
  23. void MPU6050_Init(void)
  24. {
  25. 	int result=0;
  26. 	//IIC_Init();
  27. 	result=mpu_init();
  28. 	if(!result)
  29. 	{	 		 
  30. 	
  31. 		printf("mpu initialization complete......\n ");		//mpu initialization complete	 	  
  32.  
  33. 		if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))		//mpu_set_sensor
  34. 			printf("mpu_set_sensor complete ......\n");
  35. 		else
  36. 			printf("mpu_set_sensor come across error ......\n");
  37.  
  38. 		if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))	//mpu_configure_fifo
  39. 			printf("mpu_configure_fifo complete ......\n");
  40. 		else
  41. 			printf("mpu_configure_fifo come across error ......\n");
  42.  
  43. 		if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))	   	  		//mpu_set_sample_rate
  44. 		 printf("mpu_set_sample_rate complete ......\n");
  45. 		else
  46. 		 	printf("mpu_set_sample_rate error ......\n");
  47.  
  48. 		if(!dmp_load_motion_driver_firmware())   	  			//dmp_load_motion_driver_firmvare
  49. 			printf("dmp_load_motion_driver_firmware complete ......\n");
  50. 		else
  51. 			printf("dmp_load_motion_driver_firmware come across error ......\n");
  52.  
  53. 		if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) 	  //dmp_set_orientation
  54. 		 	printf("dmp_set_orientation complete ......\n");
  55. 		else
  56. 		 	printf("dmp_set_orientation come across error ......\n");
  57.  
  58. 		if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
  59. 		    DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
  60. 		    DMP_FEATURE_GYRO_CAL))		   	 					 //dmp_enable_feature
  61. 		 	printf("dmp_enable_feature complete ......\n");
  62. 		else
  63. 		 	printf("dmp_enable_feature come across error ......\n");
  64.  
  65. 		if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))   	 			 //dmp_set_fifo_rate
  66. 		 	printf("dmp_set_fifo_rate complete ......\n");
  67. 		else
  68. 		 	printf("dmp_set_fifo_rate come across error ......\n");
  69.  
  70. 		run_self_test();		//自检
  71.  
  72. 		if(!mpu_set_dmp_state(1))
  73. 		 	printf("mpu_set_dmp_state complete ......\n");
  74. 		else
  75. 		 	printf("mpu_set_dmp_state come across error ......\n");
  76. 	}
  77. 	else												 //MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
  78. 	 {
  79. 	 GPIO_ResetBits(GPIOC, GPIO_Pin_13);				//MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
  80. 	 while(1);
  81. 	 }
  82. 	 
  83. }

三、编码器捕获部分

使用定时器3，和4的通道1和通道2进行4倍频地计数，主要代码如下：

  1. void time3_encoder_init()
  2. {
  3.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);//定时器2使能
  4.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//gpio引脚使能
  5.     
  6.     GPIO_InitTypeDef gpio_init={0};
  7.     gpio_init.GPIO_Pin=GPIO_Pin_6 | GPIO_Pin_7;
  8.     gpio_init.GPIO_Mode=GPIO_Mode_IN_FLOATING;
  9.     gpio_init.GPIO_Speed=GPIO_Speed_50MHz;
  10.     GPIO_Init(GPIOA,&gpio_init);
  11.     
  12.     TIM_TimeBaseInitTypeDef time3_base={0};
  13.     time3_base.TIM_ClockDivision=TIM_CKD_DIV1;
  14.     time3_base.TIM_CounterMode=TIM_CounterMode_Up;
  15.     time3_base.TIM_Period=65535;
  16.     time3_base.TIM_Prescaler=0;
  17.     time3_base.TIM_RepetitionCounter=DISABLE;
  18.     TIM_TimeBaseInit(TIM3,&time3_base);
  19.     
  20.     TIM_ICInitTypeDef time3_ic={0};
  21.     time3_ic.TIM_Channel=TIM_Channel_1;
  22.     time3_ic.TIM_ICFilter=0;
  23.     time3_ic.TIM_ICPolarity=TIM_ICPolarity_Rising;
  24.     time3_ic.TIM_ICPrescaler=TIM_ICPSC_DIV1;
  25.     time3_ic.TIM_ICSelection=TIM_ICSelection_DirectTI;
  26.     TIM_ICInit(TIM3,&time3_ic);
  27.     time3_ic.TIM_Channel=TIM_Channel_2;
  28.     TIM_ICInit(TIM3,&time3_ic);
  29.     TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器计数。BothEdge（底部边缘）
  30.     
  31.     //初始化标志位,计数器
  32.     TIM_ClearFlag(TIM3,TIM_FLAG_Update);//清除标志位
  33.     TIM_SetCounter(TIM3,0);//TIM3->CNT=0;
  34.     
  35.     //配置中断
  36.     NVIC_InitTypeDef nvic_init={0};
  37.     nvic_init.NVIC_IRQChannel=TIM3_IRQn;//中断通道
  38.     nvic_init.NVIC_IRQChannelCmd=ENABLE;//中断使能
  39.     nvic_init.NVIC_IRQChannelPreemptionPriority=2;//抢占优先级；
  40.     nvic_init.NVIC_IRQChannelSubPriority=1;//响应优先级
  41.     NVIC_Init(&nvic_init);
  42.     
  43.     TIM_ITConfig(TIM3,TIM_IT_Update | TIM_IT_CC1 |TIM_IT_CC2,ENABLE);//配置定时器，允许更新中断,CC1,CC2捕获中断
  44.     TIM_Cmd(TIM3,ENABLE);//开启定时器
  45. }
  46.  
  47. void time4_encoder_init()
  48. {
  49.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//定时器2使能
  50.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//gpio引脚使能
  51.     
  52.     GPIO_InitTypeDef gpio_init={0};
  53.     gpio_init.GPIO_Pin=GPIO_Pin_6 | GPIO_Pin_7;
  54.     gpio_init.GPIO_Mode=GPIO_Mode_IN_FLOATING;
  55.     gpio_init.GPIO_Speed=GPIO_Speed_50MHz;
  56.     GPIO_Init(GPIOB,&gpio_init);
  57.     
  58.     TIM_TimeBaseInitTypeDef time4_base={0};
  59.     time4_base.TIM_ClockDivision=TIM_CKD_DIV1;
  60.     time4_base.TIM_CounterMode=TIM_CounterMode_Up;
  61.     time4_base.TIM_Period=65535;
  62.     time4_base.TIM_Prescaler=0;
  63.     time4_base.TIM_RepetitionCounter=DISABLE;
  64.     TIM_TimeBaseInit(TIM4,&time4_base);
  65.     
  66.     TIM_ICInitTypeDef time4_ic={0};
  67.     time4_ic.TIM_Channel=TIM_Channel_1;
  68.     time4_ic.TIM_ICFilter=0;
  69.     time4_ic.TIM_ICPolarity=TIM_ICPolarity_Rising;
  70.     time4_ic.TIM_ICPrescaler=TIM_ICPSC_DIV1;
  71.     time4_ic.TIM_ICSelection=TIM_ICSelection_DirectTI;
  72.     TIM_ICInit(TIM4,&time4_ic);
  73.     time4_ic.TIM_Channel=TIM_Channel_2;
  74.     TIM_ICInit(TIM4,&time4_ic);
  75.     TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器计数。BothEdge（底部边缘）
  76.     
  77.     //初始化标志位,计数器
  78.     TIM_ClearFlag(TIM4,TIM_FLAG_Update);//清除标志位
  79.     TIM_SetCounter(TIM4,0);//TIM3->CNT=0;
  80.     
  81.     //配置中断
  82.     NVIC_InitTypeDef nvic_init={0};
  83.     nvic_init.NVIC_IRQChannel=TIM4_IRQn;//中断通道
  84.     nvic_init.NVIC_IRQChannelCmd=ENABLE;//中断使能
  85.     nvic_init.NVIC_IRQChannelPreemptionPriority=2;//抢占优先级；
  86.     nvic_init.NVIC_IRQChannelSubPriority=1;//响应优先级
  87.     NVIC_Init(&nvic_init);
  88.     
  89.     TIM_ITConfig(TIM4,TIM_IT_Update | TIM_IT_CC1 |TIM_IT_CC2,ENABLE);//配置定时器，允许更新中断,CC1,CC2捕获中断
  90.     TIM_Cmd(TIM4,ENABLE);//开启定时器
  91. }

四、pid部分

使用定时器1每隔5ms进行一次mpu6050的数据采集，并进行直立环控制，每40ms，进行一次速度环控制，防止影响直立控制。

1、直立环KD

小车的偏转角度作为Kp的系数，角加速度作为Kd的系数

代码如下：

  1. float angle_pid_realize(struct _pid *pid,float angle)
  2. {
  3.     if(Pitch==0||Pitch<-20||Pitch>20)              //MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
  4.     {
  5.           //Pitch=1;
  6.       GPIO_ResetBits(GPIOC, GPIO_Pin_13);          //MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
  7.     }
  8.     else 
  9.     {GPIO_SetBits(GPIOC, GPIO_Pin_13);}             //MPU6050状态正常时LED灯熄灭
  10.     
  11.     pid->err=angle-pid->target_val;//计算目标值与实际值的误差
  12.     pid->actual_val=pid->Kp*pid->err+pid->Kd*gyro[0];//角度PD控制,gyro[0]x轴偏转角速度
  13.     return pid->actual_val;
  14. }

2、速度环KI

速度环控制小车的位移，实现定点停下的功能，代码如下

  1. void speed_control(void)
  2. {
  3.     mator.car_speed = (mator.left_tal_count  + mator.right_tal_count );// * 0.5 ;          //左右电机脉冲数平均值作为小车当前车速
  4.     mator.left_tal_count =mator.right_tal_count = 0;      //全局变量 注意及时清零        
  5.     BST_fCarSpeedOld *= 0.7;//一阶滤波，占上次的70%
  6.     BST_fCarSpeedOld +=mator.car_speed*0.3;//一阶滤波，占本次的30%
  7.     
  8.     BST_fCarPosition += BST_fCarSpeedOld;          //路程  即速度积分       1/11 3:03
  9.     BST_fCarPosition +=BST_fBluetoothSpeed;
  10.     
  11.     //积分上限设限//
  12.     if((s32)BST_fCarPosition > SPEED_INTEGRAL_MAX)    BST_fCarPosition = SPEED_INTEGRAL_MAX;
  13.     if((s32)BST_fCarPosition < SPEED_INTEGRAL_MIN)    BST_fCarPosition = SPEED_INTEGRAL_MIN;
  14.                                                                                                   
  15.     mator.speed_pwm = (BST_fCarSpeedOld -CAR_SPEED_SET) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出
  16. }

3、转向环（PD）

主要是偏航角作为Kd的系数

代码如下

  1. float turn_pid_realize(uint8_t ccd,short yaw)
  2. {
  3.     float turn=0;
  4.     turn=(Turn_val-yaw)*Turn_Kd;
  5.     //printf("turn=%d\r\n",yaw);
  6.     return turn;
  7. }

五、蓝牙通信部分

使用串口3与蓝牙进行通信，手机通过蓝牙助手给蓝牙发送消息，蓝牙模块通过串口发送消息到小车。

通信协议代码如下

  1. void USART3_IRQHandler(void)                	//串口x中断服务程序
  2. {
  3.     uint8_t Res;
  4. //    uint8_t i;
  5.     if(USART_GetITStatus(USART3,USART_IT_RXNE) !=  RESET)//判断中断位
  6.     {
  7.         USART_ClearITPendingBit(USART3, USART_IT_RXNE);
  8.         Res = USART_ReceiveData(USART3); //接收数据
  9.         if(Res!='\n')
  10.         {
  11.             rx_buf[rx_size++]=Res;
  12.         }
  13.         else
  14.         {
  15. //            for(i=0;i<rx_size;i++)
  16. //                printf("%c",rx_buf[i]);
  17. //            printf("\r\n");
  18.             if(memcmp("a1",rx_buf,2)==0)//前进
  19.             {
  20.                 BST_fBluetoothSpeed=100;
  21.                 //printf("前进\r\n");
  22.             }
  23.             else if(memcmp("a2",rx_buf,2)==0)//后退
  24.             {
  25.                 BST_fBluetoothSpeed=-100;
  26.                 //printf("后退\r\n");
  27.             }
  28.             else if(memcmp("a3",rx_buf,2)==0)//左转
  29.             {
  30.                 BST_fBluetoothSpeed=0;
  31.                 Turn_val=90;
  32.                 Yaw_old=Yaw;
  33.                 //printf("左转\r\n");
  34.             }
  35.             else if(memcmp("a4",rx_buf,2)==0)//右转
  36.             {
  37.                 BST_fBluetoothSpeed=0;
  38.                 Turn_val=-90;
  39.                  Yaw_old=Yaw;
  40.                 //printf("右转\r\n");
  41.             }
  42.             else if(memcmp("a0",rx_buf,2)==0)//停下
  43.             {
  44.                 BST_fBluetoothSpeed=0;
  45.                 //printf("停下\r\n");
  46.             }
  47.             rx_size=0;
  48.         }
  49.     }
  50. }

总结

        经过对平衡车的学习，我对pid算法有了更加深刻的理解，在mpu6050陀螺仪的使用也越来越熟练，平衡小车的核心就是pid算法，所以pid算法对小车和控制类的学习是十分重要的。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Monodyee/article/detail/386786
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号