AGV吧-agv小车：基于ROS平台的移动机器人-小车底盘控制

09430

小车控制思想
控制电机转动。电机的控制我们分为两部分，一部分为电机转动方向的控制，另一个为电机转速的控制。电机转动的方向我们用两个MCU引脚来控制，假如PIN_A=1，PIN_B=0 时，电机正转；PIN_A=0，PIN_B=1 时，电机反转；PIN_A=0，PIN_B=0 时，电机停止。电机速度的控制则需要一个PWM输出引脚，我们通过控制输出不同的PWM值来控制电机转动的速度。
PID控制。如果我们想控制小车以一米每秒的速度做直线，但由于地面的摩擦阻力的影响，会造成左右轮速度与我们想控制的速度不同，所以会走不直，这时我们就需要加入PID控制，PID控制的思想就是我实时的把轮子真正的速度采集回来和控制的速度对比，差则补，多则减。这样基本就可以实现理想控制。详细PID介绍查看。
小车转弯控制。一般我们要是想控制小车以多少的速度前进或者后退，我们只需要PID控制两个轮子的速度一致就可以基本做到。但如果要想控制小车以多少的角速度转弯，我们需要做一定的计算，如图所示：
AGV吧-agv小车：基于ROS平台的移动机器人-小车底盘控制
推算过程这里就不算了，我们可以得到左右轮速度和线速度角速度之间的关系如下：
AGV吧-agv小车：基于ROS平台的移动机器人-小车底盘控制
通过以上的公式我们就可以控制小车的任意行走了。

程序结构
以下为我的STM32工程主要文件

