附录3：四足步态算法及其控制代码/*工程名：DOG*文件名：main.c*功能：上位机步态算法平台。*CPU：M167.3728MHZ晶振*作者：YY*当前版本：1.0*日期：2008-4-2*/#include #include #include #include /#include #include gait.h#include usart.h#include think.h#include ad.hunsigned char send_pwm9=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=(0FOC0)|(0WGM00)|(0WGM01)|(0COM00)|(0COM01)|(0CS02)|(0CS01)|(0CS00); /1024分频OCR0=38;/设置步态时钟节拍TIMSK=(1OCIE0)|(0TOIE0);TCCR2=(0FOC0)|(1WGM20)|(1WGM21)|(0COM20)|(1COM21)|(1CS22)|(0CS21)|(0CS20);/快速PWM模式，比较匹配清零TCCR1A=(1COM1A1)|(0COM1A0)|(1COM1B1)|(0COM1B0)|(0FOC1A)|(0FOC1B)|(0WGM11)|(1WGM10);/8位快速PWM模式，比较匹配清零TCCR1B=(0WGM13)|(1WGM12)|(0CS12)|(1CS11)|(1CS10);void io_init()DDRB|=(0PB0);DDRD=(1PD5)|(1105)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*文件名：gait.c*功能：步态算法函数文件。*当前版本：1.0*日期：2008-4-2*/*当前版本：2.0*步态参数事先计算，更改相关函数*日期：2008-4-10*/#include gait.hconst prog_uchar t_a1=/ 1#膝关节53,51,46,41,35,29,24,20,16,14,13,13,14,16,20,24,29,35,41,46,51,53,53,52,52,52,51,51,51,51,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,51,51,51,51,51,51,51,51,52,52,52,52,52,52,52,52,52,52,52,52,53,53,53,53;const prog_uchar t_a2=/ 1#髋关节18,18,16,14,12,10,8,7,6,6,6,6,8,10,12,15,19,22,26,30,32,34,34,34,33,32,32,31,31,30,29,29,28,28,28,28,28,28,28,28,27,27,27,27,27,27,27,26,26,26,26,26,26,26,26,26,25,25,25,25,25,25,25,25,25,24,24,24,24,24,24,24,24,23,23,23,23,22,22,22,21,21,21,20,20,19,19,19,19,19,19,19,19,19,19,19,19,19,19,19,18,18,18,18,18,18,18;const prog_uchar t_a3=/ 3#膝关节34,34,34,34,34,34,35,35,35,35,35,35,35,35,36,36,36,36,36,36,36,36,37,37,37,38,38,38,38,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,40,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,38,38,38,38,40,44,49,55,60,65,70,73,75,76,76,75,72,69,64,59,53,47,41,36,34;const prog_uchar t_a4=/ 3#髋关节72,72,72,72,72,72,72,72,72,72,72,72,71,71,71,71,71,71,71,71,71,71,71,70,70,70,70,69,69,68,68,68,68,68,68,67,67,67,67,67,67,67,67,67,67,66,66,66,66,66,66,66,66,66,66,65,65,65,65,65,65,65,65,65,64,64,64,64,64,64,64,64,63,63,63,63,62,62,61,61,60,60,59,58,58,58,60,62,66,69,73,76,77,78,79,80,80,81,81,80,79,78,77,76,74,73,72;const prog_uchar t_a5=/2#膝关节39,39,39,39,39,39,39,39,39,39,40,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,38,38,38,38,38,38,38,38,37,37,37,37,37,37,37,37,37,37,37,37,36,36,36,36,36,38,43,48,54,60,65,69,73,75,76,76,75,73,69,65,60,54,48,43,38,36,36,37,37,37,38,38,38,38,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39,39;const prog_uchar t_a6=/2#髋关节63,63,64,64,64,64,64,64,64,64,65,65,65,65,65,65,65,65,65,66,66,66,66,67,67,67,68,68,68,69,69,70,70,70,70,70,70,70,70,70,70,70,70,70,70,71,71,71,71,71,71,71,71,71,71,73,75,76,77,78,78,79,79,79,79,78,78,76,74,70,67,63,59,57,55,55,55,56,57,57,58,58,59,60,60,61,61,61,61,61,61,61,61,62,62,62,62,62,62,62,63,63,63,63,63,63,63;const prog_uchar t_a7=/4#膝关节50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,51,51,51,51,49,45,40,34,29,24,19,16,14,13,13,14,17,20,25,30,36,42,48,53,55,55,55,55,55,55,55,54,54,54,54,54,54,54,54,53,53,53,53,53,53,53,53,52,52,52,51,51,51,51,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50,50;const prog_uchar t_a8=/4#髋关节23,23,24,24,24,24,24,24,24,24,25,25,25,25,25,25,25,25,25,26,26,26,26,27,27,28,28,29,29,30,31,31,31,29,27,23,20,16,13,10,7,5,4,4,3,3,4,6,8,10,13,15,16,17,17,17,17,17,17,17,17,17,17,17,17,17,18,18,18,18,18,18,18,18,18,18,18,19,19,19,19,20,20,21,21,21,