51舵机控制程序
#include //包括一个52 标准内核的头文件
#include
/*****************定义数据类型（方便移植）*****************/
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
p=KServoP[0]; /*分*/
else if (Offset>=-4&&Offset
p=KServoP[1]; /*P*/
else if (Offset>=-1&&Offset
p=KServoP[2]; /*数*/
else if (Offset>=2&&Offset
p=KServoP[3];
else if (Offset>=5&&Offset
p=KServoP[4]; /***********************/
d=KServoD;
PID=p*Offset+d*(Offset-LastOffset[0]); //PID 位置型算式
SevorPWM=SERVOMID+PID; //舵机输出量 if(SevorPWM>SERVOMOSTLEFT)SevorPWM=SERVOMOSTLEFT; //限幅 if(SevorPWM
/*********************舵机测试函数*************************************/ 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; }
}