1.main.c 接收和发送串口数据，控制电机

  1. /***********************************************  说明  ***************************************************************** 
  2. * 
  3. *   1.串口接收 
  4. *    （1）内容：小车左右轮速度,单位:mm/s（所有数据都为   float型，float型占4字节） 
  5. *    （2）格式：10字节 [右轮速度4字节][左轮速度4字节][结束符”\r\n”2字节] 
  6. * 
  7. *   2.串口发送 
  8. *    （1）内容：里程计（x,y坐标、线速度、角速度和方向角，单位依次为：mm,mm,mm/s,rad/s,rad，所有数据都为float型，float型占4字节） 
  9. *    （2）格式：21字节 [x坐标4字节][y坐标4字节][方向角4字节][线速度4字节][角速度4字节][结束符”\n”1字节] 
  10. * 
  11. ************************************************************************************************************************/  
  12. #include “stm32f10x.h”  
  13. #include “stm32f10x_it.h”  
  14.  
  15. #include “delay.h”  
  16. #include “nvic_conf.h”  
  17. #include “rcc_conf.h”  
  18.  
  19. #include “usart.h”  
  20. #include “encoder.h”  
  21. #include “contact.h”  
  22. #include “gpio_conf.h”  
  23. #include “tim2_5_6.h”  
  24. #include “odometry.h”  
  25. #include “tim2_5_6.h”  
  26.  
  27. #include “stdbool.h”  
  28. #include <stdio.h>  
  29. #include “string.h”  
  30. #include “math.h”  
  31. /***********************************************  输出  *****************************************************************/  
  32.   
  33. char odometry_data[21]={0};   //发送给串口的里程计数据数组  
  34.   
  35. float odometry_right=0,odometry_left=0;//串口得到的左右轮速度  
  36.   
  37. /***********************************************  输入  *****************************************************************/  
  38.   
  39. extern float position_x,position_y,oriention,velocity_linear,velocity_angular;         //计算得到的里程计数值  
  40.   
  41. extern u8 USART_RX_BUF[USART_REC_LEN];     //串口接收缓冲,最大USART_REC_LEN个字节.  
  42. extern u16 USART_RX_STA;                   //串口接收状态标记     
  43.   
  44. extern float Milemeter_L_Motor,Milemeter_R_Motor;     //dt时间内的左右轮速度,用于里程计计算  
  45.   
  46. /***********************************************  变量  *****************************************************************/  
  47.   
  48. u8 main_sta=0; //用作处理主函数各种if，去掉多余的flag（1打印里程计）（2调用计算里程计数据函数）（3串口接收成功）（4串口接收失败）  
  49.   
  50. union recieveData  //接收到的数据  
  51. {  
  52.     float d;    //左右轮速度  
  53.     unsigned char data[4];  
  54. }leftdata,rightdata;       //接收的左右轮数据  
  55.   
  56. union odometry  //里程计数据共用体  
  57. {  
  58.     float odoemtry_float;  
  59.     unsigned char odometry_char[4];  
  60. }x_data,y_data,theta_data,vel_linear,vel_angular;     //要发布的里程计数据，分别为：X，Y方向移动的距离，当前角度，线速度，角速度  
  61.   
  62. /****************************************************************************************************************/    
  63. int main(void)  
  64. {         
  65. u8 t=0;  
  66. u8 i=0,j=0,m=0;  
  67.   
  68. RCC_Configuration();      //系统时钟配置        
  69. NVIC_Configuration();     //中断优先级配置  
  70. GPIO_Configuration();     //电机方向控制引脚配置  
  71. USART1_Config();          //串口1配置  
  72.   
  73. TIM2_PWM_Init();          //PWM输出初始化  
  74. ENC_Init();               //电机处理初始化  
  75. TIM5_Configuration();     //速度计算定时器初始化  
  76. TIM1_Configuration();     //里程计发布定时器初始化  
  77.   
  78. while (1)  
  79. {         
  80.     if(main_sta&0x01)//执行发送里程计数据步骤  
  81.     {  
  82.         //里程计数据获取  
  83.         x_data.odoemtry_float=position_x;//单位mm  
  84.         y_data.odoemtry_float=position_y;//单位mm  
  85.         theta_data.odoemtry_float=oriention;//单位rad  
  86.         vel_linear.odoemtry_float=velocity_linear;//单位mm/s  
  87.         vel_angular.odoemtry_float=velocity_angular;//单位rad/s  
  88.   
  89.         //将所有里程计数据存到要发送的数组  
  90.         for(j=0;j<4;j++)  
  91.         {  
  92.             odometry_data[j]=x_data.odometry_char[j];  
  93.             odometry_data[j+4]=y_data.odometry_char[j];  
  94.             odometry_data[j+8]=theta_data.odometry_char[j];  
  95.             odometry_data[j+12]=vel_linear.odometry_char[j];  
  96.             odometry_data[j+16]=vel_angular.odometry_char[j];  
  97.         }  
  98.   
  99.         odometry_data[20]=’\n’;//添加结束符  
  100.   
  101.         //发送数据要串口  
  102.         for(i=0;i<21;i++)  
  103.         {  
  104.             USART_ClearFlag(USART1,USART_FLAG_TC);  //在发送第一个数据前加此句，解决第一个数据不能正常发送的问题               
  105.             USART_SendData(USART1,odometry_data[i]);//发送一个字节到串口   
  106.             while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); //等待发送结束              
  107.         }  
  108.   
  109.         main_sta&=0xFE;//执行计算里程计数据步骤  
  110.     }  
  111.     if(main_sta&0x02)//执行计算里程计数据步骤  
  112.     {  
  113.         odometry(Milemeter_R_Motor,Milemeter_L_Motor);//计算里程计  
  114.   
  115.         main_sta&=0xFD;//执行发送里程计数据步骤  
  116.     }   
  117.     if(main_sta&0x08)        //当发送指令没有正确接收时  
  118.     {  
  119.         USART_ClearFlag(USART1,USART_FLAG_TC);  //在发送第一个数据前加此句，解决第一个数据不能正常发送的问题  
  120.         for(m=0;m<3;m++)  
  121.         {  
  122.             USART_SendData(USART1,0x00);      
  123.             while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);  
  124.         }         
  125.         USART_SendData(USART1,’\n’);      
  126.         while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);   
  127.         main_sta&=0xF7;  
  128.     }  
  129.     if(USART_RX_STA&0x8000)  // 串口1接收函数  
  130.     {             
  131.         //接收左右轮速度  
  132.         for(t=0;t<4;t++)  
  133.         {  
  134.             rightdata.data[t]=USART_RX_BUF[t];  
  135.             leftdata.data[t]=USART_RX_BUF[t+4];  
  136.         }  
  137.   
  138.         //储存左右轮速度  
  139.         odometry_right=rightdata.d;//单位mm/s  
  140.         odometry_left=leftdata.d;//单位mm/s  
  141.   
  142.         USART_RX_STA=0;//清楚接收标志位  
  143.     }  
  144.   
  145.     car_control(rightdata.d,leftdata.d);     //将接收到的左右轮速度赋给小车     
  146. }//end_while  
  147. }//end main  
  148. /*********************************************END OF FILE**************************************************/  

