標(biāo)題: 基于STC8A8K單片機(jī)的PID舵機(jī)小車控制程序源碼 [打印本頁]
作者: 時(shí)樂    時(shí)間: 2018-7-20 15:06
標(biāo)題: 基于STC8A8K單片機(jī)的PID舵機(jī)小車控制程序源碼
自己為電賽寫的 沒用到~

單片機(jī)源程序如下:
  1. #include<stc8.h>
  2. #include<math.h>
  3. #define PWMA1   P20
  4. #define PWMA2   P21
  5. #define PWMB1   P22
  6. #define PWMB2   P23
  7. #define SERVO   P24  //舵機(jī)引腳
  8. #define u8 unsigned char
  9. #define T 0.156f
  10. #define L 0.1445f
  11. #define K 3.114f
  12. #define PI 3.14159265
  13. int Encoder_Left,Encoder_Right;             //左右編碼器的脈沖計(jì)數(shù)
  14. float Velocity,Velocity_Set,Angle,Angle_Set;
  15. unsigned long count0,count1,count2,count3,Sensor;//編碼器脈沖計(jì)數(shù)
  16. int Motor_A,Motor_B,Servo,Target_A,Target_B;  //電機(jī)舵機(jī)控制相關(guān)   
  17. /**************************************************************************
  18. 函數(shù)功能：絕對(duì)值函數(shù)
  19. 入口參數(shù)：int
  20. 返回  值：unsigned int
  21. **************************************************************************/
  22. int myabs(int a)//絕對(duì)值函數(shù)
  23. {                    
  24.           int temp;
  25.                 if(a<0)  temp=-a;  
  26.           else temp=a;
  27.           return temp;
  28. }
  29. /**************************************************************************
  30. 函數(shù)功能：電機(jī)舵機(jī)占空比
  31. **************************************************************************/
  32. void LmotorPWM(u8 D0,u8 D1)//左電機(jī)PWM占空比
  33. {
  34.   P_SW2=0X80;
  35.         PWMCKS=0X09;//PWM時(shí)鐘為系統(tǒng)時(shí)鐘
  36.         PWMC=0XFD3F;//設(shè)置PWM周期為64831個(gè)脈沖,T=20MS,
  37.         PWM0T1=D0*648;
  38.         PWM1T1=D1*648;
  39.         PWM0CR=0X80;//使能PWM0輸出，且初始電平為低電平
  40.         PWM1CR=0X80;//使能PWM1輸出，且初始電平為低電平
  41.         P_SW2=0X00;
  42.         PWMCR=0X80;
  43. }
  44. void RmotorPWM(u8 D2,u8 D3)//右電機(jī)PWM占空比
  45. {
  46.   P_SW2=0X80;
  47.         PWMCKS=0X0E;//PWM時(shí)鐘為系統(tǒng)時(shí)鐘
  48.         PWMC=0XFD3F;//設(shè)置PWM周期為12C0H個(gè)脈沖,T=200US
  49.         PWM2T1=D2*648;
  50.         PWM3T1=D3*648;
  51.         PWM2CR=0X80;//使能PWM2輸出，且初始電平為低電平
  52.         PWM3CR=0X80;//使能PWM3輸出，且初始電平為低電平
  53.         P_SW2=0X00;
  54.         PWMCR=0X80;
  55. }
  56. void DJPWM(u8 D4)//舵機(jī)PWM占空比
  57. {
  58.   P_SW2=0X80;
  59.         PWMCKS=0X0E;//PWM時(shí)鐘為系統(tǒng)時(shí)鐘
  60.         PWMC=0XFD3F;//設(shè)置PWM周期為12C0H個(gè)脈沖,T=200US
  61.         PWM4T2=0x0380;//在0.5MS時(shí)賦值高電平
  62.         PWM4T1=0x0380+17.7*D4;
  63.         PWM4CR=0X80;//使能PWM2輸出，且初始電平為低電平
  64.         P_SW2=0X00;
  65.         PWMCR=0X80;
  66. }
  67. /**************************************************************************
  68. 函數(shù)功能：小車運(yùn)動(dòng)學(xué)分析
  69. **************************************************************************/
  70. void Analysis(float velocity,float angle)
  71. {
  72.                 Target_A=velocity*(1+T*tan(angle)/2/L);
  73.                 Target_B=velocity*(1-T*tan(angle)/2/L);      //后輪差速
  74.                 Servo=9+angle*K;                    //舵機(jī)轉(zhuǎn)向   
  75. }
  76. /**************************************************************************
  77. 函數(shù)功能：占空比設(shè)置
  78. **************************************************************************/
  79. void Set_Pwm(int motor_a,int motor_b,int servo)
  80. {

  82.             if(motor_a<0)                        PWMA1=90,PWMA2=90+motor_a;
  83.                         else                     PWMA2=90,PWMA1=90-motor_a;
  84.                
  85.                   if(motor_b<0)                        PWMB1=90,PWMB2=90+motor_b;
  86.                         else                     PWMB2=90,PWMB1=90-motor_b;
  87.      SERVO=servo;        
  88. }
  89. /**************************************************************************
  90. 函數(shù)功能：電機(jī)PI控制
  91. **************************************************************************/
  92. int PI_A (int Encoder,int Target)
  93. {         
  94.          static int Bias,Pwm,Last_bias;
  95.          Bias=Target-Encoder;                //計(jì)算偏差
  96.          Pwm+=0.015*(Bias-Last_bias)+0.015*Bias;   //增量式PI控制器
  97.          Last_bias=Bias;                           //保存上一次偏差
  98.          return Pwm;                         //增量輸出
  99. }
  100. int PI_B (int Encoder,int Target)
  101. {         
  102.          static int Bias,Pwm,Last_bias;
  103.          Bias=Target-Encoder;                //計(jì)算偏差
  104.          Pwm+=0.015*(Bias-Last_bias)+0.015*Bias;   //增量式PI控制器
  105.          Last_bias=Bias;                           //保存上一次偏差
  106.          return Pwm;                         //增量輸出
  107. }
  108. /**************************************************************************
  109. 函數(shù)功能：電機(jī)占空比限制幅度
  110. **************************************************************************/
  111. void Xianfu_Pwm(void)
  112. {        
  113.           int Amplitude=90;    //===PWM滿幅是90 限制在90
  114.     if(Motor_A<-Amplitude) Motor_A=-Amplitude;        
  115.                 if(Motor_A>Amplitude)  Motor_A=Amplitude;        
  116.           if(Motor_B<-Amplitude) Motor_B=-Amplitude;        
  117.                 if(Motor_B>Amplitude)  Motor_B=Amplitude;               
  118.                 if(Servo<(0))     Servo=0;          //舵機(jī)限幅
  119.                 if(Servo>(120))    Servo=120;                  //舵機(jī)限幅
  120. }
  121. /**************************************************************************
  122. 函數(shù)功能：計(jì)算偏差角度
  123. **************************************************************************/
  124. void Get_RC(void)
  125. {
  126.         static float Bias,Last_Bias;
  127.         Velocity=45;          //電磁巡線模式下的速度
  128.         Bias=45-Sensor;  //提取偏差
  129.         Angle=myabs(Bias)*Bias*0.0002+Bias*0.001+(Bias-Last_Bias)*0.05; //PI控制
  130.         Last_Bias=Bias;   //上一次的偏差
  131.         
  132. }
  133. /**************************************************************************
  134. 函數(shù)功能：編碼器計(jì)數(shù)
  135. **************************************************************************/
  136. void PCA_C()
  137. {
  138. count0=0;
  139. count1=0;
  140. CCON=0X00;
  141. CMOD=0X09;//設(shè)定為系統(tǒng)時(shí)鐘，使能PCA溢出中斷
  142. CL=0X00;
  143. CH=0X00;
  144. CCAPM0=0X21;//設(shè)置PCA0模塊為16位捕獲模式，且為上升沿捕獲
  145. CCAPM1=0X21;//設(shè)置PCA1模塊為16位捕獲模式，且為上升沿捕獲
  146. CCAPM2=0X21;//設(shè)置PCA2模塊為16位捕獲模式，且為上升沿捕獲
  147. CCAPM3=0X21;//設(shè)置PCA3模塊為16位捕獲模式，且為上升沿捕獲
  148. CCAP0L=0X00;
  149. CCAP0H=0X00;
  150. CCAP1L=0X00;
  151. CCAP1H=0X00;
  152. CCAP2L=0X00;
  153. CCAP2H=0X00;
  154. CCAP3L=0X00;
  155. CCAP3H=0X00;
  156. CR=1;//啟動(dòng)PCA
  157. EA=1;
  158. }
  159. /**************************************************************************
  160. 函數(shù)功能：電機(jī)運(yùn)動(dòng)控制算法
  161. **************************************************************************/
  162. void control(void)  
  163. {   
  164.           Encoder_Left=count0;        //===讀取編碼器的值                 //為了保證M法測(cè)速的時(shí)間基準(zhǔn)，首先讀取編碼器數(shù)據(jù)
  165.                 Encoder_Right=count1;       //===讀取編碼器的值
  166.                 Get_RC();
  167.                 Analysis(Velocity,Angle);                     //小車運(yùn)動(dòng)學(xué)分析   
  168.                 Motor_A=PI_A(Encoder_Left,Target_A);     //===速度閉環(huán)控制計(jì)算電機(jī)A最終PWM
  169.                 Motor_B=PI_B(Encoder_Right,Target_B);    //===速度閉環(huán)控制計(jì)算電機(jī)B最終PWM
  170.                 Xianfu_Pwm();            //===PWM限幅
  171.                 Set_Pwm(Motor_A,Motor_B,Servo);    //===賦值給PWM寄存器           
  172. }

  174. void main()
  175. {
  176.                 while(1)
  177.         {
  178.                 control();
  179.                 LmotorPWM(PWMA1,PWMA2);
  180.                 RmotorPWM(PWMB1,PWMB2);
  181.                 DJPWM(Servo);//0-120度
  182.                 PCA_c();
  183.         }
  184. }
  185. void PCA() interrupt 7 using 1//編碼器中斷
  186. {
  187. if(CCF0)
  188. {
  189.         CCF0=0;
  190.         count0++;        
  191. }
  192. if(CCF1)
  193. {
  194.         CCF1=0;
  195.         count1++;
  196. }
  197. if(CCF2)
  198. {
  199.         CCF2=0;
  200.         count1++;
  201. }
  202. if(CCF3)
  203. {
  204.         CCF3=0;
  205.         count1++;
  206. }
  207. }
復(fù)制代碼

所有資料51hei提供下載:
STC8電機(jī)控制.zip (71.69 KB, 下載次數(shù): 117)


作者: 51小萌新z    時(shí)間: 2019-4-22 00:23
很好的資料，加油
作者: 單片機(jī)小白368    時(shí)間: 2019-7-20 11:20
51小萌新z 發(fā)表于 2019-4-22 00:23
很好的資料，加油

輸出口是哪個(gè)啊
作者: 單片機(jī)小白368    時(shí)間: 2019-7-20 11:21
該怎么接單片機(jī)啊，求大佬解釋
作者: woyaodwn    時(shí)間: 2021-12-28 10:00
能補(bǔ)全電路原理圖,源碼嗎



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