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

关于球上自平衡机器人_球平衡机器人平衡原理

作者：从前慢现在也慢 | 2024-06-12 12:26:55

赞

踩

球平衡机器人平衡原理

其实弄清了无人机的控制系统之后，再看球上自平衡的控制系统，你就自己立马能写出来了，

 

无非是先位置控制再姿态控制，输入的话无人机是输入位置和偏航角，球上自平衡偏航角都不需要输入，而且位置只需要水平位置就可以了。而且如果你不循迹的话，感觉位置控制都可以不用，就一个姿态环就OK了。能站着就已经不错了。

 

我们自己的球车好像就只用了俯仰角和翻滚角两个PID。

 

 

球上自平衡车感觉和平衡车的控制系统差不多，都是一个角度环，再加一个速度环，我们学校球车的源码，还有平衡小车之家的球车的源码都是这样，一个角度环一个速度环。那这样就跟平衡车其实差不多。都不用当作什么四旋翼。其实我推测球车可能一个角度环都够了。球车的角度环和速度环好像不是串级的，好像也没必要串级啊角度的期望不永远都是0度么，没必要用速度环的输出给角度环作为角度的期望。似乎就像平衡车的速度环和角度环可以串级也可以不串级。角度环输入的偏差直接取当时实际的小车角度就行了，因为期望就是0。加速度环应该还是为了让它能够长久地站着。单角度环可能不容易长久地站着。可能因为速度比角度反应更灵敏？那讲道理不如加个角速度内环？是的，好好想想为什么加速度环。

 

有一个发现就是平衡小车之家的平衡车的代码框架和球车的代码框架差不多。

平衡车都是角度环+速度环+转向环，球上自平衡不需要转向，所以就只有角度环+速度环了，当然它的这个速度环要分解为X,Y方向，角度也有两个方向的角度，一个俯仰角一个翻滚角，平衡车它就一个方向的角度，这样就好理解一些，球上自平衡可以就看作自平衡小车演化出来的。无人机则是有三个角度，三个方向的速度，是的，可以总结一下这个，这里面应该有某种通用的模式。叫自由度？让我想起二自由度的倒立摆。我觉得PID的关键就是你要控制几个量，就有几个PID嘛，就这么简单的事情嘛，你要控制一个角度，就有一个角度的PID嘛，你要控制一个维度的速度，那就有一个PID嘛，平面上的东西，你确定了两个维度的速度就可以完全确定一个东西了嘛，这有点像线性代数里面的矩阵的秩或者向量组的秩，

三维空间里面你最少确定三个量就可以确定一个东西，所以你用三个PID，控制住三个量，就可以控制住一个东西，对吧，无人机，那个系统，所以只需要输入三维坐标和偏航角，你就可以完全确定一个无人机，对吧，这是最少的最少的，我们为了主球效果好点多点也无所谓应该，但是至少这样我们能够更为系统地去分析一个控制系统。平衡车一维地我们最少控制一个量也就是角度就可以了，这就像一级倒立摆，二级倒立摆，三级倒立摆，其实就是一维空间二维空间三维空间地倒立摆嘛，一维的你就只需控制一个量嘛，二维的控制两个量嘛，自由度应该也是这个意思。这样我们去分析一个控制系统会更有条理。

球面自平衡，讲道理，如果不考虑偏航角，整个系统我就输入X,Y就可以确定了，对吧，平衡车我输入X，就可以了，无人机我输入X,Y,Z,偏航角就可以了。甚至板球控制系统我也可以这么分析对吧。我要控制一个球，在平面上，我只需要对这个系统输入X,Y就可以对吧。

这样分析感觉就像变成一类题型，万变不离其宗。

像无人机，你外在看是四个控制量，XYZ偏航角，但是你直接能控制的是四个电机，那么你就得把这两者联系起来。平衡车也是，直接控制的就是电机，球上自平衡也是，直接控制的就三个电机。我发现这些系统它的电机数量和它的外在的控制量是一致的，平衡车本质上可以看作一个电机，另一个起支撑作用，是这样的，无人机如果三个电机的话，它的偏航角没法保证，可能会旋转，球上自平衡两个电机也不行，独轮车一个电机不行，真的是这样的，电机数量不能少于控制系统的维度或者说自由度。少一个都不行，都保证不了，这里面肯定是有道理的。这样我对控制系统的认识又上升到一个新的层面。串级PID的作用可能就是一步步把外在的控制量转化为直接控制量。

 

 

 

平衡车里面已经承认了角度环和速度环直接相加是会有干扰的，我估计球车这样直接加也是会有干扰的。

 

 

问你如何调PID，肯定是先调姿态环，再调速度环，而且姿态环的频率应该是远高于速度环的。

 

感觉球车只需要控制住两个方向的角度和两个方向的速度总共四个PID环就OK了。

是的，如下面所说，可以等效为两个一级倒立摆模型！！！这个太棒了，这不是和我上面的分析本质一样么。平衡车就像一个一级倒立摆模型。

 

 

下面是平衡小车之家的球车的代码，可以看到给了两组PID参数。一组是角度环一组是速度环。

X,Y的期望速度可能是遥控器给的。还有如果我是让球车自主循迹呢，是不是要加位置环？或者让球车定点不移动，是不是要加位置环。确实如果是遥控的话，位置环就形同虚设了，就像无人机，期望的角度或者速度直接遥控器给你了，不需要位置环输出，球车应该也是的，就是如果是遥控，期望速度那遥控器直接给力了，不需要位置，如果是自主循迹，我觉得和无人机一样，应该是需要一个位置环的。无人机有不同的模式，我觉得球车应该也是的。

真正PID的代码在这，在control.c的int EXTI15_10_IRQHandler(void)这个函数里。这里面可以清晰看到角度环和速度环是直接叠加的。我仔细看了下学校的球车代码，也是角度环的输出和速度环的输出相加

我仔细看了下学校的球车代码，也是角度环的输出和速度环的输出相加，仔细注意，第二个图等号右边的Motor_No_1_PWM就是第一张图里面角度环的，第二张图就是说明了最后角度环输出的PWM和速度环输出的PWM相加了，最终就是电机的PWM。

 

 

 