2.odometry.c 里程计计算

  1. #include “odometry.h”  
  2.   
  3. /***********************************************  输出  *****************************************************************/  
  4.   
  5. float position_x=0,position_y=0,oriention=0,velocity_linear=0,velocity_angular=0;  
  6.   
  7. /***********************************************  输入  *****************************************************************/  
  8.   
  9. extern float odometry_right,odometry_left;//串口得到的左右轮速度  
  10.   
  11. /***********************************************  变量  *****************************************************************/  
  12.   
  13. float wheel_interval= 268.0859f;//    272.0f;        //  1.0146  
  14. //float wheel_interval=276.089f;    //轴距校正值=原轴距/0.987  
  15.   
  16. float multiplier=4.0f;           //倍频数  
  17. float deceleration_ratio=90.0f;  //减速比  
  18. float wheel_diameter=100.0f;     //轮子直径，单位mm  
  19. float pi_1_2=1.570796f;          //π/2  
  20. float pi=3.141593f;              //π  
  21. float pi_3_2=4.712389f;          //π*3/2  
  22. float pi_2_1=6.283186f;          //π*2  
  23. float dt=0.005f;                 //采样时间间隔5ms  
  24. float line_number=4096.0f;       //码盘线数  
  25. float oriention_interval=0;  //dt时间内方向变化值  
  26.   
  27. float sin_=0;        //角度计算值  
  28. float cos_=0;  
  29.   
  30. float delta_distance=0,delta_oriention=0;   //采样时间间隔内运动的距离  
  31.   
  32. float const_frame=0,const_angle=0,distance_sum=0,distance_diff=0;  
  33.   
  34. float oriention_1=0;  
  35.   
  36. u8 once=1;  
  37.   
  38. /****************************************************************************************************************/  
  39.   
  40. //里程计计算函数  
  41. void odometry(float right,float left)  
  42. {     
  43.     if(once)  //常数仅计算一次  
  44.     {  
  45.         const_frame=wheel_diameter*pi/(line_number*multiplier*deceleration_ratio);  
  46.         const_angle=const_frame/wheel_interval;  
  47.         once=0;  
  48.     }  
  49.   
  50.     distance_sum = 0.5f*(right+left);//在很短的时间内，小车行驶的路程为两轮速度和  
  51.     distance_diff = right-left;//在很短的时间内，小车行驶的角度为两轮速度差  
  52.   
  53.     //根据左右轮的方向，纠正短时间内，小车行驶的路程和角度量的正负  
  54.     if((odometry_right>0)&&(odometry_left>0))            //左右均正  
  55.     {  
  56.         delta_distance = distance_sum;  
  57.         delta_oriention = distance_diff;  
  58.     }  
  59.     else if((odometry_right<0)&&(odometry_left<0))       //左右均负  
  60.     {  
  61.         delta_distance = -distance_sum;  
  62.         delta_oriention = -distance_diff;  
  63.     }  
  64.     else if((odometry_right<0)&&(odometry_left>0))       //左正右负  
  65.     {  
  66.         delta_distance = -distance_diff;  
  67.         delta_oriention = -2.0f*distance_sum;         
  68.     }  
  69.     else if((odometry_right>0)&&(odometry_left<0))       //左负右正  
  70.     {  
  71.         delta_distance = distance_diff;  
  72.         delta_oriention = 2.0f*distance_sum;  
  73.     }  
  74.     else  
  75.     {  
  76.         delta_distance=0;  
  77.         delta_oriention=0;  
  78.     }  
  79.   
  80.     oriention_interval = delta_oriention * const_angle;//采样时间内走的角度    
  81.     oriention = oriention + oriention_interval;//计算出里程计方向角  
  82.     oriention_1 = oriention + 0.5f * oriention_interval;//里程计方向角数据位数变化，用于三角函数计算  
  83.   
  84.     sin_ = sin(oriention_1);//计算出采样时间内y坐标  
  85.     cos_ = cos(oriention_1);//计算出采样时间内x坐标  
  86.   
  87.     position_x = position_x + delta_distance * cos_ * const_frame;//计算出里程计x坐标  
  88.     position_y = position_y + delta_distance * sin_ * const_frame;//计算出里程计y坐标  
  89.   
  90.     velocity_linear = delta_distance*const_frame / dt;//计算出里程计线速度  
  91.     velocity_angular = oriention_interval / dt;//计算出里程计角速度  
  92.   
  93.     //方向角角度纠正  
  94.     if(oriention > pi)  
  95.     {  
  96.         oriention -= pi_2_1;  
  97.     }  
  98.     else  
  99.     {  
  100.         if(oriention < -pi)  
  101.         {  
  102.             oriention += pi_2_1;  
  103.         }  
  104.     }  
  105. }  

