设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › 51单片机 › 基于STC8A8K单片机的PID舵机小车控制程序源码
返回列表 发新帖
查看: 6196|回复: 4
收起左侧

基于STC8A8K单片机的PID舵机小车控制程序源码

  [复制链接]
ID:375898 发表于 2018-7-20 15:06 | 显示全部楼层 |阅读模式
自己为电赛写的 没用到~

单片机源程序如下:
  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  //舵机引脚
  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;             //左右编码器的脉冲计数
  14. float Velocity,Velocity_Set,Angle,Angle_Set;
  15. unsigned long count0,count1,count2,count3,Sensor;//编码器脉冲计数
  16. int Motor_A,Motor_B,Servo,Target_A,Target_B;  //电机舵机控制相关   
  17. /**************************************************************************
  18. 函数功能：绝对值函数
  19. 入口参数：int
  20. 返回  值：unsigned int
  21. **************************************************************************/
  22. int myabs(int a)//绝对值函数
  23. {                    
  24.           int temp;
  25.                 if(a<0)  temp=-a;  
  26.           else temp=a;
  27.           return temp;
  28. }
  29. /**************************************************************************
  30. 函数功能：电机舵机占空比
  31. **************************************************************************/
  32. void LmotorPWM(u8 D0,u8 D1)//左电机PWM占空比
  33. {
  34.   P_SW2=0X80;
  35.         PWMCKS=0X09;//PWM时钟为系统时钟
  36.         PWMC=0XFD3F;//设置PWM周期为64831个脉冲,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)//右电机PWM占空比
  45. {
  46.   P_SW2=0X80;
  47.         PWMCKS=0X0E;//PWM时钟为系统时钟
  48.         PWMC=0XFD3F;//设置PWM周期为12C0H个脉冲,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)//舵机PWM占空比
  57. {
  58.   P_SW2=0X80;
  59.         PWMCKS=0X0E;//PWM时钟为系统时钟
  60.         PWMC=0XFD3F;//设置PWM周期为12C0H个脉冲,T=200US
  61.         PWM4T2=0x0380;//在0.5MS时赋值高电平
  62.         PWM4T1=0x0380+17.7*D4;
  63.         PWM4CR=0X80;//使能PWM2输出，且初始电平为低电平
  64.         P_SW2=0X00;
  65.         PWMCR=0X80;
  66. }
  67. /**************************************************************************
  68. 函数功能：小车运动学分析
  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;                    //舵机转向   
  75. }
  76. /**************************************************************************
  77. 函数功能：占空比设置
  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. 函数功能：电机PI控制
  91. **************************************************************************/
  92. int PI_A (int Encoder,int Target)
  93. {         
  94.          static int Bias,Pwm,Last_bias;
  95.          Bias=Target-Encoder;                //计算偏差
  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;                //计算偏差
  104.          Pwm+=0.015*(Bias-Last_bias)+0.015*Bias;   //增量式PI控制器
  105.          Last_bias=Bias;                           //保存上一次偏差
  106.          return Pwm;                         //增量输出
  107. }
  108. /**************************************************************************
  109. 函数功能：电机占空比限制幅度
  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;          //舵机限幅
  119.                 if(Servo>(120))    Servo=120;                  //舵机限幅
  120. }
  121. /**************************************************************************
  122. 函数功能：计算偏差角度
  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. 函数功能：编码器计数
  135. **************************************************************************/
  136. void PCA_C()
  137. {
  138. count0=0;
  139. count1=0;
  140. CCON=0X00;
  141. CMOD=0X09;//设定为系统时钟，使能PCA溢出中断
  142. CL=0X00;
  143. CH=0X00;
  144. CCAPM0=0X21;//设置PCA0模块为16位捕获模式，且为上升沿捕获
  145. CCAPM1=0X21;//设置PCA1模块为16位捕获模式，且为上升沿捕获
  146. CCAPM2=0X21;//设置PCA2模块为16位捕获模式，且为上升沿捕获
  147. CCAPM3=0X21;//设置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;//启动PCA
  157. EA=1;
  158. }
  159. /**************************************************************************
  160. 函数功能：电机运动控制算法
  161. **************************************************************************/
  162. void control(void)  
  163. {   
  164.           Encoder_Left=count0;        //===读取编码器的值                 //为了保证M法测速的时间基准，首先读取编码器数据
  165.                 Encoder_Right=count1;       //===读取编码器的值
  166.                 Get_RC();
  167.                 Analysis(Velocity,Angle);                     //小车运动学分析   
  168.                 Motor_A=PI_A(Encoder_Left,Target_A);     //===速度闭环控制计算电机A最终PWM
  169.                 Motor_B=PI_B(Encoder_Right,Target_B);    //===速度闭环控制计算电机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. }
复制代码

所有资料51hei提供下载:
STC8电机控制.zip (71.69 KB, 下载次数: 116)

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励！

查看全部评分

回复

使用道具 举报

ID:487103 发表于 2019-4-22 00:23 | 显示全部楼层
很好的资料，加油
回复

使用道具 举报

ID:587285 发表于 2019-7-20 11:20 | 显示全部楼层
51小萌新z 发表于 2019-4-22 00:23
很好的资料，加油

输出口是哪个啊
回复

使用道具 举报

ID:587285 发表于 2019-7-20 11:21 | 显示全部楼层
该怎么接单片机啊，求大佬解释
回复

使用道具 举报

ID:462629 发表于 2021-12-28 10:00 | 显示全部楼层
能补全电路原理图,源码吗
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表