51单片机超高精度6路舵机控制程序

之前写了一个51单片机的6路舵机程序，其原理很简单，舵机需要一个约20ms的脉冲，其中高电平0.5-2.5ms，其余为低电平。这样我们可以用一个定时器，每0.1ms中断，各用一个变量来记录每个舵机的计数，这样就用软件方式实现了6个定时器。

这样的程序一般也够用了，不过，如果要用来驱动改造的连续旋转舵机的话就困难了。如果像刚才那样的程序，精度是0.1ms（100μs），将舵机的角度平分成20个角度，每次调节舵机就转了9度。一般的舵机死区都在10μs以下，应该可以0.9度地调节。

一开始我理所当然的将原来的程序，由0.1ms中断改为0.01ms中断，经过多次调整测试都不行，后来将STC C52的CPU换成STC12的CPU，发现可以控制1个舵机了，但后来换成2个舵机又不行了。正当百思不得其解的时候，忽然醒悟，0.01ms就是10μs，10μs才几个CPU时钟周期，根本不可能做那么多的运算，这样写的效率很低。

正当我打算放弃51单片机的时候，忽然想到一个算法。将20ms的脉冲分成2部分，10ms和10ms（后来改成17ms和3ms），第一段时间负责计算，将接下来6个舵机分别要在什么时候中断计算好，存放在变量里，第二段时间开始的时候，将全部舵机输出高电平，然后依次中断，将舵机置低电平，然后循环。（后来又改成主程序定时排序，定时器中断只负责根据数组置高低电平），这样精度就可以达到计时器级别（12M的晶振是1μs），但这里我们只需要10μs即可。

这个算法的关键是要对数组进行排序。一开始程序写好了，但总是会在某些点抖动，总是找不到原因。后来尝试将排序算法由“快速排序法”换成其他的排序法（插入排序法，选择排序法，冒泡排序法），发现各有不同的表现，最后发现冒泡排序法最稳定，比较适合。

附上上下位程序代码，供网友参考。单片机程序加了一个路由是否启动的检测，这样就不用先启动路由，然后再打开单片机了。由于单片机需要一个1-200的数来控制舵机，很多都是不可见字符，所以上位机就干脆省略了控制字符设置，直接写在代码里。其实下位机程序效率还是稍微有点不够，STC C52还是偶尔会卡顿，STC12系列就比较流畅，但我也不知道怎样再提高效率了。

编译好的程序：http://bbs.igee.cn/read.php?tid=5649

http://download.csdn.net/detail/channel_z/4074607

 

2012-1-14更新：

经一位网友指点，找到一个更好的算法，将20ms分成8个2.5ms，轮流对8个舵机输出PWM，这样效率很高，STC89和12都能用，而且还学到了一招，强推挽输出，不用上拉电阻也可以推动舵机了，不过STC12才有强推挽输出。

2012-2-11更新：

STC12的强推挽输出还真的是鸡肋啊，驱动能力还是太弱，连续改变脉冲就反应不过来，一开始还没找到原因，后来换另外那个焊了电阻的就没问题了，建议大家还是老老实实焊个电阻吧。

更新后的下位机代码如下：

 

/*预处理命令*/

#include  //包含单片机寄存器的头文件

#define uchar unsigned char

#define uint unsigned int

P0M1=0X00;

P0M0=0XFF;//设置P0 为强推挽输出

 

sbit servo0=P0^0;

sbit servo1=P0^1;

sbit servo2=P0^2;

sbit servo3=P0^3;

sbit servo4=P0^4;

sbit servo5=P0^5;

sbit servo6=P0^6;

sbit servo7=P0^7;

 

uchar serVal[2];

uint pwm[]={1382,1382,1382,1382,1382,1382,1382,1382}; //初始90度,（实际是1382.4，取整得1382）

uchar pwm_flag=0;

uint code ms0_5Con=461; //0.5ms计数 （实际是460.8，取整得461）

uint code ms2_5Con=2304; //2.5ms计数

 