3.PID.c PID处理函数

  1. #include “PID.h”  
  2.   
  3. extern int span;  
  4.   
  5. float MaxValue=9;//输出最大限幅  
  6. float MinValue=-9;//输出最小限幅  
  7.   
  8. float OutputValue;//PID输出暂存变量,用于积分饱和抑制  
  9.   
  10. float PID_calculate(struct PID *Control,float CurrentValue_left )//位置PID计算B  
  11. {  
  12.   
  13.     float Value_Kp;//比例分量  
  14.     float Value_Ki;//积分分量  
  15.     float Value_Kd;//微分分量  
  16.   
  17.     Control->error_0 = Control->OwenValue – CurrentValue_left + 0*span;//基波分量，Control->OwenValue为想要的速度，CurrentValue_left为电机真实速度  
  18.     Value_Kp = Control->Kp * Control->error_0 ;  
  19.     Control->Sum_error += Control->error_0;       
  20.   
  21.     /***********************积分饱和抑制********************************************/  
  22.     OutputValue = Control->OutputValue;  
  23.     if(OutputValue>5 || OutputValue<-5)   
  24.     {  
  25.         Control->Ki = 0;   
  26.     }  
  27.     /*******************************************************************/  
  28.   
  29.     Value_Ki = Control->Ki * Control->Sum_error;  
  30.     Value_Kd = Control->Kd * ( Control->error_0 – Control->error_1);  
  31.     Control->error_1 = Control->error_0;//保存一次谐波  
  32.     Control->OutputValue = Value_Kp  + Value_Ki + Value_Kd;//输出值计算，注意加减  
  33.   
  34.     //限幅  
  35.     if( Control->OutputValue > MaxValue)  
  36.         Control->OutputValue = MaxValue;  
  37.     if (Control->OutputValue < MinValue)  
  38.         Control->OutputValue = MinValue;  
  39.   
  40.     return (Control->OutputValue) ;  
  41. }  

