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

附录3：四足步态算法及其控制代码 /*********************************** *工程名：DOG *文件名：main.c *功能：上位机步态算法平台。 *CPU：M16 7.3728MHZ晶振 *作者：YY *当前版本：1.0 *日期：2008-4-2 ************************************/ #include avr/io.h #include avr/interrupt.h #include avr/wdt.h #include util/delay.h //#include avr/signal.h #include gait.h #include usart.h #include think.h #include ad.h 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=(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)|(1PD7); } /***********产生步态时钟************************/ SIGNAL(SIG_OUTPUT_COMPARE0) { if(t105) { 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.h const 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,5