標(biāo)題: 基于STM32的平衡小車核心算法 [打印本頁]
作者: kuangleilei    時間: 2020-11-1 17:45
標(biāo)題: 基于STM32的平衡小車核心算法
  1. #include "math.h"
  2. #include "stdio.h"
  3. #include "control.h"
  4. #include "debug.H"
  5. #include "MPU6050.H"
  6. #include "communicate.h"
  7. #include "bsp.h"
  8. #include "ultrasonic.h"
  9. #include "infrare.h"


  12. unsigned char g_u8MainEventCount;
  13. unsigned char g_u8SpeedControlCount;
  14. unsigned char g_u8SpeedControlPeriod;
  15. unsigned char g_u8DirectionControlPeriod;
  16. unsigned char g_u8DirectionControlCount;

  18. unsigned char g_cMotorDisable = 0;//值等于0時電機(jī)正常轉(zhuǎn)動，否則停止轉(zhuǎn)動


  21. int g_iGravity_Offset = 0;

  23. /******電機(jī)控制參數(shù)******/
  24. float g_fSpeedControlOut;
  25. float g_fSpeedControlOutOld;
  26. float g_fSpeedControlOutNew;
  27. float g_fAngleControlOut;
  28. float g_fLeftMotorOut;
  29. float g_fRightMotorOut;

  31. /******速度控制參數(shù)******/

  33. short  g_s16LeftMotorPulse;
  34. short  g_s16RightMotorPulse;

  36. int  g_s32LeftMotorPulseOld;
  37. int  g_s32RightMotorPulseOld;
  38. int  g_s32LeftMotorPulseSigma;
  39. int  g_s32RightMotorPulseSigma;

  41. float g_fCarSpeed;
  42. float g_iCarSpeedSet;
  43. float g_fCarSpeedOld;
  44. float g_fCarPosition;

  46. /*-----角度環(huán)和速度環(huán)PID控制參數(shù)-----*/
  47. PID_t g_tCarAnglePID={17.0, 0, 23.0};        //*5 /10
  48. PID_t g_tCarSpeedPID={15.25, 1.08, 0};        //i/10
  49. /******藍(lán)牙控制參數(shù)******/
  50. float g_fBluetoothSpeed;
  51. float g_fBluetoothDirection;
  52. float g_fBluetoothDirectionOld;
  53. float g_fBluetoothDirectionNew;
  54. float g_fBluetoothDirectionOut;

  56. float g_fCarAngle;                 //
  57. float g_fGyroAngleSpeed;                //                             
  58. float g_fGravityAngle;                        //


  61. int g_iLeftTurnRoundCnt = 0;
  62. int g_iRightTurnRoundCnt = 0;

  64. static int AbnormalSpinFlag = 0;
  65. /***************************************************************
  66. ** 函數(shù)名稱: CarUpstandInit
  67. ** 功能描述: 全局變量初始化函數(shù)
  68. ***************************************************************/
  69. void CarUpstandInit(void)
  70. {
  71.         //g_iAccelInputVoltage_X_Axis = g_iGyroInputVoltage_Y_Axis = 0;
  72.         g_s16LeftMotorPulse = g_s16RightMotorPulse = 0;
  73.         g_s32LeftMotorPulseOld = g_s32RightMotorPulseOld = 0;
  74.         g_s32LeftMotorPulseSigma = g_s32RightMotorPulseSigma = 0;

  76.         g_fCarSpeed = g_fCarSpeedOld = 0;
  77.         g_fCarPosition = 0;
  78.         g_fCarAngle    = 0;
  79.         g_fGyroAngleSpeed = 0;
  80.         g_fGravityAngle   = 0;

  82.         g_fAngleControlOut = g_fSpeedControlOut = g_fBluetoothDirectionOut = 0;
  83.         g_fLeftMotorOut    = g_fRightMotorOut   = 0;
  84.         g_fBluetoothSpeed  = g_fBluetoothDirection = 0;
  85.         g_fBluetoothDirectionNew = g_fBluetoothDirectionOld = 0;

  87.   g_u8MainEventCount=0;
  88.         g_u8SpeedControlCount=0;
  89.          g_u8SpeedControlPeriod=0;
  90. }


  93. /***************************************************************
  94. ** 函數(shù)名稱: AbnormalSpinDetect
  95. ** 功能描述: 電機(jī)轉(zhuǎn)速異常檢測      
  96. ***************************************************************/

  98. void AbnormalSpinDetect(short leftSpeed,short rightSpeed)
  99. {
  100.         static unsigned short count = 0;
  101.         
  102.         //速度設(shè)置為0時檢測，否則不檢測
  103.         if(g_iCarSpeedSet==0)
  104.         {
  105.                 if(((leftSpeed>30)&&(rightSpeed>30)&&(g_fCarAngle > -30) && (g_fCarAngle < 30))
  106.                         ||((leftSpeed<-30)&&(rightSpeed<-30))&&(g_fCarAngle > -30) && (g_fCarAngle < 30))
  107.                 {// 左右電機(jī)轉(zhuǎn)速大于30、方向相同、持續(xù)時間超過250ms，且車身角度不超過30度，則判斷為懸空空轉(zhuǎn)
  108.                         count++;
  109.                         if(count>50){
  110.                                 count = 0;
  111.                                 AbnormalSpinFlag = 1;
  112.                         }
  113.                 }
  114.                 else{
  115.                         count = 0;
  116.                 }
  117.         }
  118.         else{
  119.                 count = 0;
  120.         }
  121. }

  123. /***************************************************************
  124. ** 函數(shù)名稱: LandingDetect
  125. ** 功能描述: 小車著地檢測      
  126. ***************************************************************/
  127. void LandingDetect(void)
  128. {
  129.         static float lastCarAngle = 0;
  130.         static unsigned short count = 0,count1 = 0;
  131.         
  132.         if(AbnormalSpinFlag == 0)return;
  133.         
  134.         // 小車角度5°~-5°啟動檢測
  135.         if((g_fCarAngle > -5) && (g_fCarAngle < 5))
  136.         {
  137.                 count1++;
  138.                 if(count1 >= 50)
  139.                 {//每隔250ms判斷一次小車角度變化量，變化量小于0.8°或大于-0.8°判斷為小車靜止
  140.                         count1 = 0;
  141.                         if(((g_fCarAngle - lastCarAngle) < 0.8) && ((g_fCarAngle - lastCarAngle) > -0.8))
  142.                         {
  143.                                 count++;
  144.                                 if(count >= 4){
  145.                                         count = 0;
  146.                                         count1 = 0;
  147.                                         g_fCarPosition = 0;
  148.                                         AbnormalSpinFlag = 0;
  149.                                 }
  150.                         }
  151.                         else{
  152.                                 count = 0;
  153.                         }
  154.                         lastCarAngle = g_fCarAngle;
  155.                 }
  156.         }
  157.         else
  158.         {
  159.                 count1 = 0;
  160.                 count = 0;
  161.         }
  162. }

  164. /***************************************************************
  165. ** 函數(shù)名稱: MotorManage
  166. ** 功能描述: 電機(jī)使能/失能控制      
  167. ***************************************************************/
  168. void MotorManage(void)
  169. {

  171.         AbnormalSpinDetect(g_s16LeftMotorPulse, g_s16RightMotorPulse);
  172.                
  173.         LandingDetect();
  174.         
  175.         if(AbnormalSpinFlag)
  176.         {        
  177.                 g_cMotorDisable |= (0x01<<1);
  178.         }
  179.         else
  180.         {
  181.                 g_cMotorDisable &= ~(0x01<<1);
  182.         }
  183.         
  184.         if(g_fCarAngle > 30 || g_fCarAngle < (-30))
  185.         {
  186.                 g_cMotorDisable |= (0x01<<2);
  187.         }
  188.         else
  189.         {
  190.                 g_cMotorDisable &= ~(0x01<<2);
  191.         }
  192.         
  193. }

  195. /***************************************************************
  196. ** 函數(shù)名稱: SetMotorVoltageAndDirection
  197. ** 功能描述: 電機(jī)轉(zhuǎn)速及方向控制函數(shù)            
  198. ***************************************************************/
  199. void SetMotorVoltageAndDirection(int i16LeftVoltage,int i16RightVoltage)
  200. {
  201.           if(i16LeftVoltage<0)
  202.     {        
  203.                         GPIO_SetBits(GPIOA, GPIO_Pin_3 );                                    
  204.       GPIO_ResetBits(GPIOA, GPIO_Pin_4 );
  205.       i16LeftVoltage = (-i16LeftVoltage);
  206.     }
  207.     else
  208.     {        
  209.       GPIO_SetBits(GPIOA, GPIO_Pin_4 );                                    
  210.       GPIO_ResetBits(GPIOA, GPIO_Pin_3 );
  211.     }

  213.     if(i16RightVoltage<0)
  214.     {        
  215.              GPIO_SetBits(GPIOB, GPIO_Pin_0 );                                    
  216.       GPIO_ResetBits(GPIOB, GPIO_Pin_1 );
  217.       i16RightVoltage = (-i16RightVoltage);
  218.     }
  219.     else
  220.     {
  221.                         GPIO_SetBits(GPIOB, GPIO_Pin_1 );                                    
  222.                         GPIO_ResetBits(GPIOB, GPIO_Pin_0 );              
  223.     }

  225.         if(i16RightVoltage > MOTOR_OUT_MAX)  
  226.         {
  227.                 i16RightVoltage = MOTOR_OUT_MAX;
  228.         }
  229.         if(i16LeftVoltage > MOTOR_OUT_MAX)
  230.         {
  231.            i16LeftVoltage = MOTOR_OUT_MAX;
  232.         }  
  233.         
  234.         if(g_cMotorDisable)
  235.         {
  236.                 TIM_SetCompare1(TIM3,0);
  237.                 TIM_SetCompare2(TIM3,0);
  238.         }
  239.         else
  240.         {
  241.                 TIM_SetCompare1(TIM3,i16RightVoltage);
  242.                 TIM_SetCompare2(TIM3,i16LeftVoltage);
  243.         }
  244. }


  247. /***************************************************************
  248. ** 函數(shù)名稱: MotorOutput
  249. ** 功能描述: 電機(jī)輸出函數(shù)
  250.              將直立控制、速度控制、方向控制的輸出量進(jìn)行疊加,并加
  251.                          入死區(qū)常量，對輸出飽和作出處理。
  252. ***************************************************************/
  253. void MotorOutput(void)
  254. {
  255.         g_fLeftMotorOut  = g_fAngleControlOut - g_fSpeedControlOut - g_fBluetoothDirection ;        
  256.         g_fRightMotorOut = g_fAngleControlOut - g_fSpeedControlOut + g_fBluetoothDirection ;


  259.         /*增加死區(qū)常數(shù)*/
  260.         if((int)g_fLeftMotorOut>0)       g_fLeftMotorOut  += MOTOR_OUT_DEAD_VAL;
  261.         else if((int)g_fLeftMotorOut<0)  g_fLeftMotorOut  -= MOTOR_OUT_DEAD_VAL;
  262.         if((int)g_fRightMotorOut>0)      g_fRightMotorOut += MOTOR_OUT_DEAD_VAL;
  263.         else if((int)g_fRightMotorOut<0) g_fRightMotorOut -= MOTOR_OUT_DEAD_VAL;

  265.         /*輸出飽和處理，防止超出PWM范圍*/                        
  266.         if((int)g_fLeftMotorOut  > MOTOR_OUT_MAX)        g_fLeftMotorOut  = MOTOR_OUT_MAX;
  267.         if((int)g_fLeftMotorOut  < MOTOR_OUT_MIN)        g_fLeftMotorOut  = MOTOR_OUT_MIN;
  268.         if((int)g_fRightMotorOut > MOTOR_OUT_MAX)        g_fRightMotorOut = MOTOR_OUT_MAX;
  269.         if((int)g_fRightMotorOut < MOTOR_OUT_MIN)        g_fRightMotorOut = MOTOR_OUT_MIN;
  270.         
  271.     SetMotorVoltageAndDirection((int)g_fLeftMotorOut,(int)g_fRightMotorOut);
  272. }



  276. void GetMotorPulse(void)  //采集電機(jī)速度脈沖
  277. {         
  278.   g_s16LeftMotorPulse = TIM_GetCounter(TIM2);     
  279.   g_s16RightMotorPulse= -TIM_GetCounter(TIM4);
  280.   TIM2->CNT = 0;
  281.   TIM4->CNT = 0;   //清零

  283.   g_s32LeftMotorPulseSigma +=  g_s16LeftMotorPulse;
  284.   g_s32RightMotorPulseSigma += g_s16RightMotorPulse;
  285.         
  286.         g_iLeftTurnRoundCnt -= g_s16LeftMotorPulse;
  287.         g_iRightTurnRoundCnt -= g_s16RightMotorPulse;

  289. }

  291. void AngleCalculate(void)
  292. {
  293.         //-------加速度--------------------------
  294.         //量程為±2g時，靈敏度：16384 LSB/g
  295.     g_fGravityAngle = atan2(g_fAccel_y/16384.0,g_fAccel_z/16384.0) * 180.0 / 3.14;
  296.           g_fGravityAngle = g_fGravityAngle - g_iGravity_Offset;

  298.         //-------角速度-------------------------
  299.         //范圍為2000deg/s時，換算關(guān)系：16.4 LSB/(deg/s)
  300.         g_fGyro_x  = g_fGyro_x / 16.4;  //計(jì)算角速度值                           
  301.         g_fGyroAngleSpeed = g_fGyro_x;        
  302.         
  303.         //-------互補(bǔ)濾波---------------
  304.         g_fCarAngle = 0.98 * (g_fCarAngle + g_fGyroAngleSpeed * 0.005) + 0.02 *        g_fGravityAngle;
  305. }
  306. /***************************************************************
  307. ** 函數(shù)名稱: AngleControl
  308. ** 功能描述: 角度環(huán)控制函數(shù)           
  309. ***************************************************************/
  310. void AngleControl(void)         
  311. {
  312.         g_fAngleControlOut =  (CAR_ANGLE_SET-g_fCarAngle) * g_tCarAnglePID.P *5 + \
  313.         (CAR_ANGLE_SPEED_SET-g_fGyroAngleSpeed) * (g_tCarAnglePID.D /10);
  314. }



  318. /***************************************************************
  319. ** 函數(shù)名稱: SpeedControl
  320. ** 功能描述: 速度環(huán)控制函數(shù)
  321. ***************************************************************/

  323. void SpeedControl(void)
  324. {
  325.           float fP,fI;           
  326.         float fDelta;
  327.         
  328.         
  329.         g_fCarSpeed = (g_s32LeftMotorPulseSigma  + g_s32RightMotorPulseSigma ) * 0.5 ;
  330.   g_s32LeftMotorPulseSigma = g_s32RightMotorPulseSigma = 0;          //全局變量 注意及時清零
  331.             
  332.         g_fCarSpeed = 0.7 * g_fCarSpeedOld + 0.3 * g_fCarSpeed ;//低通濾波，使速度更平滑
  333.         g_fCarSpeedOld = g_fCarSpeed;

  335.         fDelta = CAR_SPEED_SET;
  336.         fDelta -= g_fCarSpeed;   
  337.         
  338.         fP = fDelta * (g_tCarSpeedPID.P);
  339.   fI = fDelta * (g_tCarSpeedPID.I/10.0);

  341.         g_fCarPosition += fI;
  342.         g_fCarPosition += g_fBluetoothSpeed;         
  343.         
  344. //積分上限設(shè)限
  345.         if((s16)g_fCarPosition > CAR_POSITION_MAX)    g_fCarPosition = CAR_POSITION_MAX;
  346.         if((s16)g_fCarPosition < CAR_POSITION_MIN)    g_fCarPosition = CAR_POSITION_MIN;
  347.         
  348.         g_fSpeedControlOutOld = g_fSpeedControlOutNew;
  349.   g_fSpeedControlOutNew = fP + g_fCarPosition;
  350. }
  351. /***************************************************************
  352. ** 函數(shù)名稱: SpeedControlOutput
  353. ** 功能描述: 速度環(huán)控制輸出函數(shù)-分多步逐次逼近最終輸出，盡可能將對直立環(huán)的干擾降低。
  354. ***************************************************************/
  355. void SpeedControlOutput(void)
  356. {
  357.   float fValue;
  358.   fValue = g_fSpeedControlOutNew - g_fSpeedControlOutOld ;
  359.   g_fSpeedControlOut = fValue * (g_u8SpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + g_fSpeedControlOutOld;
  360. }


  363. /***************************************************************
  364. ** 函數(shù)名稱: Scale
  365. ** 功能描述: 量程歸一化處理
  366. ***************************************************************/
  367. float Scale(float input, float inputMin, float inputMax, float outputMin, float outputMax) {
  368.   float output;
  369.   if (inputMin < inputMax)
  370.     output = (input - inputMin) / ((inputMax - inputMin) / (outputMax - outputMin));
  371.   else
  372.     output = (inputMin - input) / ((inputMin - inputMax) / (outputMax - outputMin));
  373.   if (output > outputMax)
  374.     output = outputMax;
  375.   else if (output < outputMin)
  376.     output = outputMin;
  377.   return output;
  378. }

  380. /***************************************************************
  381. ** 函數(shù)名稱: Steer
  382. ** 功能描述: 遙控速度及方向處理函數(shù)
  383. ***************************************************************/
  384. void Steer(float direct, float speed)
  385. {
  386.         if(direct > 0)
  387.                 g_fBluetoothDirection = Scale(direct, 0, 10, 0, 400);
  388.         else
  389.                 g_fBluetoothDirection = -Scale(direct, 0, -10, 0, 400);

  391.         if(speed > 0)
  392.                 g_iCarSpeedSet = Scale(speed, 0, 10, 0, 70);
  393.         else
  394.                 g_iCarSpeedSet = -Scale(speed, 0, -10, 0, 70);

  396. }

  398. void UltraControl(int mode)
  399. {
  400.         if(mode == 0)
  401.         {
  402.                 if((Distance >= 0) && (Distance<= 12))
  403.                 {//距離小于12cm則后退
  404.                         Steer(0, -4);
  405.                 }
  406.                 else if((Distance> 18) && (Distance<= 30))        
  407.                 {//距離大于18cm且小于30則前進(jìn)
  408.                         Steer(0, 4);
  409.                 }
  410.                 else
  411.                         Steer(0, 0);
  412.         }
  413.         else if(mode == 1)
  414.         {
  415.                 if((Distance >= 0) && (Distance<= 20))
  416.                 {//右轉(zhuǎn)750個脈沖計(jì)數(shù)，轉(zhuǎn)彎角度約為90度
  417.                         Steer(5, 0);
  418.                         g_iLeftTurnRoundCnt = 750;
  419.                         g_iRightTurnRoundCnt = -750;
  420.                 }
  421.                 if((g_iLeftTurnRoundCnt < 0)&&(g_iRightTurnRoundCnt > 0))
  422.                 {
  423.                         Steer(0, 4);
  424.                 }
  425.         }
  426. }

  428. /***************************************************************
  429. ** 函數(shù)名稱: TailingControl
  430. ** 功能描述: 紅外尋跡           
  431. ***************************************************************/
  432. void TailingControl(void)
  433. {
  434. #if INFRARE_DEBUG_EN > 0
  435.         char buff[32];        
  436. #endif
  437.         char result;
  438.         float direct = 0;
  439.         float speed = 0;

  441.         result = InfraredDetect();
  442.         
  443.         if(result & infrared_channel_Lc)
  444.                 direct = -10;
  445.         else if(result & infrared_channel_Lb)
  446.                 direct = -6;
  447.         else if(result & infrared_channel_La)
  448.                 direct = -4;
  449.         else if(result & infrared_channel_Rc)
  450.                 direct = 10;
  451.         else if(result & infrared_channel_Rb)
  452.                 direct = 6;
  453.         else if(result & infrared_channel_Ra)
  454.                 direct = 4;
  455.         else
  456.                 direct = 0.0;

  458.         speed = 3;

  460.         Steer(direct, speed);

  462. #if INFRARE_DEBUG_EN > 0
  463.         sprintf(buff, "Steer:%d, Speed:%d\r\n",(int)direct,  (int)speed);
  464.         DebugOutStr(buff);
  465. #endif
  466. }

復(fù)制代碼




歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1