4.encoder.c 电机编码处理

  1. #include “encoder.h”  
  2.   
  3. /****************************************************************************************************************/  
  4.   
  5. s32 hSpeed_Buffer2[SPEED_BUFFER_SIZE]={0}, hSpeed_Buffer1[SPEED_BUFFER_SIZE]={0};//左右轮速度缓存数组  
  6. static unsigned int hRot_Speed2;//电机A平均转速缓存  
  7. static unsigned int hRot_Speed1;//电机B平均转速缓存  
  8. unsigned int Speed2=0; //电机A平均转速 r/min，PID调节  
  9. unsigned int Speed1=0; //电机B平均转速 r/min，PID调节  
  10.   
  11. static volatile u16 hEncoder_Timer_Overflow1;//电机B编码数采集   
  12. static volatile u16 hEncoder_Timer_Overflow2;//电机A编码数采集  
  13.   
  14. //float A_REMP_PLUS;//电机APID调节后的PWM值缓存  
  15. float pulse = 0;//电机A PID调节后的PWM值缓存  
  16. float pulse1 = 0;//电机B PID调节后的PWM值缓存  
  17.   
  18. int span;//采集回来的左右轮速度差值  
  19.   
  20. static bool bIs_First_Measurement2 = true;//电机A以清除速度缓存数组标志位  
  21. static bool bIs_First_Measurement1 = true;//电机B以清除速度缓存数组标志位  
  22.   
  23. struct PID Control_left  ={0.01,0.1,0.75,0,0,0,0,0,0};//左轮PID参数，适于新电机4096  
  24. struct PID Control_right ={0.01,0.1,0.75,0,0,0,0,0,0};//右轮PID参数，适于新电机4096  
  25.   
  26. /****************************************************************************************************************/  
  27.   
  28. s32 hPrevious_angle2, hPrevious_angle1;  
  29.   
  30. /****************************************************************************************************************/  
  31.   
  32. void ENC_Init2(void)//电机A码盘采集定时器，TIM4编码器模式  
  33. {  
  34.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;  
  35.     TIM_ICInitTypeDef TIM_ICInitStructure;      
  36.     GPIO_InitTypeDef GPIO_InitStructure;  
  37.   
  38.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);  
  39.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);  
  40.     GPIO_StructInit(&GPIO_InitStructure);  
  41.   
  42.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;  
  43.     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;  
  44.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  
  45.     GPIO_Init(GPIOB, &GPIO_InitStructure);  
  46.   
  47.     TIM_DeInit(ENCODER2_TIMER);  
  48.     TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);  
  49.   
  50.     TIM_TimeBaseStructure.TIM_Prescaler = 0;    
  51.     TIM_TimeBaseStructure.TIM_Period = (4*ENCODER2_PPR)-1;  
  52.   
  53.     TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;  
  54.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;     
  55.     TIM_TimeBaseInit(ENCODER2_TIMER, &TIM_TimeBaseStructure);  
  56.   
  57.     TIM_EncoderInterfaceConfig(ENCODER2_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);  
  58.     TIM_ICStructInit(&TIM_ICInitStructure);  
  59.     TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;  
  60.     TIM_ICInit(ENCODER2_TIMER, &TIM_ICInitStructure);  
  61.   
  62.     TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update);  
  63.     TIM_ITConfig(ENCODER2_TIMER, TIM_IT_Update, ENABLE);  
  64.   
  65.     TIM_Cmd(ENCODER2_TIMER, ENABLE);   
  66. }  
  67.   
  68. void ENC_Init1(void)//电机B码盘采集定时器，TIM3编码器模式  
  69. {  
  70.     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;  
  71.     TIM_ICInitTypeDef TIM_ICInitStructure;  
  72.   
  73.     GPIO_InitTypeDef GPIO_InitStructure;  
  74.   
  75.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);  
  76.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);  
  77.     GPIO_StructInit(&GPIO_InitStructure);  
  78.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;  
  79.     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;  
  80.     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  
  81.     GPIO_Init(GPIOA, &GPIO_InitStructure);  
  82.   
  83.   
  84.     TIM_DeInit(ENCODER1_TIMER);  
  85.     TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);  
  86.   
  87.     TIM_TimeBaseStructure.TIM_Prescaler = 0;  
  88.     TIM_TimeBaseStructure.TIM_Period = (4*ENCODER1_PPR)-1;    
  89.   
  90.     TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;  
  91.     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;     
  92.     TIM_TimeBaseInit(ENCODER1_TIMER, &TIM_TimeBaseStructure);  
  93.   
  94.     TIM_EncoderInterfaceConfig(ENCODER1_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);  
  95.     TIM_ICStructInit(&TIM_ICInitStructure);  
  96.     TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;  
  97.     TIM_ICInit(ENCODER1_TIMER, &TIM_ICInitStructure);  
  98.   
  99.     TIM_ClearFlag(ENCODER1_TIMER, TIM_FLAG_Update);  
  100.     TIM_ITConfig(ENCODER1_TIMER, TIM_IT_Update, ENABLE);  
  101.   
  102.     TIM_Cmd(ENCODER1_TIMER, ENABLE);   
  103. }  
  104.   
  105. /****************************************************************************************************************/  
  106.   
  107. s16 ENC_Calc_Rot_Speed2(void)//计算电机A的编码数  
  108. {     
  109.     s32 wDelta_angle;  
  110.     u16 hEnc_Timer_Overflow_sample_one;  
  111.     u16 hCurrent_angle_sample_one;  
  112.     s32 temp;  
  113.     s16 haux;  
  114.   
  115.     if (!bIs_First_Measurement2)//电机A以清除速度缓存数组  
  116.     {    
  117.         hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow2;    
  118.         hCurrent_angle_sample_one = ENCODER2_TIMER->CNT;  
  119.         hEncoder_Timer_Overflow2 = 0;  
  120.         haux = ENCODER2_TIMER->CNT;     
  121.   
  122.         if ( (ENCODER2_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down)    
  123.         {  
  124.             // encoder timer down-counting 反转的速度计算       
  125.             wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR) -(hCurrent_angle_sample_one – hPrevious_angle2));  
  126.         }  
  127.         else    
  128.         {  
  129.             //encoder timer up-counting 正转的速度计算  
  130.             wDelta_angle = (s32)(hCurrent_angle_sample_one – hPrevious_angle2 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR));  
  131.         }         
  132.         temp=wDelta_angle;  
  133.     }   
  134.     else  
  135.     {  
  136.         bIs_First_Measurement2 = false;//电机A以清除速度缓存数组标志位  
  137.         temp = 0;  
  138.         hEncoder_Timer_Overflow2 = 0;  
  139.         haux = ENCODER2_TIMER->CNT;         
  140.     }  
  141.     hPrevious_angle2 = haux;    
  142.     return((s16) temp);  
  143. }  
  144.   
  145.   
  146. s16 ENC_Calc_Rot_Speed1(void)//计算电机B的编码数  
  147. {     
  148.     s32 wDelta_angle;  
  149.     u16 hEnc_Timer_Overflow_sample_one;  
  150.     u16 hCurrent_angle_sample_one;  
  151.     s32 temp;  
  152.     s16 haux;  
  153.   
  154.     if (!bIs_First_Measurement1)//电机B以清除速度缓存数组  
  155.     {     
  156.         hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow1;  //得到采样时间内的编码数     
  157.         hCurrent_angle_sample_one = ENCODER1_TIMER->CNT;  
  158.         hEncoder_Timer_Overflow1 = 0;//清除脉冲数累加  
  159.         haux = ENCODER1_TIMER->CNT;     
  160.   
  161.         if ( (ENCODER1_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down)    
  162.         {  
  163.             // encoder timer down-counting 反转的速度计算  
  164.             wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER1_PPR) -(hCurrent_angle_sample_one – hPrevious_angle1));    
  165.         }  
  166.         else    
  167.         {  
  168.             //encoder timer up-counting 正转的速度计算  
  169.             wDelta_angle = (s32)(hCurrent_angle_sample_one – hPrevious_angle1 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER1_PPR));  
  170.         }  
  171.         temp=wDelta_angle;  
  172.     }   
  173.     else  
  174.     {  
  175.         bIs_First_Measurement1 = false;//电机B以清除速度缓存数组标志位  
  176.         temp = 0;  
  177.         hEncoder_Timer_Overflow1 = 0;  
  178.         haux = ENCODER1_TIMER->CNT;         
  179.     }  
  180.     hPrevious_angle1 = haux;    
  181.     return((s16) temp);  
  182. }  
  183.   
  184.   
  185. /****************************************************************************************************************/  
  186.   
  187. void ENC_Clear_Speed_Buffer(void)//速度存储器清零  
  188. {     
  189.     u32 i;  
  190.   
  191.     //清除左右轮速度缓存数组  
  192.     for (i=0;i<SPEED_BUFFER_SIZE;i++)  
  193.     {  
  194.         hSpeed_Buffer2[i] = 0;  
  195.         hSpeed_Buffer1[i] = 0;  
  196.     }  
  197.   
  198.     bIs_First_Measurement2 = true;//电机A以清除速度缓存数组标志位  
  199.     bIs_First_Measurement1 = true;//电机B以清除速度缓存数组标志位  
  200. }  
  201.   
  202. void ENC_Calc_Average_Speed(void)//计算三次电机的平均编码数  
  203. {     
  204.     u32 i;  
  205.     signed long long wtemp3=0;  
  206.     signed long long wtemp4=0;  
  207.   
  208.     //累加缓存次数内的速度值  
  209.     for (i=0;i<SPEED_BUFFER_SIZE;i++)  
  210.     {  
  211.         wtemp4 += hSpeed_Buffer2[i];  
  212.         wtemp3 += hSpeed_Buffer1[i];  
  213.     }  
  214.   
  215.     //取平均，平均脉冲数单位为 个/s    
  216.     wtemp3 /= (SPEED_BUFFER_SIZE);  
  217.     wtemp4 /= (SPEED_BUFFER_SIZE); //平均脉冲数 个/s    
  218.   
  219.     //将平均脉冲数单位转为 r/min  
  220.     wtemp3 = (wtemp3 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER1_PPR);  
  221.     wtemp4 = (wtemp4 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER2_PPR);   
  222.   
  223.     hRot_Speed2= ((s16)(wtemp4));//平均转速 r/min  
  224.     hRot_Speed1= ((s16)(wtemp3));//平均转速 r/min  
  225.     Speed2=hRot_Speed2;//平均转速 r/min  
  226.     Speed1=hRot_Speed1;//平均转速 r/min  
  227. }  
  228.   
  229. /****************************************************************************************************************/  
  230.   
  231. void Gain2(void)//设置电机A PID调节 PA2  
  232. {  
  233.     //static float pulse = 0;  
  234.   
  235.     span=1*(Speed1-Speed2);//采集回来的左右轮速度差值  
  236.     pulse= pulse + PID_calculate(&Control_right,hRot_Speed2);//PID调节  
  237.   
  238.     //pwm幅度抑制  
  239.     if(pulse > 3600) pulse = 3600;  
  240.     if(pulse < 0) pulse = 0;  
  241.   
  242.     //A_REMP_PLUS=pulse;//电机APID调节后的PWM值缓存  
  243. }  
  244.   
  245.   
  246. void Gain1(void)//设置电机B PID调节 PA1  
  247. {  
  248.     //static float pulse1 = 0;  
  249.   
  250.     span=1*(Speed2-Speed1);//采集回来的左右轮速度差值  
  251.     pulse1= pulse1 + PID_calculate(&Control_left,hRot_Speed1);//PID调节  
  252.   
  253.     ////pwm 幅度抑制  
  254.     if(pulse1 > 3600) pulse1 = 3600;  
  255.     if(pulse1 < 0) pulse1 = 0;  
  256.   
  257.     TIM2->CCR2 = pulse1;//电机B赋值PWM  
  258.     //TIM2->CCR3 = A_REMP_PLUS;//电机A赋值PWM  
  259.     TIM2->CCR3 = pulse;//电机A赋值PWM  
  260. }  
  261.   
  262. /****************************************************************************************************************/  
  263.   
  264. void ENC_Init(void)//电机处理初始化  
  265. {  
  266.     ENC_Init2();              //设置电机A TIM4编码器模式PB6 PB7 右电机  
  267.     ENC_Init1();              //设置电机B TIM3编码器模式PA6 PA7 左电机  
  268.     ENC_Clear_Speed_Buffer();//速度存储器清零  
  269. }  
  270.   
  271. /****************************************************************************************************************/  
  272.   
  273. void TIM4_IRQHandler (void)//执行TIM4(电机A编码器采集)计数中断  
  274. {     
  275.     TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update);  
  276.     if (hEncoder_Timer_Overflow2 != U16_MAX)//不超范围    
  277.     {  
  278.         hEncoder_Timer_Overflow2++; //脉冲数累加  
  279.     }  
  280. }  
  281.   
  282. void TIM3_IRQHandler (void)//执行TIM3(电机B编码器采集)计数中断  
  283. {    
  284.     TIM_ClearFlag(ENCODER1_TIMER, TIM_FLAG_Update);  
  285.     if (hEncoder_Timer_Overflow1 != U16_MAX)//不超范围      
  286.     {  
  287.         hEncoder_Timer_Overflow1++;  //脉冲数累加  
  288.     }  
  289. }  