这是State_Capture.c这个文件

  1. #include "common.h"
  2. #include "include.h"
  3. #include "State_Capture.h"
  4.  
  5. extern unsigned char Bluetooth_flag;
  6. extern unsigned char  Command;
  7. void Gyro_Control(void);
  8. void Kalman_Fliter(void);
  9. IMU_Data State_Data;
  10.  
  11.  
  12. float invSqrt(float x)
  13. {
  14.   float halfx = 0.5f * x;
  15.   float y = x;
  16.   long i = *(long*)&y;
  17. 	
  18.   i = 0x5f3759df - (i>>1);
  19.   y = *(float*)&i;
  20.   y = y * (1.5f - (halfx * y * y));
  21. 	
  22.   return y;
  23. }
  24.  
  25. int16 X_Gyro_OFF,Y_Gyro_OFF,Z_Gyro_OFF;
  26. void ReadIMU_OFF()
  27. {
  28.   uint16 i=0;
  29.   int32 Sum_X,Sum_Y,Sum_Z;
  30.   for(i=0;i<100;i++)
  31.   {
  32.     Sum_X += GetData(GYRO_XOUT_H);
  33.     Sum_Y += GetData(GYRO_YOUT_H);
  34.     Sum_Z += GetData(GYRO_ZOUT_H);
  35.     DELAY_MS(5);
  36.   }
  37.  
  38.   X_Gyro_OFF=Sum_X/100;
  39.   Y_Gyro_OFF=Sum_Y/100;
  40.   Z_Gyro_OFF=Sum_Z/100;
  41. }
  42. void Angle_Get()
  43. {
  44.  
  45. float ACCE_X_TEMP,ACCE_Y_TEMP,ACCE_Z_TEMP;
  46. GET_ACCE();
  47.  
  48.  ACCE_X_TEMP=IMU.X_ACCE_Data;
  49. if(ACCE_X_TEMP>16384)
  50.     ACCE_X_TEMP=(8192-ACCE_X_TEMP)/8192.0;
  51.   else
  52.     ACCE_X_TEMP=-ACCE_X_TEMP/8192.0;
  53.   
  54. ACCE_Y_TEMP=IMU.Y_ACCE_Data;
  55. if(ACCE_Y_TEMP>16384)
  56.     ACCE_Y_TEMP=(8192-ACCE_Y_TEMP)/8192.0;
  57. else
  58.     ACCE_Y_TEMP=-ACCE_Y_TEMP/8192.0;
  59.   
  60. ACCE_Z_TEMP=IMU.Z_ACCE_Data;
  61. if(ACCE_Z_TEMP>16384)
  62.     ACCE_Z_TEMP=(8192-ACCE_Z_TEMP)/8192.0;
  63. else
  64.     ACCE_Z_TEMP=-ACCE_Z_TEMP/8192.0;
  65.  
  66. State_Data.X_Angle=57.3*atan(ACCE_X_TEMP*invSqrt(ACCE_Y_TEMP*ACCE_Y_TEMP+ACCE_Z_TEMP*ACCE_Z_TEMP));
  67. State_Data.Y_Angle=57.3*atan(ACCE_Y_TEMP*invSqrt(ACCE_X_TEMP*ACCE_X_TEMP+ACCE_Z_TEMP*ACCE_Z_TEMP));
  68. State_Data.Z_Angle=57.3*atan(sqrt(ACCE_X_TEMP*ACCE_X_TEMP+ACCE_Y_TEMP*ACCE_Y_TEMP)/ACCE_Z_TEMP);																  
  69.  
  70. }
  71.  
  72. void Gyro_Get()
  73. {
  74.   GET_GYRO();
  75.   State_Data.X_Omiga=(IMU.X_GYRO_Data-X_Gyro_OFF)/16.4;
  76.   State_Data.Y_Omiga=(IMU.Y_GYRO_Data-Y_Gyro_OFF)/16.4;
  77.   State_Data.Z_Omiga=(IMU.Z_GYRO_Data-Z_Gyro_OFF)/16.4;
  78. }
  79.  
  80. float Rotate_Angle=0;
  81. void Get_Bnlance_State()
  82. {        
  83.       Angle_Get();
  84.       Gyro_Get();
  85.       Rotate_Angle+=State_Data.Z_Omiga*0.002;
  86.       Kalman_Fliter();
  87.   
  88. }
  89.  
  90. float Rotate_kp=8,Rotate_ki=0.3,Rotate_kd=0.3;
  91. float Rotate_pwm,rotate_integrate=0;
  92. void Rotate_Control()
  93. {
  94.   if(ABS(Rotate_Angle)<=5) rotate_integrate+=Rotate_ki*Rotate_Angle;
  95.   if(rotate_integrate>=20) rotate_integrate=20;
  96.   if(rotate_integrate>=-20) rotate_integrate=-20;
  97.  
  98.   Rotate_pwm=Rotate_kp*Rotate_Angle+rotate_integrate-Rotate_kd*State_Data.Z_Omiga;
  99.   if(Rotate_pwm>=150)   Rotate_pwm=150;
  100.   if(Rotate_pwm<=-150)  Rotate_pwm=-150;
  101.  
  102. }
  103.  
  104. #define dt 0.002
  105. float W_Pitch=0.88,W_Roll=0.9;
  106.  
  107. /**************************************************************************************							  
  108.                                 角度融合																	
  109. **************************************************************************************/							 //*************
  110. float Kalman_Filter_Roll(float angle_m,float gyro_m)//非矩阵卡尔曼滤波												 //*************
  111. {																												 //*************
  112. static float x_roll=0;																								 //*************
  113. static float P_roll=0.000001; 																						//*************
  114. static float Q_roll=0.000001;																						 //*************
  115. static float R_roll=0.35;//0.35																						//*************
  116. static float k_roll=0;																								//*************
  117. gyro_m=W_Roll*gyro_m;//角速度修正																					//*************
  118. x_roll=x_roll+gyro_m*dt;																									//*************
  119. P_roll=P_roll+Q_roll;																											//*************
  120. k_roll=P_roll/(P_roll+R_roll);																										//*************
  121. x_roll=x_roll+k_roll*(angle_m-x_roll);																							   //*************
  122. P_roll=(1-k_roll)*P_roll;																									   //*************
  123. return x_roll;																										//*************
  124. }
  125.  
  126. /**************************************************************************************							  
  127.                                 角度融合																	
  128. **************************************************************************************/							 //*************
  129. float Kalman_Filter_Pitch(float angle_m0,float gyro_m0)//非矩阵卡尔曼滤波												 //*************
  130. {																												 //*************
  131. static float x_pitch=0;																								 //*************
  132. static float P_pitch=0.000001; 																						//*************
  133. static float Q_pitch=0.000001;																						 //*************
  134. static float R_pitch=0.35;//0.35																						//*************
  135. static float k_pitch=0;																								//*************
  136. gyro_m0=W_Pitch*gyro_m0;//角速度修正																					//*************
  137. x_pitch=x_pitch+gyro_m0*dt;																									//*************
  138. P_pitch=P_pitch+Q_pitch;																											//*************
  139. k_pitch=P_pitch/(P_pitch+R_pitch);																										//*************
  140. x_pitch=x_pitch+k_pitch*(angle_m0-x_pitch);																							   //*************
  141. P_pitch=(1-k_pitch)*P_pitch;																									   //*************
  142. return x_pitch;																										//*************
  143. }
  144. uint8 Danger_Flag=0;
  145. float Pitch_Angle=0,Pitch_Gyro=0;
  146. float Roll_Angle=0,Roll_Gyro=0;
  147. void Kalman_Fliter()
  148. {
  149.   
  150.   Pitch_Gyro=-State_Data.Y_Omiga;
  151.   Roll_Gyro=State_Data.X_Omiga;
  152.   
  153.   Pitch_Angle=Kalman_Filter_Pitch(State_Data.X_Angle,Pitch_Gyro);
  154.   Roll_Angle  =Kalman_Filter_Roll(State_Data.Y_Angle,Roll_Gyro);
  155.   
  156.       
  157.     if(ABS(Pitch_Angle)>=10)  Danger_Flag=1;
  158.     if(ABS(Roll_Angle)>=10)  Danger_Flag=1;
  159. }
  160.  
  161.  
  162. float Pitch_Kp=35,Pitch_Ki=10,Pitch_Kd=0;
  163. float Roll_Kp=35,Roll_Ki=10,Roll_Kd;
  164. float Pitch_Balance_Pwm;
  165. float Roll_Balance_Pwm;
  166. float Pitch_Setpoint=-1.2;
  167. float Roll_Setpoint=0.3;
  168. float Motor_No_1_PWM,Motor_No_2_PWM,Motor_No_3_PWM;
  169. float Pitch_Angle_Output,Roll_Angle_Output;
  170. float Pitch_Angle_Integrate,Roll_Angle_Integrate;
  171. float Pitch_Angle_Error,Roll_Angle_Error;
  172. void Ball_Balance_Control()
  173. {
  174.   Pitch_Angle_Error=(Pitch_Angle-Pitch_Setpoint) ;
  175.   Roll_Angle_Error=(Roll_Angle-Roll_Setpoint);
  176.   if(Pitch_Angle_Error<=3&&Pitch_Angle_Error>=-3) Pitch_Angle_Integrate+=Pitch_Ki*Pitch_Angle_Error;
  177.   if(Pitch_Angle_Integrate>=20) Pitch_Angle_Integrate=20;
  178.   if(Pitch_Angle_Integrate<=-20) Pitch_Angle_Integrate=-20;
  179.   
  180.   if(Roll_Angle_Error<=3&&Roll_Angle_Error>=-3) Roll_Angle_Integrate+=Roll_Ki*Roll_Angle_Error;  
  181.   if(Roll_Angle_Integrate>=20)  Roll_Angle_Integrate=20;
  182.   if(Roll_Angle_Integrate<=-20) Roll_Angle_Integrate=-20;
  183.   
  184.   
  185.   Pitch_Angle_Output=Pitch_Kp*Pitch_Angle_Error+Pitch_Angle_Integrate+Pitch_Kd*Pitch_Gyro;
  186.   
  187.   Roll_Angle_Output =Roll_Kp*Roll_Angle_Error+Roll_Angle_Integrate+Roll_Kd*Roll_Gyro;
  188.   
  189.  
  190.  // Motor_No_1_PWM=Roll_Balance_Pwm/2;
  191.  // Motor_No_2_PWM=(1.732*Pitch_Balance_Pwm-Roll_Balance_Pwm)/4;
  192.  // Motor_No_3_PWM=(-1.732*Pitch_Balance_Pwm-Roll_Balance_Pwm)/4;
  193.   
  194.     
  195.  
  196.   
  197.  //Motor_No_1_PWM=0;
  198.  /// Motor_No_2_PWM=Pitch_Balance_Pwm;
  199.   //Motor_No_3_PWM=-Pitch_Balance_Pwm;
  200.   
  201.   //Motor_No_1_PWM=Roll_Balance_Pwm/2;
  202.  // Motor_No_2_PWM=-Roll_Balance_Pwm/4;
  203.  // Motor_No_3_PWM=-Roll_Balance_Pwm/4;
  204.   
  205. }
  206.  
  207.  
  208. float Pitch_Gyro_Kp=12,Pitch_Gyro_Ki=5,Pitch_Gyro_Kd=100;
  209. float Roll_Gyro_Kp=12,Roll_Gyro_Ki=5,Roll_Gyro_Kd=100;
  210. float Pitch_Gyro_Integrate=0;
  211. float Roll_Gyro_Integrate=0;
  212. float Pitch_Gyro_Error,Roll_Gyro_Error;
  213. float Last_Pitch_Gyro_Error,Last_Roll_Gyro_Error;
  214. void Gyro_Control()
  215. {
  216.   Last_Pitch_Gyro_Error=Pitch_Gyro_Error;
  217.   Last_Roll_Gyro_Error=Roll_Gyro_Error;
  218.     
  219.   Pitch_Gyro_Error=Pitch_Angle_Output-(-Pitch_Gyro)                                      ;
  220.   Roll_Gyro_Error=Roll_Angle_Output-(-Roll_Gyro);
  221.   
  222.   if(Pitch_Gyro_Error<=20&&Pitch_Gyro_Error>=-20) Pitch_Gyro_Integrate+=Pitch_Gyro_Ki*Pitch_Gyro_Error;
  223.   if(Pitch_Gyro_Integrate>=300) Pitch_Gyro_Integrate=300;
  224.   if(Pitch_Gyro_Integrate<=-300) Pitch_Gyro_Integrate=-300;
  225.   
  226.   if(Roll_Gyro_Error<=20&&Roll_Gyro_Error>=-20) Roll_Gyro_Integrate+=Roll_Gyro_Ki*Roll_Gyro_Error;  
  227.   if(Roll_Gyro_Integrate>=300)  Roll_Gyro_Integrate=300;
  228.   if(Roll_Gyro_Integrate<=-300) Roll_Gyro_Integrate=-300;
  229.   
  230.   
  231.   
  232.   
  233.   Pitch_Balance_Pwm=Pitch_Gyro_Kp*Pitch_Gyro_Error+Pitch_Gyro_Kd*(Pitch_Gyro_Error-Last_Pitch_Gyro_Error);
  234.   Roll_Balance_Pwm =Roll_Gyro_Kp *Roll_Gyro_Error +Roll_Gyro_Kd *(Roll_Gyro_Error -Last_Roll_Gyro_Error );
  235.   
  236.   
  237.   
  238.  
  239.   Motor_No_1_PWM=-Roll_Balance_Pwm/2;
  240.   Motor_No_2_PWM=(-1.732*Pitch_Balance_Pwm+Roll_Balance_Pwm)/4;
  241.   Motor_No_3_PWM=(1.732*Pitch_Balance_Pwm+Roll_Balance_Pwm)/4;
  242.   
  243.   Rotate_Control();
  244. }
  245.  
  246.  
  247.  
  248.  
  249.  
  250. extern int Motor_Speed_No_1,Motor_Speed_No_2,Motor_Speed_No_3;//三个电机反馈脉冲
  251. float Pitch_Speed_Feedback,Roll_Speed_Feedback;//速度返回值
  252. float Pitch_Speedset=0,Roll_Speedset=0;//速度设定
  253. float Pitch_Speederror,Roll_Speederror;//速度偏差
  254. float Last_Pitch_Speederror,Last_Roll_Speederror;//速度偏差
  255. float Pitch_SpeedIntegrate,Roll_SpeedIntegrate;//速度积分
  256. float Pitch_Speed_Kp=6,Pitch_Speed_Ki=1.8;//速度PI
  257. float Roll_Speed_Kp=6,Roll_Speed_Ki=1.8;//速度PI
  258. float Pitch_Speed_Output,Roll_Speed_Output;//速度输出
  259. float Last_Pitch_Speed_Output,Last_Roll_Speed_Output;//上次速度输出
  260. extern uint8 Speed_Sampile_Cnt;
  261. float Pitch_Speed_PWM,Roll_Speed_PWM;
  262.  
  263. void SpeedControl()
  264. {
  265.   
  266.   Last_Pitch_Speed_Output=Pitch_Speed_Output;
  267.   Last_Roll_Speed_Output=Roll_Speed_Output;
  268.   
  269.   Last_Pitch_Speederror=Pitch_Speederror;
  270.   Last_Roll_Speederror=Roll_Speederror;
  271.   
  272.   
  273.   Roll_Speed_Feedback=Motor_Speed_No_1/2;
  274.   Pitch_Speed_Feedback=(Motor_Speed_No_2+Motor_Speed_No_3)*1.732/2;
  275.   
  276.   Pitch_Speederror=Pitch_Speedset-Pitch_Speed_Feedback;
  277.   Roll_Speederror=Roll_Speederror-Roll_Speed_Feedback;
  278.   
  279.   if(Pitch_Speederror>=100)  Pitch_Speederror=100;
  280.   if(Pitch_Speederror<=-100)  Pitch_Speederror=-100;
  281.   if(ABS(Pitch_Speederror)<=15)  Pitch_SpeedIntegrate+=Pitch_Speed_Ki*Pitch_Speederror;
  282.   if(Pitch_SpeedIntegrate>=200)  Pitch_SpeedIntegrate=200;
  283.   if(Pitch_SpeedIntegrate<=-200) Pitch_SpeedIntegrate=-200;
  284.   Pitch_Speed_Output=Pitch_Speed_Kp*Pitch_Speederror+Pitch_SpeedIntegrate; 
  285.  
  286.   
  287.   //Pitch_Speed_PWM=Last_Pitch_Speed_Output+(Pitch_Speed_Output);
  288.   
  289.  
  290.   if(Pitch_Speed_Output>=500)  Pitch_Speed_Output=500;
  291.   if(Pitch_Speed_Output<=-500) Pitch_Speed_Output=-500;  
  292. ///  
  293.   if(Roll_Speederror>=100)   Roll_Speederror=100;
  294.   if(Roll_Speederror<=-100)  Roll_Speederror=-100;
  295.   if(ABS(Roll_Speederror)<=15)     Roll_SpeedIntegrate+=Roll_Speed_Ki*Roll_Speederror;
  296.   if(Roll_SpeedIntegrate>=200)  Roll_SpeedIntegrate=200;
  297.   if(Roll_SpeedIntegrate<=-200) Roll_SpeedIntegrate=-200; 
  298.   Roll_Speed_Output=Roll_Speed_Kp*Roll_Speederror+Roll_SpeedIntegrate;
  299.   //Roll_Speed_PWM=Last_Roll_Speed_Output+(Roll_Speed_Output);
  300.   
  301.   
  302.   
  303.   if(Roll_Speed_Output>=500)  Roll_Speed_Output=500;
  304.   if(Roll_Speed_Output<=-500) Roll_Speed_Output=-500; 
  305. }
  306.  
  307.  
  308. void Speed_Control_Output()
  309. { 
  310.   Pitch_Speed_PWM=Last_Pitch_Speed_Output+(Speed_Sampile_Cnt+1)*(Pitch_Speed_Output-Last_Pitch_Speed_Output)/50;
  311.   Roll_Speed_PWM=Last_Roll_Speed_Output+(Speed_Sampile_Cnt+1)*(Roll_Speed_Output-Last_Roll_Speed_Output)/50;
  312.   //Pitch_Speed_PWM=Last_Pitch_Speed_Output+(Pitch_Speed_Output);
  313.   //Roll_Speed_PWM=Last_Roll_Speed_Output+(Roll_Speed_Output);
  314.   
  315. }
  316.  
  317.  
  318.  
  319. void Control_Output()
  320. { 
  321.  
  322.   
  323.  Motor_No_1_PWM=Motor_No_1_PWM+Roll_Speed_PWM/2+Pitch_Speed_PWM*0-Rotate_pwm;
  324.  Motor_No_2_PWM=Motor_No_2_PWM-Roll_Speed_PWM/4+Pitch_Speed_PWM*1.732/4-Rotate_pwm;
  325.  Motor_No_3_PWM=Motor_No_3_PWM-Roll_Speed_PWM/4-Pitch_Speed_PWM*1.732/4-Rotate_pwm;
  326. if(Danger_Flag==1)
  327. {
  328. Motor_No_1_PWM=0;
  329. Motor_No_2_PWM=0;
  330. Motor_No_3_PWM=0;
  331. }
  332.  
  333. if(Bluetooth_flag==1)
  334.  {
  335.   // if(Command==0)        {Motor_No_1_PWM=0;  Motor_No_3_PWM=0;    Motor_No_2_PWM=0;}
  336.   // else if(Command==1)   {Motor_No_1_PWM=-149;Motor_No_3_PWM=0;Motor_No_2_PWM=408;}
  337.   // else if(Command==2)   {Motor_No_1_PWM=0;Motor_No_3_PWM=-500;Motor_No_2_PWM=500;}
  338.   // else if(Command==3)   {Motor_No_1_PWM=149;Motor_No_3_PWM=-408;Motor_No_2_PWM=0;}
  339.    //else if(Command==4)   {Motor_No_1_PWM=-500;Motor_No_3_PWM=433;Motor_No_2_PWM=433;}
  340.   // else if(Command==5)   {Motor_No_1_PWM=500;Motor_No_3_PWM=-433;Motor_No_2_PWM=-433;}
  341.   // else if(Command==6)   {Motor_No_1_PWM=0;Motor_No_3_PWM=408;Motor_No_2_PWM=149;}
  342.   // else if(Command==7)   {Motor_No_1_PWM=0;Motor_No_3_PWM=500;Motor_No_2_PWM=-500;}
  343.   // else if(Command==8)   {Motor_No_1_PWM=408;Motor_No_3_PWM=149;Motor_No_2_PWM=0;}
  344.   // else if(Command=='A') {Motor_No_1_PWM=-500;Motor_No_3_PWM=-500;Motor_No_2_PWM=-500;}
  345.   // else if(Command=='B') {Motor_No_1_PWM=-500;Motor_No_3_PWM=-500;Motor_No_2_PWM=-500;}
  346.   // else if(Command=='C') {Motor_No_1_PWM=-500;Motor_No_3_PWM=-500;Motor_No_2_PWM=-500;}
  347.   // else if(Command=='D') {Motor_No_1_PWM=-500;Motor_No_3_PWM=-500;Motor_No_2_PWM=-500;}
  348.   // else                  {Motor_No_1_PWM=0;   Motor_No_3_PWM=0;   Motor_No_2_PWM=0;}
  349.    if(Command==0)        {Motor_No_1_PWM=0;  Motor_No_3_PWM=0;    Motor_No_2_PWM=0;}
  350. //对角   
  351.    else if(Command==1)   {Motor_No_1_PWM=-2*288;Motor_No_3_PWM=0;Motor_No_2_PWM=2*288;}
  352.    else if(Command==3)   {Motor_No_1_PWM=2*288;Motor_No_3_PWM=-2*288;Motor_No_2_PWM=0;}
  353.    else if(Command==6)   {Motor_No_1_PWM=-2*288;Motor_No_3_PWM=2*288;Motor_No_2_PWM=0;}
  354.    else if(Command==8)   {Motor_No_1_PWM=2*288;Motor_No_3_PWM=0;Motor_No_2_PWM=-2*288;}
  355.    
  356. //正交  
  357.    else if(Command==2)   {Motor_No_1_PWM=0;Motor_No_3_PWM=-1.5*500;Motor_No_2_PWM=1.5*500;}
  358.    else if(Command==4)   {Motor_No_1_PWM=-1.5*500;Motor_No_3_PWM=1.5*433/2;Motor_No_2_PWM=1.5*433/2;}
  359.    else if(Command==5)   {Motor_No_1_PWM=1.5*500;Motor_No_3_PWM=-1.5*433/2;Motor_No_2_PWM=-1.5*433/2;}
  360.    else if(Command==7)   {Motor_No_1_PWM=0;Motor_No_3_PWM=1.5*500;Motor_No_2_PWM=-1.5*500;}
  361.    
  362. //自旋
  363.    else if(Command=='A') {Motor_No_1_PWM=-500;Motor_No_3_PWM=-500;Motor_No_2_PWM=-500;}
  364.    else if(Command=='B') {Motor_No_1_PWM=500;Motor_No_3_PWM=500;Motor_No_2_PWM=500;}
  365.    else if(Command=='C') {Motor_No_1_PWM=999;Motor_No_3_PWM=999;Motor_No_2_PWM=999;}
  366.    else                  {Motor_No_1_PWM=0;   Motor_No_3_PWM=0;   Motor_No_2_PWM=0;}
  367.  }
  368.   
  369. if(Motor_No_1_PWM>=1000)  Motor_No_1_PWM=1000;
  370. if(Motor_No_1_PWM<=-1000) Motor_No_1_PWM=-1000;
  371.  
  372. if(Motor_No_2_PWM>=1000)  Motor_No_2_PWM=1000;
  373. if(Motor_No_2_PWM<=-1000) Motor_No_2_PWM=-1000;
  374.  
  375. if(Motor_No_3_PWM>=1000)  Motor_No_3_PWM=1000;
  376. if(Motor_No_3_PWM<=-1000) Motor_No_3_PWM=-1000;
  377.  
  378.    if(Motor_No_1_PWM>=0)//电机1#
  379.    {
  380.      ftm_pwm_duty(FTM0, FTM_CH1,(uint32)(Motor_No_1_PWM/10));
  381.      ftm_pwm_duty(FTM0, FTM_CH2,0); 
  382.    }
  383.    else
  384.    {
  385.      ftm_pwm_duty(FTM0, FTM_CH1,0);
  386.      ftm_pwm_duty(FTM0, FTM_CH2,(uint32)(-Motor_No_1_PWM/10)); 
  387.    }
  388.  
  389.    
  390.       if(Motor_No_2_PWM>=0)//电机2#
  391.    {
  392.      ftm_pwm_duty(FTM0, FTM_CH5,(uint32)(Motor_No_2_PWM/10));
  393.      ftm_pwm_duty(FTM0, FTM_CH6,0); 
  394.    }
  395.    else
  396.    {
  397.      ftm_pwm_duty(FTM0, FTM_CH5,0);
  398.      ftm_pwm_duty(FTM0, FTM_CH6,(uint32)(-Motor_No_2_PWM/10)); 
  399.    }
  400.    
  401.    
  402.    if(Motor_No_3_PWM>=0)//电机3#
  403.    {
  404.      ftm_pwm_duty(FTM0, FTM_CH4,(uint32)(Motor_No_3_PWM/10));
  405.      ftm_pwm_duty(FTM0, FTM_CH7,0); 
  406.    }
  407.    else
  408.    {
  409.      ftm_pwm_duty(FTM0, FTM_CH4,0);
  410.      ftm_pwm_duty(FTM0, FTM_CH7,(uint32)(-Motor_No_3_PWM/10)); 
  411.    }
  412.    
  413.  
  414.  
  415. }

 

 