/********************************************************************

* 功能 : 串口初始化，晶振11.0592,波特率9600，使能了串口中断

***********************************************************************/

void Com_Init()

{

TMOD |= 0x20;   //用定时器设置串口波特率

TH1=0xFD;   //256-11059200/(32*12*9600)=253 (FD)

TL1=0xFD;//同上

TR1=1;//定时器1开关打开

REN=1;          //开启允许串行接收位

SM0=0;//串口方式，8位数据

SM1=1;//同上

EA=1;           //开启总中断

ES=1;  //串行口中断允许位

}

/********************************************************************

* 功能 : 舵机PWM中断初始化

***********************************************************************/

void Timer0Init()

{

//0度=0.5ms, 45度=1ms, 90度=1.5ms, 135度=2ms, 180度=2.5ms

//2.5 ms初始值 F700, (12n/11059200=2.5/1000, n=2304, X=65536-2304=63232 > F700)

TMOD |= 0x01;  //使用模式1，16位定时器，使用”|”符号可以在使用多个定时器时不受影响

TH0=-ms2_5Con>>8;      //给定初值，17ms中断

TL0=-ms2_5Con;

EA=1;            //总中断打开

ET0=1;           //定时器0中断打开

TR0=1;           //定时器0开关打开

}

 

/********************************************************************

* 功能 : 舵机PWM中断, //舵机控制函数 周期为20ms 一个循环20MS  = 8*2.5ms

***********************************************************************/

void SteeringGear() interrupt 1

{

switch(pwm_flag)

{

case 1:  servo0=1; TH0=-pwm[0]>>8; TL0=-pwm[0]; break;

case 2:  servo0=0; TH0=-(ms2_5Con-pwm[0])>>8; TL0=-(ms2_5Con-pwm[0]); break;

case 3:  servo1=1; TH0=-pwm[1]>>8; TL0=-pwm[1]; break;

case 4:  servo1=0; TH0=-(ms2_5Con-pwm[1])>>8; TL0=-(ms2_5Con-pwm[1]); break;

case 5:  servo2=1; TH0=-pwm[2]>>8; TL0=-pwm[2]; break;

case 6:  servo2=0; TH0=-(ms2_5Con-pwm[2])>>8; TL0=-(ms2_5Con-pwm[2]); break;

case 7:  servo3=1; TH0=-pwm[3]>>8; TL0=-pwm[3]; break;

case 8:  servo3=0; TH0=-(ms2_5Con-pwm[3])>>8; TL0=-(ms2_5Con-pwm[3]); break;

case 9:  servo4=1; TH0=-pwm[4]>>8; TL0=-pwm[4]; break;

case 10: servo4=0; TH0=-(ms2_5Con-pwm[4])>>8; TL0=-(ms2_5Con-pwm[4]); break;

case 11: servo5=1; TH0=-pwm[5]>>8; TL0=-pwm[5]; break;

case 12: servo5=0; TH0=-(ms2_5Con-pwm[5])>>8; TL0=-(ms2_5Con-pwm[5]); break;

case 13: servo6=1;TH0=-pwm[6]>>8; TL0=-pwm[6]; break;

case 14: servo6=0;TH0=-(ms2_5Con-pwm[6])>>8; TL0=-(ms2_5Con-pwm[6]); break;

case 15: servo7=1;TH0=-pwm[7]>>8;  TL0=-pwm[7]; break;

case 16: servo7=0;TH0=-(ms2_5Con-pwm[7])>>8; TL0=-(ms2_5Con-pwm[7]); break;

default: TH0=0xff; TL0=0x80; pwm_flag=0;

}

pwm_flag++;

}

 

void SetSteeringGear(uchar i, uchar val)

{

uint a = (val+46)*10;

if(ams2_5Con)

a=ms2_5Con;

pwm[i]=a;

serVal[0]=255; //清除缓存

}

 

void SteeringGearUp(uchar i)

{

if(pwm[i]>ms0_5Con)

pwm[i]=pwm[i]-10;

}

 

void SteeringGearDown(uchar i)

{

if(pwm[i]