5.contact.c 电机控制函数

  1. #include “contact.h”  
  2.   
  3. /***********************************************  输出  *****************************************************************/  
  4.   
  5. /***********************************************  输入  *****************************************************************/  
  6.   
  7. extern struct PID Control_left;//左轮PID参数，适于新电机4096  
  8. extern struct PID Control_right;//右轮PID参数，适于新电机4096  
  9.   
  10. /***********************************************  变量  *****************************************************************/  
  11.   
  12. /*******************************************************************************************************************/  
  13.   
  14. void LeftMovingSpeedW(unsigned int val)//左轮方向和速度控制函数  
  15. {       
  16.     if(val>10000)  
  17.     {    
  18.         GPIO_SetBits(GPIOC, GPIO_Pin_6);      
  19.         GPIO_ResetBits(GPIOC, GPIO_Pin_7);    
  20.   
  21.         Control_left.OwenValue=(val-10000);//PID调节的目标编码数              
  22.     }  
  23.     else if(val<10000)  
  24.     {    
  25.         GPIO_SetBits(GPIOC, GPIO_Pin_7);      
  26.         GPIO_ResetBits(GPIOC, GPIO_Pin_6);    
  27.   
  28.         Control_left.OwenValue=(10000-val);//PID调节的目标编码数       
  29.     }     
  30.     else  
  31.     {  
  32.          GPIO_SetBits(GPIOC, GPIO_Pin_6);     
  33.          GPIO_SetBits(GPIOC, GPIO_Pin_7);  
  34.   
  35.          Control_left.OwenValue=0;//PID调节的目标编码数  
  36.     }                     
  37. }  
  38.   
  39. void RightMovingSpeedW(unsigned int val2)//右轮方向和速度控制函数  
  40. {      
  41.     if(val2>10000)  
  42.     {    
  43.         /* motor A 正转*/  
  44.         GPIO_SetBits(GPIOC, GPIO_Pin_10);     
  45.         GPIO_ResetBits(GPIOC, GPIO_Pin_11);   
  46.   
  47.         Control_right.OwenValue=(val2-10000);//PID调节的目标编码数  
  48.     }  
  49.     else if(val2<10000)  
  50.     {    
  51.         /* motor A 反转*/  
  52.         GPIO_SetBits(GPIOC, GPIO_Pin_11);     
  53.         GPIO_ResetBits(GPIOC, GPIO_Pin_10);   
  54.   
  55.         Control_right.OwenValue=(10000-val2);//PID调节的目标编码数     
  56.     }     
  57.     else  
  58.     {  
  59.         GPIO_SetBits(GPIOC, GPIO_Pin_10);     
  60.         GPIO_SetBits(GPIOC, GPIO_Pin_11);  
  61.   
  62.         Control_right.OwenValue=0;//PID调节的目标编码数  
  63.     }                                                 
  64. }  
  65.   
  66. void car_control(float rightspeed,float leftspeed)//小车速度转化和控制函数  
  67. {  
  68.     float k2=17.179;         //速度转换比例,转/分钟    
  69.   
  70.     //将从串口接收到的速度转换成实际控制小车的速度？还是PWM？  
  71.     int right_speed=(int)k2*rightspeed;  
  72.     int left_speed=(int)k2*leftspeed;  
  73.   
  74.     RightMovingSpeedW(right_speed+10000);  
  75.     LeftMovingSpeedW(left_speed+10000);  
  76. }  
  77.   
  78. //void Contact_Init(void)//左右轮方向和速度初始化  
  79. //{  
  80. //  LeftMovingSpeedW(12000); //电机B  
  81. //  RightMovingSpeedW(12000);//电机A    
  82. //}  