平衡小车之家球车里地control.c

  1. #include "control.h"	
  2. #include "filter.h"	
  3.   /**************************************************************************
  4. 作者：平衡小车之家
  5. 我的淘宝小店：http://shop114407458.taobao.com/
  6. **************************************************************************/
  7. u8 Flag_Target;                             //相关标志位
  8. float Voltage_Count,Voltage_All;  //电压采样相关变量
  9. int Balance_Pwm_X,Velocity_Pwm_X,Balance_Pwm_Y,Velocity_Pwm_Y;
  10. #define X_PARAMETER           (0.5f)               
  11. #define Y_PARAMETER           (sqrt(3)/2.f)      
  12. #define L_PARAMETER           (1.0f)  
  13. /**************************************************************************
  14. 函数功能：小车运动 逆运动学分析
  15. 入口参数：X Y Z 三轴速度或者位置
  16. 返回  值：无
  17. **************************************************************************/
  18. void Kinematic_Analysis(float Vx,float Vy,float Vz)
  19. {
  20. 	      Target_A   = Vx + L_PARAMETER*Vz;
  21.         Target_B   = -X_PARAMETER*Vx + Y_PARAMETER*Vy + L_PARAMETER*Vz;
  22. 	      Target_C   = -X_PARAMETER*Vx - Y_PARAMETER*Vy + L_PARAMETER*Vz;
  23. }
  24. /**************************************************************************
  25. 函数功能：小车运动 正运动学分析 注：实际注释掉的是理想的运动学分析，实际使用放大了3倍，对计算结果没影响，主要是避免舍去误差
  26. 入口参数：A B C三个电机的速度
  27. 返回  值：无
  28. **************************************************************************/
  29. void Forward_Kinematics(float Va,float Vb,float Vc)
  30. {
  31. //		Motor_X=(Va*2-Vb-Vc)/3;
  32. //		Motor_Y=(Vb-Vc)/2/Y_PARAMETER;
  33. //		Motor_Z=(Va+Vb+Vc)/3;
  34. 			Motor_X=Va*2-Vb-Vc;
  35. 		  Motor_Y=(Vb-Vc)*sqrt(3);
  36. 		  Motor_Z=Va+Vb+Vc;
  37. }
  38. /**************************************************************************
  39. 函数功能：所有的控制代码都在这里面
  40.          5ms定时中断由MPU6050的INT引脚触发
  41.          严格保证采样和数据处理的时间同步				 
  42. **************************************************************************/
  43. int EXTI15_10_IRQHandler(void) 
  44. {    
  45. 	 if(INT==0)		
  46. 	{     
  47. 		  EXTI->PR=1<<15;   //清除LINE5上的中断标志位  		
  48. 		  Flag_Target=!Flag_Target;
  49. 		  if(delay_flag==1)
  50. 			 {
  51. 				 if(++delay_50==10)	 delay_50=0,delay_flag=0;  //给主函数提供50ms的精准延时
  52. 			 }
  53. 		  if(Flag_Target==1)                     //5ms读取一次陀螺仪和加速度计的值
  54. 			{	 
  55. 				Read_DMP();                          //===更新姿态		
  56. 				Key();													     //扫描按键变化	
  57. 				if(Pitch_Bias>-0.8&&Pitch_Bias<0.8&&Roll_Bias>-0.8&&Roll_Bias<0.8)Led_Flash(0);     //接近平衡位置，常亮
  58.         else 		Led_Flash(100);  		     //===LED闪烁
  59. 				Voltage_All+=Get_battery_volt();     //多次采样累积
  60. 				if(++Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//求平均值 获取电池电压	
  61. 				return 0;	                                               
  62. 			}     //===10ms控制一次	
  63. 			 Read_DMP();    //===更新姿态		
  64. 			 Roll_Bias =Roll-Roll_Zero;		//获取Y方向的偏差
  65. 		   Pitch_Bias=Pitch-Pitch_Zero; //获取X方向的偏差		
  66. 			 Forward_Kinematics(Motor_A,Motor_B,Motor_C);  //正运动学分析，得到X Y Z 方向的速度
  67. 		   Balance_Pwm_X=balance_Pitch(Pitch_Bias,gyro[1]);   //X方向的倾角控制
  68. 			 Balance_Pwm_Y=-balance_Roll(Roll_Bias, gyro[0]);   //Y方向的倾角控制
  69. 			 Velocity_Pwm_X=velocity_X(Motor_X);      //X方向的速度控制
  70. 			 Velocity_Pwm_Y=velocity_Y(Motor_Y);  	  //Y方向的速度控制
  71. 			 Move_X =Balance_Pwm_X+Velocity_Pwm_X;    //===X方向控制量累加				
  72. 			 Move_Y =Balance_Pwm_Y+Velocity_Pwm_Y;    //===Y方向控制量累加	
  73. 			 Move_Z=0;  //Z轴不做控制
  74. 				Kinematic_Analysis(Move_X,Move_Y,Move_Z);//逆运动学分析，得到A B C电机控制量
  75. 				Motor_A=Target_A;//直接调节PWM频率
  76. 				Motor_B=Target_B;//直接调节PWM频率
  77. 				Motor_C=Target_C;//直接调节PWM频率	 			 		 
  78. 				Xianfu_Pwm(1300);//===PWM频率限幅  因为频率太大之后扭矩减小不再满足小车需求
  79. 			 if(Turn_Off(Voltage)==0)	Set_Pwm(Motor_A,Motor_B,Motor_C);    //赋值给PWM寄存器
  80. 			 if(Flag_Zero)   //这是人为更新零点值
  81. 			 {
  82. 				 Roll_Zero=Roll; //ROLL更新
  83. 				 Pitch_Zero=Pitch; //Pitch更新
  84. 				 Flag_Zero=0;  //清零过程仅执行一次 等待下一次指令
  85. 			 }
  86.  }
  87. 	 return 0;	 
  88. } 
  89.  
  90. /**************************************************************************
  91. 函数功能：直立PD控制Y
  92. 入口参数：角度、角速度
  93. 返回  值：直立控制PWM
  94. 作    者：平衡小车之家
  95. **************************************************************************/
  96. int balance_Roll(float Angle,float Gyro)
  97. {  
  98.    float Bias;
  99. 	 int balance;
  100. 	 Bias=Angle;  //===求出平衡的角度中值 和机械等重心分布相关
  101. 	 balance=Balance_Kp*Bias+Gyro*Balance_Kd/100;   //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数 
  102. 	 return balance;
  103. }
  104. /**************************************************************************
  105. 函数功能：直立PD控制
  106. 入口参数：角度、角速度
  107. 返回  值：直立控制PWM
  108. 作    者：平衡小车之家X
  109. **************************************************************************/
  110. int balance_Pitch(float Angle,float Gyro)
  111. {  
  112.    float Bias;
  113. 	 int balance;
  114. 	 Bias=Angle;        //===求出平衡的角度中值和机械等重心分布相关
  115. 	 balance=Balance_Kp*Bias+Gyro*Balance_Kd/100;   //===计算平衡控制的电机PWM  PD控制  kp是P系数 kd是D系数 
  116. 	 return balance;
  117. }
  118.  
  119. /**************************************************************************
  120. 函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity
  121. 入口参数：左轮速度、右轮速度
  122. 返回  值：速度控制PWM
  123. 作    者：平衡小车之家
  124. **************************************************************************/
  125. int velocity_X(int velocity)
  126. {  
  127.     static float Velocity,Encoder_Least,Encoder,Movement;
  128. 	  static float Target_Velocity=2500;
  129. 	  static float Encoder_Integral;  
  130. 		if(1==Flag_Left)    	Movement=-Target_Velocity;	           //===前进标志位置1 
  131. 		else if(1==Flag_Right)	Movement=Target_Velocity;           //===后退标志位置1
  132.   	else  Movement=0;
  133.     //=============速度PI控制器=======================//	
  134. 		Encoder_Least=Mean_Filter_X(velocity);        //速度滤波  
  135. 		Encoder *= 0.7;		                                                //===一阶低通滤波器       
  136. 		Encoder += Encoder_Least*0.3;	                                    //===一阶低通滤波器    
  137.  		Encoder_Integral +=Encoder;                                       //===积分出位移 
  138. 		Encoder_Integral +=Movement;                                      //===接收遥控器数据，控制前进后退
  139. 		if(Encoder_Integral>150000)  	Encoder_Integral=150000;               //===积分限幅
  140. 		if(Encoder_Integral<-150000)	Encoder_Integral=-150000;              //===积分限幅	
  141. 	  if(Flag_Stop)   Encoder_Integral=0; //===电机关闭后清除积分
  142. 		Velocity=Encoder*Velocity_Kp/100+Encoder_Integral*Velocity_Ki/5000;        //===速度控制	
  143. 	  if(Flag_Stop)   Velocity=0;      //===电机关闭后清除积分
  144. 		Show_Data1=Encoder_Integral;
  145. 	  Show_Data3=Velocity;
  146. 		if(Velocity>1000)  	Velocity=1000;               //===速度环限幅
  147. 		if(Velocity<-1000)	  Velocity=-1000;              //===速度环限幅
  148. 	  return Velocity;
  149. }
  150. /**************************************************************************
  151. 函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity
  152. 入口参数：左轮速度、右轮速度
  153. 返回  值：速度控制PWM
  154. 作    者：平衡小车之家
  155. **************************************************************************/
  156. int velocity_Y(int velocity)
  157. {  
  158.     static float Velocity,Encoder_Least,Encoder,Movement;
  159. 	  static float Target_Velocity=2500;
  160. 	  static float Encoder_Integral;  
  161. 	  if(1==Flag_Qian)    	  Movement=Target_Velocity;	           //===前进标志位置1 
  162. 		else if(1==Flag_Hou)	  Movement=-Target_Velocity;           //===后退标志位置1
  163.   	else  Movement=0;
  164. //   //=============速度PI控制器=======================//	
  165. 		Encoder_Least=Mean_Filter_Y(velocity);          //速度滤波      
  166. 		Encoder *= 0.7;		                                                //===一阶低通滤波器       
  167. 		Encoder += Encoder_Least*0.3;	                                    //===一阶低通滤波器    
  168. 		Encoder_Integral +=Encoder;                                       //===积分出位移 
  169. 		Encoder_Integral +=Movement;                                  //===接收遥控器数据，控制前进后退
  170. 		if(Encoder_Integral>150000)  	Encoder_Integral=150000;            //===积分限幅
  171. 		if(Encoder_Integral<-150000)	Encoder_Integral=-150000;              //===积分限幅	
  172.     if(Flag_Stop)   Encoder_Integral=0;      //===电机关闭后清除积分
  173.   	Velocity=Encoder*Velocity_Kp/100+Encoder_Integral*Velocity_Ki/5000;      //===速度控制	
  174. 	  if(Flag_Stop)   Velocity=0;      //===电机关闭后清除积分
  175.    	Show_Data2=Encoder_Integral;
  176. 	  Show_Data4=Velocity;
  177. 	  if(Velocity>1000)  	Velocity=1000;               //===速度环限幅
  178. 		if(Velocity<-1000)	  Velocity=-1000;              //===速度环限幅
  179. 	  return Velocity;
  180. }
  181.  
  182. /**************************************************************************
  183. 函数功能：赋值给PWM寄存器
  184. 入口参数：PWM
  185. 返回  值：无
  186. **************************************************************************/
  187. void Set_Pwm(int motor_a,int motor_b,int motor_c)
  188. {
  189. 			int Final_Motor_A,Final_Motor_B,Final_Motor_C;
  190. 	   	if(motor_a>0)			    INA=0;   //电机A方向控制
  191. 			else 	             	  INA=1;
  192. 	   	if(motor_b>0)			    INB=0;   //电机B方向控制
  193. 			else 	             	  INB=1;
  194. 			if(motor_c>0)			    INC=0;   //电机C方向控制
  195. 			else 	                INC=1;
  196.  
  197. 			Final_Motor_A=Linear_Conversion(motor_a);  //线性化
  198.     	Final_Motor_B=Linear_Conversion(motor_b);
  199. 			Final_Motor_C=Linear_Conversion(motor_c);
  200. 			Set_PWM_Final(Final_Motor_A,Final_Motor_B,Final_Motor_C);  
  201. }
  202. /**************************************************************************
  203. 函数功能：对控制输出的PWM线性化,便于给系统寄存器赋值
  204. 入口参数：PWM
  205. 返回  值：线性化后的PWM
  206. **************************************************************************/
  207. u16  Linear_Conversion(int motor)
  208. { 
  209. 	 u32 temp;
  210.    u16 Linear_Moto;
  211.    temp=1000000/myabs(motor);   //1000000是经验值
  212. 	 if(temp>65535) Linear_Moto=65535;
  213. 	 else Linear_Moto=(u16)temp;
  214. 	 return Linear_Moto;
  215. }	
  216.  
  217. /**************************************************************************
  218. 函数功能：限制PWM赋值 
  219. 入口参数：幅值
  220. 返回  值：无
  221. **************************************************************************/
  222. void Xianfu_Pwm(int amplitude)
  223. {	
  224.     if(Motor_A<-amplitude) Motor_A=-amplitude;	
  225. 		if(Motor_A>amplitude)  Motor_A=amplitude;	
  226. 	  if(Motor_B<-amplitude) Motor_B=-amplitude;	
  227. 		if(Motor_B>amplitude)  Motor_B=amplitude;		
  228. 	  if(Motor_C<-amplitude) Motor_C=-amplitude;	
  229. 		if(Motor_C>amplitude)  Motor_C=amplitude;			
  230. }
  231.  
  232. /**************************************************************************
  233. 函数功能：按键修改小车运行状态 
  234. 入口参数：无
  235. 返回  值：无
  236. **************************************************************************/
  237. void Key(void)
  238. {	
  239. 	u8 tmp,tmp2;
  240. 	tmp=click_N_Double(75);    //读取按键信息
  241.   tmp2=Long_Press();
  242. 	if(tmp==1)Flag_Stop=!Flag_Stop;//单击开机/关闭电机   
  243. 	if(tmp==2)Flag_Zero=!Flag_Zero;//双击读取零点 
  244.  
  245. 	if(tmp2==1)Flag_Show=!Flag_Show;//长按切换显示模式 	
  246. }
  247.  
  248. /**************************************************************************
  249. 函数功能：异常关闭电机
  250. 入口参数：电压
  251. 返回  值：1：异常  0：正常
  252. **************************************************************************/
  253. u8 Turn_Off( int voltage)
  254. {
  255. 	    u8 temp;
  256. 			if(Flag_Stop==1||Pitch<-60||Pitch>60||Roll<-60||Roll>60)// 角度过大或者Flag_Stop置1则关闭并失能电机
  257. 			{	                                                
  258. 			temp=1;      
  259.       INA=0;
  260.       INB=0;
  261.       INC=0;				
  262. 			ST=0;   //失能电机
  263. 			Flag_Stop=1;
  264.       }
  265. 			else
  266. 			ST=1,	
  267.       temp=0;
  268.       return temp;			
  269. }
  270.  
  271. /**************************************************************************
  272. 函数功能：绝对值函数
  273. 入口参数：long int
  274. 返回  值：unsigned int
  275. **************************************************************************/
  276. u32 myabs(long int a)
  277. { 		   
  278. 	  u32 temp;
  279. 		if(a<0)  temp=-a;  
  280. 	  else temp=a;
  281. 	  return temp;
  282. }
  283. /**************************************************************************
  284. 函数功能：速度滤波
  285. 入口参数：速度
  286. 返回  值：滤波后的速度
  287. **************************************************************************/
  288. int Mean_Filter_X(int motor)
  289. {
  290.   u8 i;
  291.   s32 Sum_Speed = 0; 
  292. 	s16 Filter_Speed;
  293.   static  s16 Speed_Buf[FILTERING_TIMES]={0};
  294.   for(i = 1 ; i<FILTERING_TIMES; i++)//平滑数据
  295.   {
  296.     Speed_Buf[i - 1] = Speed_Buf[i];
  297.   }
  298.   Speed_Buf[FILTERING_TIMES - 1] =motor;
  299.  
  300.   for(i = 0 ; i < FILTERING_TIMES; i++)
  301.   {
  302.     Sum_Speed += Speed_Buf[i];
  303.   }
  304.   Filter_Speed = (s16)(Sum_Speed / FILTERING_TIMES);//取平均
  305. 	return Filter_Speed;
  306. }
  307. /**************************************************************************
  308. 函数功能：速度滤波
  309. 入口参数：速度
  310. 返回  值：滤波后的速度
  311. **************************************************************************/
  312. int Mean_Filter_Y(int motor)
  313. {
  314.   u8 i;
  315.   s32 Sum_Speed = 0; 
  316. 	s16 Filter_Speed;
  317.   static  s16 Speed_Buf[FILTERING_TIMES]={0};
  318.   for(i = 1 ; i<FILTERING_TIMES; i++)
  319.   {
  320.     Speed_Buf[i - 1] = Speed_Buf[i];
  321.   }
  322.   Speed_Buf[FILTERING_TIMES - 1] =motor;
  323.  
  324.   for(i = 0 ; i < FILTERING_TIMES; i++)
  325.   {
  326.     Sum_Speed += Speed_Buf[i];
  327.   }
  328.   Filter_Speed = (s16)(Sum_Speed / FILTERING_TIMES);//取平均
  329. 	return Filter_Speed;
  330. }

 

 

 

关于球上自平衡机器人X,Y方向的速度和万向轮速度的转换，注意万向轮智能沿着切面转动

 

 

我还想找到角度的转换关系，就是俯仰角和翻滚角对应三个万向轮的这个怎么分配的。这个

我看了下平衡小车之家的源码，好像分配是跟速度一样的，X方向的角度PID的输出和速度PID的输出加在一起，Y一样，再通过上面的关系式分配给三个电机的控制量。

可以看他control.c的代码

可以看到分配在Kinematic_Analysis函数中进行，输入Move X,Move Y,得到Target A ,Target B,Target C,也就是三个电机对应的控制量。而MoveX就是X方向的倾角和X方向的速度这两个环输出的和。像balancd_Pitch这个函数你去看它定义就是计算pitch角度偏差的PID输出。其他的一样。这些函数都在control.c里面。

我们可以看看Kinematic_Analysis函数的定义，就在control.c里面，那里面的公式就是上面哪个毕业论文截图的公式！！！！

  1. #include "control.h"	
  2. #include "filter.h"	
  3.   /**************************************************************************
  4. 作者：平衡小车之家
  5. 我的淘宝小店：http://shop114407458.taobao.com/
  6. **************************************************************************/
  7. u8 Flag_Target;                             //相关标志位
  8. float Voltage_Count,Voltage_All;  //电压采样相关变量
  9. int Balance_Pwm_X,Velocity_Pwm_X,Balance_Pwm_Y,Velocity_Pwm_Y;
  10. #define X_PARAMETER           (0.5f)               
  11. #define Y_PARAMETER           (sqrt(3)/2.f)      
  12. #define L_PARAMETER           (1.0f)  
  13. /**************************************************************************
  14. 函数功能：小车运动 逆运动学分析
  15. 入口参数：X Y Z 三轴速度或者位置
  16. 返回  值：无
  17. **************************************************************************/
  18. void Kinematic_Analysis(float Vx,float Vy,float Vz)
  19. {
  20. 	      Target_A   = Vx + L_PARAMETER*Vz;
  21.         Target_B   = -X_PARAMETER*Vx + Y_PARAMETER*Vy + L_PARAMETER*Vz;
  22. 	      Target_C   = -X_PARAMETER*Vx - Y_PARAMETER*Vy + L_PARAMETER*Vz;
  23. }
  24. /**************************************************************************
  25. 函数功能：小车运动 正运动学分析 注：实际注释掉的是理想的运动学分析，实际使用放大了3倍，对计算结果没影响，主要是避免舍去误差
  26. 入口参数：A B C三个电机的速度
  27. 返回  值：无
  28. **************************************************************************/
  29. void Forward_Kinematics(float Va,float Vb,float Vc)
  30. {
  31. //		Motor_X=(Va*2-Vb-Vc)/3;
  32. //		Motor_Y=(Vb-Vc)/2/Y_PARAMETER;
  33. //		Motor_Z=(Va+Vb+Vc)/3;
  34. 			Motor_X=Va*2-Vb-Vc;
  35. 		  Motor_Y=(Vb-Vc)*sqrt(3);
  36. 		  Motor_Z=Va+Vb+Vc;
  37. }
  38. /**************************************************************************
  39. 函数功能：所有的控制代码都在这里面
  40.          5ms定时中断由MPU6050的INT引脚触发
  41.          严格保证采样和数据处理的时间同步				 
  42. **************************************************************************/
  43. int EXTI15_10_IRQHandler(void) 
  44. {    
  45. 	 if(INT==0)		
  46. 	{     
  47. 		  EXTI->PR=1<<15;   //清除LINE5上的中断标志位  		
  48. 		  Flag_Target=!Flag_Target;
  49. 		  if(delay_flag==1)
  50. 			 {
  51. 				 if(++delay_50==10)	 delay_50=0,delay_flag=0;  //给主函数提供50ms的精准延时
  52. 			 }
  53. 		  if(Flag_Target==1)                     //5ms读取一次陀螺仪和加速度计的值
  54. 			{	 
  55. 				Read_DMP();                          //===更新姿态		
  56. 				Key();													     //扫描按键变化	
  57. 				if(Pitch_Bias>-0.8&&Pitch_Bias<0.8&&Roll_Bias>-0.8&&Roll_Bias<0.8)Led_Flash(0);     //接近平衡位置，常亮
  58.         else 		Led_Flash(100);  		     //===LED闪烁
  59. 				Voltage_All+=Get_battery_volt();     //多次采样累积
  60. 				if(++Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//求平均值 获取电池电压	
  61. 				return 0;	                                               
  62. 			}     //===10ms控制一次	
  63. 			 Read_DMP();    //===更新姿态		
  64. 			 Roll_Bias =Roll-Roll_Zero;		//获取Y方向的偏差
  65. 		   Pitch_Bias=Pitch-Pitch_Zero; //获取X方向的偏差		
  66. 			 Forward_Kinematics(Motor_A,Motor_B,Motor_C);  //正运动学分析，得到X Y Z 方向的速度
  67. 		   Balance_Pwm_X=balance_Pitch(Pitch_Bias,gyro[1]);   //X方向的倾角控制
  68. 			 Balance_Pwm_Y=-balance_Roll(Roll_Bias, gyro[0]);   //Y方向的倾角控制
  69. 			 Velocity_Pwm_X=velocity_X(Motor_X);      //X方向的速度控制
  70. 			 Velocity_Pwm_Y=velocity_Y(Motor_Y);  	  //Y方向的速度控制                  //maxi:原来在这里
  71. 			 Move_X =Balance_Pwm_X+Velocity_Pwm_X;    //===X方向控制量累加				
  72. 			 Move_Y =Balance_Pwm_Y+Velocity_Pwm_Y;    //===Y方向控制量累加	
  73. 			 Move_Z=0;  //Z轴不做控制
  74. 				Kinematic_Analysis(Move_X,Move_Y,Move_Z);//逆运动学分析，得到A B C电机控制量
  75. 				Motor_A=Target_A;//直接调节PWM频率
  76. 				Motor_B=Target_B;//直接调节PWM频率
  77. 				Motor_C=Target_C;//直接调节PWM频率	 			 		 
  78. 				Xianfu_Pwm(1300);//===PWM频率限幅  因为频率太大之后扭矩减小不再满足小车需求
  79. 			 if(Turn_Off(Voltage)==0)	Set_Pwm(Motor_A,Motor_B,Motor_C);    //赋值给PWM寄存器
  80. 			 if(Flag_Zero)   //这是人为更新零点值
  81. 			 {
  82. 				 Roll_Zero=Roll; //ROLL更新
  83. 				 Pitch_Zero=Pitch; //Pitch更新
  84. 				 Flag_Zero=0;  //清零过程仅执行一次 等待下一次指令
  85. 			 }
  86.  }
  87. 	 return 0;	 
  88. }

 

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

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

  

闽ICP备14008679号