51舵机控制程序

51舵机控制程序 #include <reg52.h> //包括一个52 标准 excel标准偏差excel标准偏差函数exl标准差函数国标检验抽样标准表免费下载红头文件格式标准下载 内核的头文件 #include <math.h> /*****************定义数据类型(方便移植) *****************/ typedef unsigned char UINT8; typedef char SINT8; typedef unsigned int UINT16; typedef int SINT16; unsigned char flag=0; /**********************************************************/ /*****************定义布尔值(增强程序可读性) *************/ #define HIGH_LEVEL 1 #define LOW_LEVEL 0 /********************************************************** */ /**********定义相关常数(移植于不同的小车时，需根据实际情况改变该常数***********/ #define SERVOMID 1400 //舵机中值 ——————————————————————————————————————————————— #define SERVOMOSTLEFT 1600 //舵机左值 #define SERVOMOSTRIGHT 1200 //舵机右值 #define SERVO_PERIOD 20000 //舵机PWM 周期:20ms，晶振12M 公式:计数值=定时时间*晶振 //频率/12，如20ms: 计数值=0.02 s * 12 000 000 Hz/12 = 20000 #define MOTOR_PERIOD 1000 //电机PWM 周期:1ms，晶振12M /********************************************************** ****/ /*****************输出口定义*************************/ sbit SevorPort = P2^7; sbit MotorPort = P2^5; sbit l_1=P1^0; sbit l_2=P1^1; sbit l_3=P1^2; sbit cen=P1^3; sbit r_1=P1^6; sbit r_2=P1^5; sbit r_3=P1^4; sbit M1_1=P2^0; sbit M1_2=P2^1; sbit M2_1=P2^2; ——————————————————————————————————————————————— sbit M2_2=P2^3; /*******************************************************/ //*****************定义公共变量 **************************/ UINT8 KServoD=10; //舵机D 参数 UINT8 KServoP[5]={65,40,10,40,65}; //舵机分段P 参数 UINT8 SampleData=0; //采样数据 SINT8 Offset=0; //当前赛道位置 SINT8 LastOffset[2]={0}; //上一次赛道位置 UINT16 SevorPWM=0; //舵机PWM 高电平时间 UINT16 MotorPWM=0; //电机PWM 高电平时间 /*******************************************************/ /********************初始化函数 *************************/ void Init(void) { TMOD=0x11; //定时器0，16 位工作方式;定时器1，16 位工作方式 TR0=1; //启动定时器0 TR1=1; //启动定时器1 ET0=1; //打开定时器0 中断 ET1=1; //打开定时器0 中断 EA=1; //打开总中断 ——————————————————————————————————————————————— } //开启中断和定时器 /********************************************************** ****/ void run() //小车直走 { SevorPWM=SERVOMID; //测试舵机中值 } void left(int l) //小车左转 { //SevorPWM=SERVOMOSTLEFT; //测试舵机极左值 SevorPWM=l; } void right(int r) //小车右转 { //SevorPWM=SERVOMOSTRIGHT; //测试舵机极右值 SevorPWM=r; } /********************数据采样函数 ************************/ void scan() { ——————————————————————————————————————————————— if(cen==0) //中 { flag=0; } else if(r_1==0) { flag=1; } else if(r_2==0) { flag=2; //右2 P15 //右1 P16 } else if(r_3==0) { flag=3; //右3 P14 } else if(l_1==0) // 左1 P10 { flag=4; } else if(l_2==0) //左2 P11 { flag=5; } else if(l_3==0) //左3 P12 { } flag=6; switch(flag) { case 0: {run();break;} case 1: {right(1150);break;} case 2: {right(1250);break;} case 3: {right(1300);break;} case 4: {left(1600);break;} case 5: {left(1500);break;} case 6: {left(1450);break;} } } ——————————————————————————————————————————————— /********************************************************** */ /***********赛道位置数据滤波(减少某次采样错误对系统的干 扰)****************/ void Filter(void) { if (Offset==8) //滤除错误信号，没有检测到黑线(是需要保持上 一次测量值的) { Offset=LastOffset[0]; } else if(abs(Offset-LastOffset[0])>5) //滤除尖峰信号 { Offset=LastOffset[0]; } } /**********************舵机控制函数 ******************************************/ void ServoCtrl(void) { UINT8 p=0,d=0; SINT16 PID=0; if(Offset>=-7&&Offset<-4) /***********************/ ——————————————————————————————————————————————— p=KServoP[0]; /*分*/ else if (Offset>=-4&&Offset<-1) /*段*/ p=KServoP[1]; /*P*/ else if (Offset>=-1&&Offset<2) /*参*/ p=KServoP[2]; /*数*/ else if (Offset>=2&&Offset<5) p=KServoP[3]; else if (Offset>=5&&Offset<=7) p=KServoP[4]; /***********************/ d=KServoD; PID=p*Offset+d*(Offset-LastOffset[0]); //PID 位置型算式 SevorPWM=SERVOMID+PID; //舵机输出量 if(SevorPWM>SERVOMOSTLEFT)SevorPWM=SERVOMOSTLEFT; //限幅 if(SevorPWM<SERVOMOSTRIGHT)SevorPWM=SERVOMOSTRIGHT; //限幅 } /*********************舵机测试函数 ——————————————————————————————————————————————— *************************************/ ServoTest() { // SevorPWM=SERVOMID; //测试舵机中值 SevorPWM=SERVOMOSTLEFT; //测试舵机极左值 // SevorPWM=SERVOMOSTRIGHT; //测试舵机极右值 } /********************速度控制函数 *************************************/ void MotorCtrl(void) { M2_1=0; M2_2=1; MotorPWM=500; } /*********************数据存储函数 *******************************************/ void SaveData(void) { static UINT8 TCnt2=0; if(TCnt2==20) { LastOffset[1]=LastOffset[0]; TCnt2=0; ——————————————————————————————————————————————— } TCnt2++; LastOffset[0]=Offset; } /**********************主函数 *******************************/ void main(void) { Init(); //初始化 while (1) { scan(); //ServoCtrl(); //舵机控制 //ServoTest(); //舵机测试 MotorCtrl(); //电机控制 SaveData(); } } /**********************舵 机 定 时 器 中 断 函 数******************************/ void TC0Isr(void) interrupt 1 { ——————————————————————————————————————————————— static UINT8 TC20msFlag=LOW_LEVEL; //舵机输出状态标志 UINT16 LowlvlTime=0; UINT16 HighlvlTime=0; if(TC20msFlag==HIGH_LEVEL) { LowlvlTime=65535-(SERVO_PERIOD-SevorPWM); TH0=LowlvlTime/256; TL0=LowlvlTime%256; SevorPort=LOW_LEVEL; TC20msFlag=LOW_LEVEL; } else { HighlvlTime=65535-SevorPWM; TH0=HighlvlTime/256; TL0=HighlvlTime%256; SevorPort=HIGH_LEVEL; TC20msFlag=HIGH_LEVEL; } } /****************************电机定时器 ********************************/ ——————————————————————————————————————————————— void TC1Isr(void) interrupt 3 { static UINT8 TC10msFlag=LOW_LEVEL; //电机输出状态标志 UINT16 LowlvlTime=0; UINT16 HighlvlTime=0; if(TC10msFlag==HIGH_LEVEL) { LowlvlTime=65535-(MOTOR_PERIOD-MotorPWM); TH1=LowlvlTime/256; TL1=LowlvlTime%256; MotorPort=LOW_LEVEL; TC10msFlag=LOW_LEVEL; } else { HighlvlTime=65535-MotorPWM; TH1=HighlvlTime/256; TL1=HighlvlTime%256; MotorPort=HIGH_LEVEL; 断函数中 TC10msFlag=HIGH_LEVEL; } } ———————————————————————————————————————————————