———————
作者：Forrest-Z
来源：CSDN
原文：https://blog.csdn.net/Forrest_Z/article/details/55001231

© 版权声明
1. 本网站的文章部分内容可能来源于网络，仅供大家学习与参考，如有侵权，请联系站长 QQ2766242327进行删除处理。
2. 本站资源大多存储在云盘，如发现链接失效，请联系我们我们会第一时间更新。
THE END
AGV设计资料
# AGV小车# AGV吧# 移动机器人# ROS平台
喜欢就支持一下吧
点赞0 分享
AGV吧的头像-AGV吧
基于QR码的视觉导航AGV系统-AGV吧
基于QR码的视觉导航AGV系统
基于QR码的视觉导航AGV系统
5年前 2.5W+
Nipper AGV – 内部托盘运输-AGV吧
Nipper AGV – 内部托盘运输
Nipper AGV – 内部托盘运输
6年前 5350
OTTO -AGV安装调试车间-AGV吧
OTTO -AGV安装调试车间
OTTO -AGV安装调试车间
6年前 4874
AGV吧-agv小车：AGV设计电机选择计算-AGV吧
AGV吧-agv小车：AGV设计电机选择计算
AGV吧-agv小车：AGV设计电机选择计算
5年前 4590
快围观：亚马逊KIVA机器人没有装外壳-AGV吧
快围观：亚马逊KIVA机器人没有装外壳
快围观：亚马逊KIVA机器人没有装外壳
7年前 4502
怎么把最新款的亚马逊AGV的外壳打开了-AGV吧
怎么把最新款的亚马逊AGV的外壳打开了
怎么把最新款的亚马逊AGV的外壳打开了
6年前 4414
相关推荐
全向移动机器人驱动轮同步转向机构设计-AGV吧
全向移动机器人驱动轮同步转向机构设计
全向移动机器人驱动轮同步转向机构设计
5年前 146
AGV吧-agv小车：CC-Link-IE现场网络Basic连接指南-AGV吧
AGV吧-agv小车：CC-Link-IE现场网络Basic连接指南
AGV吧-agv小车：CC-Link-IE现场网络Basic连接指南
5年前 539
激光导引AGV的控制系统设计-AGV吧
激光导引AGV的控制系统设计
激光导引AGV的控制系统设计
5年前 400
车间布局与自动导引车系统规划协同设计方法-AGV吧
车间布局与自动导引车系统规划协同设计方法
车间布局与自动导引车系统规划协同设计方法
5年前 163
agvba-agv吧：看看激光导航叉车是如何在顶部自动充电的-AGV吧
agvba-agv吧：看看激光导航叉车是如何在顶部自动充电的
agvba-agv吧：看看激光导航叉车是如何在顶部自动充电的
5年前 290
AGV吧-agv小车：Vecna搬运AGV在仓库应用-AGV吧
AGV吧-agv小车：Vecna搬运AGV在仓库应用
AGV吧-agv小车：Vecna搬运AGV在仓库应用
5年前 407
评论 抢沙发

请登录后发表评论

    暂无评论内容

默认头像
月度热门
热门标签
AGV (425)AGV小车 (202)机器人 (138)AGV吧 (137)移动机器人 (81)SLAM (56)agvba (55)无人搬运车 (43)自动化 (37)路径规划 (35)自动导引车 (33)SLAM算法 (32)控制系统 (30)传感器 (26)算法 (25)ROS (25)阿哥五 (23)PLC (22)机械设计 (21)自主移动机器人 (19)

网站统计

  • 626
  • 710
  • 2,052
  • 2023年8月7日