[原创]mc9s12dg128b控制舵机程序
0赞前一段时间跟同学们一起写了一个mc9s12dg128b控制舵机程序，今天乘着这个热乎劲，发出来，与大家一起共勉！！！
#include <hidef.h> /* common defines and macros */
#include <mc9s12dg128.h> /* derivative information //引用头文件，通过该头文件将单片机的各寄存器转换为相对应的C语言的变量名
*/
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
#define mid_direction 147 //设置舵机中间位置的占空比
#define right_direction 0 //设置直线路程时的相对中间位置的变化量
#define micro_direction 12 //设置转小弯时舵机相对中间位置的变化量
#define deep_direction 19 //设置转大弯时舵机相对中间位置的变化量
#define emergence_direction 22 //设置紧急情况时舵机转向
#define direct_speed 14 //设置直线时输出到电机的占空比从而控制电机转速
#define micro_speed 13 //设置小弯时电机转速
#define deep_speed 11 //设置大弯时电机转速
#define emergence_speed 10 //设置紧急情况时电机转速
#define inter 5
#define inter1 500 //通过设置该参数数值来调节每次采集轮速传感的脉冲时间
#define speed_keep 0.4 //为轮速的比例因子，因使用效果不好最后取消使用
#define speed_kd 0.1
#define kp 0.5
//#define kd 1.3 //下列的全局变量的设定，是为了在小车冲出赛道后仍能记住返回而设
int turn_symbol; //转向标志，在信号分析程序中设置1时为向右转为-1时向左转
float memory_direction; //记忆上次转向
unsigned char signal;
int error; //定义转向本次与中间位置的偏差
int last_error; //定义上次转向与中间位置的偏差
int mem_error; //记忆上次转向偏差
int speed;
int speed1;
float kd;
float speed_error;
float speed_last_error;
int speed_back; //轮速反回脉冲数
int stop_symbol; // 停车标志
int flag;
void PLL_init(void) // 系统时钟的初始化，因当时摸索欠考虑，时钟初始化比较乱，需要改进
{
CLKSEL_PLLSEL=0; //选定外部时钟，为1时选择锁相环时钟 时钟选择寄存器初始化
CLKSEL=0 ; //选择外部晶振为时钟源
PLLCTL_PLLON=0; //锁相环电路禁止；
PLLCTL_PRE=1; //实时中断允许
PLLCTL_PCE=1; //允许看门狗
PLLCTL_AUTO=1; //选择高频宽带控制
PLLCTL_SCME=1; //探测到外部时钟失效时产生自给时钟信号
SYNR=8; //时钟合成寄存器初始化
REFDV=0X07; //时钟分频寄存器初始化 与上句为做实验时确定的参数与理论参数有差距，可重新设置
//CLKSEL_PLLSEL=1 ; //选定锁相环时钟 此句被注解掉
PLLCTL_CME=1; //时钟监控允许 锁相环控制寄存器初始化
PLLCTL_PLLON=1;
while(!CRGFLG_LOCK)；//循环直到该位为1即时钟频率已稳定，
CLKSEL_PLLSEL=1; //选定锁相环时钟
}
void PORTAB_init(void) //信号输入输出口的设置，较简单
{
DDRA=0X00; //设置A口输入DDR寄存器置0为输入，置1为输出
DDRB=0XFF;//设置B口为输出，作为信号指示
PORTB=0X00;//初始化时设置为全亮，0亮1灭
}
void PWM_init(void) //脉宽调制模块的初始化。参考中文PPT
{
PWME=0; //关闭PWM
PWMCNT01=0; //01通道被禁止
PWMCTL_CON01=1; //01共同组成16位通道作为舵机的控制信号输入口 为1级联为0分开
PWMCTL_CON45=0; //分别组成8位通道 分别为驱动电机的正反转的输入口
PWMCTL_PSWAI=1; //不准许等待模式下分频时钟禁止运行
PWMCTL_PFRZ=1; //不准许冷结模式下PWM波形输出
PWMPOL=0X03; //对应通道脉冲起始位为高电平 极性为1
PWMCLK=0X33; //01、45分频
PWMPRCLK=0X03; //A_CLOCK=BUS_CLOCK/8=3MHZ
PWMSCLA=15; // 比例因子寄存器设置PWM寄存器的工作频率 SA_CLOCK=A_CLOCK/2*15=100KHZ
PWMCAE=0X00; //输出波形左对齐，否则中心对齐
PWMDTY01=80; //初始化时可任意设置
PWMPER01=2000; //设置PWM01通道频率100KHZ/2000=50HZ
PWMPER4=20; //设置PWM4频率为5KHZ
PWMPER5=20;
PWMDTY5=10;
PWMDTY4=15; // 占空比为15：20初始化时可任意设置
PWME=0X13; //使能PWM
}
void MDC_init(void)//模数计数器初始化 参考英文PPT008 轮速传感时有用
{
MCCTL_MCEN=0;//模数计数器禁止运行
MCCTL=0XE2;//MCZI=1中断允许MOCMC=1自动赋与最后给定值ROMCL=1读计数器数 返回存储器值 否3位 MCEN=0,MCPR1=1MCPR0= 8倍分频
MCCTL_MCEN=1;//模数计数器使能
MCCNT=1118;//从3000开始向下计数(1/24M)*8*1118
MCCTL_FLMC=1; //此句为参考他人程序，强制讲寄存器的值写到模数计数器
}
void PACB_init() //输入捕捉 用于轮速传感 英文PPT008
{
TCTL4 = 0X02; //下降沿捕捉脉冲
PBCTL = 0x40; //级联两个8位累加器(PAC0和PAC1)
ICPAR = 0X03; //使能累加器
PACN10 = 0X0000; //01口清零
}
void TCNT_init() //主时钟自由计数器初始化设置，用于起步延时
{
TSCR1=0X80;
TSCR2=0X07; //自由计数器128倍分频
}
int signal_count(unsigned char signal) //采集信号的个数
{unsigned char compare;//设置比较变量
int i;
int signal_num;
compare=0X80;
signal_num=0;
for(i=0;i<8;i++) //通过逐位比较并循环比较计算出有多少传感器探测到黑线
{if((signal&(compare>>i))>>(7-i))
signal_num++;}
return signal_num;} //返回计算值
void set_direction(float direction) //设置舵机转角
{if(direction<122)
direction=122;
if(direction>168)
direction=168; //限制舵机转角范围
error=direction-mid_direction; // 以下为本次比赛的粗糙的PD算法，微分因子的数值确定是实验估计值，缺少理论的计算，是本次比赛小车不能完美表现的的一个原因，由于积分算法没研究透，所以没采用
if(turn_symbol==1) //当转向标志为1时，此时向左转
{
if((last_error-error)==(right_direction-micro_direction)) //当上次偏差见减去本次偏差为副是表示小车有向外转的趋势，应加大微分因子，使小车大角度左转，迅速回到黑线中央
kd=0.5;
else if((last_error-error)==micro_direction-right_direction) //当上次偏差减去本次偏差为正，表示小车已经渐渐从偏离位置回到中间位置，此时应减小微分因子，以避免过多转向一起的超调量，使小车尽快稳定下来，以下的算法类似
kd=0.2 ;
else if((last_error-error)==(deep_direction-micro_direction))
kd=0.3;
else if((last_error-error)==(micro_direction-deep_direction))
kd=0.7;
else if((last_error-error)==(deep_direction-emergence_direction))
kd=0;
else if((last_error-error)==(emergence_direction-deep_direction))
kd=0.4;
}
if(turn_symbol==-1){
if((last_error-error)==(right_direction-micro_direction))
kd=0.2;
else if((last_error-error)==micro_direction-right_direction)
kd=0.5;
else if((last_error-error)==(deep_direction-micro_direction))
kd=0.7;
else if((last_error-error)==(micro_direction-deep_direction))
kd=0.3;
else if((last_error-error)==(deep_direction-emergence_direction))
kd=0.4;
else if((last_error-error)==(emergence_direction-deep_direction))
kd=0;
}
direction=direction-turn_symbol*kd*(last_error-error);
PWMDTY01=direction;
mem_error=last_error; //记忆上次偏差
last_error=error; //将即将成为过去式的转向偏差赋值给上次偏差以供下一循环程序引用
memory_direction=direction; //将本次最终转角赋值给记忆转角变量，以供小车探头在伸出赛道后按照最后检测到的信号来转向
}
int set_speed(int speed)
{
if(speed<5)
speed=5;
if(speed>18)
speed=18; //设置驱动电机速度范围，以下为检测到需要减速时的减速控制程序
if(turn_symbol>0&&(mem_error-error)>=7&&(speed_back>=13)) //当转向标志大于零，上次偏差减去本次偏差值大于七且轮速传感返回脉冲值大于13个脉冲时实施减速，但是13为估计值也是缺少精确的理论计算，希望各位能改善
{
while(speed_back>13)//次句为循环直到返回脉冲信号低于13时跳出循环，小车正常行驶
{
PWME=0X23; //舵机口正常输出，PWM4切断。PWM5口正常输出，反向驱动
PWMDTY5=2; //为估计值，可视情况而设
}
//signal=PORTA;
//signal_analysis(signal);
//set_direction(memory_direction);
}
if(turn_symbol<0&&(error-mem_error)>=7&&(speed_back>=13)) 当转向标志大于零，本次偏差减去上次偏差值大于七且轮速传感返回脉冲值大于13个脉冲时实施减速，但是13为估计值也是缺少精确的理论计算，希望各位能改善
{
while(speed_back>13) {
PWME=0X23;
PWMDTY5=3;
}
}
//signal=PORTA;
//signal_analysis(signal);
//set_direction(memory_direction);
PWME=0X13;
speed1=speed_back/4;
speed_last_error=speed_error;
speed_error=speed-speed1;
//speed=speed+speed_kp*speed_error+speed_kd*(speed_error-speed_last_error);本打算用轮速的PD控制算法，后来因无法精确控制，造成效果不好而注解掉了
//}
PWMDTY4=speed;
}
void normal_state(unsigned char signal,int turn_symbol)//正常行驶状态，调用相应的转向和速度设置子程序
{
set_direction(mid_direction+right_direction);
set_speed(direct_speed);
}
void micro_adjust_state(unsigned char signal,int turn_symbol) //转小弯时状态
{
int direction ;
int speed;
direction=mid_direction-turn_symbol*micro_direction; //设置转小弯时的转角
set_direction(direction);
speed=micro_speed; //设置转小弯时的速度
set_speed(speed);
}
void deep_adjust_state(unsigned char signal,int turn_symbol) //设置原理同上
{
int direction;
int speed;
direction=mid_direction-turn_symbol*deep_direction;
set_direction(direction);
speed=deep_speed;
set_speed(speed);
}
void emergence_state(unsigned char signal,int turn_symbol) // 同上
{
float direction;
int speed;
direction=mid_direction-turn_symbol*emergence_direction;
set_direction(direction);
speed=emergence_speed;
set_speed(speed);
}
void quick_back_state(void) //当探头由于伸出跑道外而探测不到信号时的控制策略
{set_direction(memory_direction);
PWME=0X13;
PWMDTY4=deep_speed;
}
int signal_analysis( unsigned char signal) //分析信号 根据采集信号数，在根据不同信号数中不同情况调用不同子程序
{int signal_num;
double delay;
int flag;
flag=0;
signal_num=signal_count(signal);
if(signal_num==1)
{if(signal>0X08)
turn_symbol=1; //设置转向标志，左正右4副
else turn_symbol=-1;
if(signal==0X08||signal==0X10)
normal_state(signal,turn_symbol); //视不同情况将不同的信号和转向信号传递给子函数
else if(signal==0X20||signal==0X04)
micro_adjust_state(signal,turn_symbol);
else if(signal==0X40||signal==0X02)
deep_adjust_state(signal,turn_symbol);
else if(signal==0X80||signal==0X01)
emergence_state(signal,turn_symbol);}
else if(signal_num==2)
{if(signal>0X0C)
turn_symbol=1;
else turn_symbol=-1;
if(signal==0X18)
normal_state(signal,turn_symbol);
else if(signal==0X30||signal==0X0C)
micro_adjust_state(signal,turn_symbol);
else if(signal==0X60||signal==0X06)
deep_adjust_state(signal,turn_symbol);
else if(signal==0XC0||signal==0X03)
emergence_state(signal,turn_symbol);}
else if(signal_num==3)
{if(signal>=0X1C)
turn_symbol=1;
else turn_symbol=-1;
if(signal==0X70||signal==0X0E)
emergence_state(signal,turn_symbol);
else if(signal==0X38||signal==0X1C)
deep_adjust_state(signal,turn_symbol);
else if(signal==0XE0||signal==0x07)
emergence_state(signal,turn_symbol);
else normal_state(signal,turn_symbol);}
/*else if(signal_num==4) 过多考虑，已注解掉
{if(signal>0X3C)
turn_symbol=1;
else turn_symbol=-1;
if(signal==0X78||signal==0X1E||signal==0XF0||signal==0X0F)
emergence_state(signal,turn_symbol);
else normal_state(signal,turn_symbol);
}*/
else if(signal_num==0)
quick_back_state();
else if(signal_num>3) //设置停车程序，因控制不稳定而注解掉
{
//flag++;
stop_symbol++; //停车信号自加
for(delay=0;delay<4000;delay++); //为防止单片机运行速度快而过多计数设置延时，在这段时间不计数
/*TCNT=0X0000; //该段延时因时间过长而容易使单片机失去控制，注解掉了
while(TCNT!=0X0000)
signal_analysis(signal);
while(TCNT==0X0000);*/
/*if(flag!=0){
stop_symbol++;
flag=0;
} */
if(stop_symbol>6){
PWME=0X23;
PWMDTY5=5;
TCNT=0X0000;
while(TCNT!=0X0000); //防止车速过快而在3米内无法停车而设置的延时制动
while(TCNT==0X0000);
for(;;)
PWME=0X03; //电机口切断，舵机口正常输出，从而停车
set_direcion(mid_direction); //停车后为防止小车在惯性的作用下在向前冲的过程中缺少控制而在最终是冲出跑道，应该在此时设置，使单片机仍能监控，舵机正常运行，但这次比赛没能实现，希望改进
}
}
}
void main(void) //主程序
{int i;
stop_symbol=0; //停车标志置0
PLL_init(); //调用各初始化子程序
TCNT_init();
TCNT=0X0000;
for(i=0;i<5;i++){
while(TCNT!=0x0000); //延时起步，不是很理解其工作原理，初步估计是从零开始一直运行到溢出返回，大概延迟时间为四秒
while(TCNT==0x0000)
set_direction(mid_direction);
}
MDC_init();
PACB_init();
PWM_init();
PORTAB_init();
last_error=0;
memory_direction=147;
EnableInterrupts; //允许中断，不知道是否是固定用法
for(;;) {
signal=PORTA;
signal_analysis(signal);
PORTB=PACN10;
}
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED //中断应用程序
void interrupt 26 MDC_ISR(void)
{static int count1=0; //设置成静态变量，以最后次赋值来定值
/*signal=PORTA;
signal_analysis(signal);*/
MCFLG=0X80; //设置信号上升沿触发见英文PPT008
count1++;
if(count1==inter1) //当计数到预定值时采集次脉冲数从而计算出速度
{
speed_back=PACN10; //返回速度
粘贴以后就直接能用的，朋友们有福了
