附录3：四足步态算法及其控制代码.doc

附录3：四足步态算法及其控制代码
/***＊*＊＊＊*＊**＊＊*＊＊＊＊***＊*＊*＊****＊***
＊工程名:DOG
＊文件名：main。c
*功能：上位机步态算法平台。
＊CPU:M16
*作者:YY
＊当前版本:1。0
＊日期：2008—4-2
＊**＊*＊**＊＊*＊＊＊＊＊＊**＊＊＊＊＊*＊＊*＊*＊＊＊＊**/
#include <avr/io。h>
#include 〈avr/interrupt。h>
#include <avr/wdt。h>
#include 〈util/delay。h>
//#include 〈avr/>
＃include ”gait。h"
#include ”usart。h"
＃include ””
＃include "”
unsigned char send_pwm[9］=｛53，18，34，67,39，63，50,23,45｝； //PWM参数数组
unsigned char up_t=0; //时钟更新标志
unsigned char t； //步态时钟节拍
unsigned char error=0; //错误标志
unsigned char t_backup； //t的备份
unsigned char one_T=0；
/*＊＊＊＊*＊**＊＊*＊＊*＊*＊＊*＊＊*＊*＊＊*＊＊＊*****＊＊****＊***＊＊＊*＊*****＊**＊***＊＊＊
*定时器0用来产生步态时钟，定时器1和2用来产生PWM波，调配输出光色
＊***＊＊＊＊＊＊***＊*＊＊＊*＊***＊＊＊＊＊*＊＊＊＊＊＊＊*＊*＊****＊＊*＊＊***＊**＊＊*＊＊**＊***/
void time_init（void)
{
TCCR0=(0<<FOC0）|(0<〈WGM00)｜(0<<WGM01)｜(0〈<COM00）|(0〈<COM01)|（0〈〈CS02)｜(0<<CS01)|(0<〈CS00); //1024分频
OCR0=38； //设置步态时钟节拍
TIMSK=（1<<OCIE0)|（0〈〈TOIE0）；
TCCR2=（0<〈FOC0)|(1〈〈WGM20)|(1<〈WGM21)｜(0<〈COM20）|(1〈<COM21)｜（1〈〈CS22)｜(0〈〈CS21)｜（0〈〈CS20）; //快速PWM模式，比较匹配清零
TCCR1A=（1<〈COM1A1)|（0<<COM1A0）｜（1<<COM1B1)｜（0〈<COM1B0）｜(0〈〈FOC1A）｜(0<<FOC1B）|(0<〈WGM11）|(1〈〈WGM10）； //8位快速PWM模式，比较匹配清零
TCCR1B=(0<〈WGM13)｜（1<〈WGM12）｜（0〈<CS12)|(1〈〈CS11)｜（1〈〈CS10)；
}
void io_init（）
｛
DDRB｜=(0<<PB0)；
DDRD=（1〈<PD5)|（1<<PD7）；
}
/＊*＊*＊＊＊**＊＊产生步态时钟＊＊＊*＊＊***＊*＊＊＊*＊*＊***＊＊＊/
SIGNAL(SIG_OUTPUT_COMPARE0)
{
if(t>105)
｛
t=0；
one_T=1;
}
else t++;
up_t=1；
｝
/＊*＊*＊＊**＊＊容错处理，USART接收中断＊*＊*＊*＊***＊＊**/
SIGNAL（SIG_UART_RECV)
｛
if（'N’==UDR）
｛
error=1；
｝
｝
int main（void)
{
cli（）；
wdt_disable();
io_init(）;
usart_init（）；
time_init()；
sei（）；
usart_send_pwm(); //位置初始化
find_do（);
｝
/＊******＊**＊＊＊*＊＊＊**＊＊*＊*＊＊****＊*＊**
*工程名：DOG
*文件名：
*功能：步态算法函数文件。
*当前版本：
*日期:2008-4—2
＊＊****＊＊*＊＊**＊＊**＊*****＊*＊*＊＊*＊*＊＊＊＊/
/*＊＊＊＊＊*＊＊＊*＊＊**＊*＊**＊＊＊＊＊****＊＊＊＊**
＊当前版本：2。0
＊步态参数事先计算，更改相关函数
＊