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

1、附录 3：四足步态算法及其控制代码 /* *工程名：DOG *文件名：main.c *功能：上位机步态算法平台。 *CPU： M16 7.3728MHZ 晶振 *作者：YY *当前版本：1.0 *日期：2008-4-2 */ #include #include #include #include /#include #include “gait.h“ #include “usart.h“ #include “think.h“ #include “ad.h“ unsigned char send_pwm9=53,18,34,67,39,63,50,23,45; /PWM 参数数组 unsigne

2、d 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=(00) if(1=up_t) up_t=0; up_data(t); usart_send_pwm(); if(0=t%10) if(vola()0) /检测脚底环境 n_temp=

3、0; /检测到出界，提前结束 along_error=1; if(1=one_T) /已完成一周期 one_T=0; n_temp-; TCCR0 /关闭定时器 if(1=along_error) return(t); else return (0); /*后退函数，参数 n 为目前脚的状态* void back(unsigned char n) /退回周期初始状态 unsigned char n_temp; n_temp=n; t=0; TCCR0|=0x05; /开启定时器 while(n_temp0) if(1=up_t) up_t=0; up_data(n_temp-t); usart

4、_send_pwm(); if(0=n_temp-t) n_temp=0; TCCR0 /关闭定时器 void go_R(unsigned char n) unsigned char n_temp; n_temp=n; TCCR0|=0x05; /开启定时器 while(n_temp0) if(1=up_t) up_t=0; right_data(t); usart_send_pwm(); if(1=one_T) one_T=0; n_temp-; TCCR0 /关闭定时器 void go_L(void ) /*4-10 注销这段代码，这些功能由 MATLAB 计算，并预存在 t_pwm数组中

5、* unsigned int angel_pwm(double angel) /计算角度对应脉宽 /角度参数 angel,返回脉宽参数 double temp; unsigned int OCR1A_N; /计算角度对应脉宽系数代码 temp=angel/M_PI+0.25 ; /得到角度对应的脉宽，单位 ms OCR1A_N=500*temp-1; /得到脉宽对应的 OCRIA 值 （即 pwm 参数） return (OCR1A_N); double track(unsigned char t_clk) /计算时间对应的角度参数 /时间参数 t，返回上肢角度参数 double angel,

6、ft_1; ft_1=-(M_PI)*square(t_clk-50)/1600 ; ft_1=6-exp(ft_1); angel=asin(ft_1/(3*M_SQRT2)-M_PI/4 ; return(angel); void update_pwm(void) double angel_1; switch(t/50) case 0 : angel_1=track(t); /存放上肢角度 send_pwm0=angel_pwm(angel_1); /计算上肢角度对应 pwm 值 send_pwm1=angel_pwm(M_PI/2-feet_h*angel_1/feet_l); brea

7、k; case 1 : angel_1=track(t); send_pwm0=angel_pwm(angel_1); send_pwm1=angel_pwm(M_PI/2-feet_h*angel_1/feet_l); angel_1=track(t-50); send_pwm2=angel_pwm(angel_1); send_pwm3=angel_pwm(M_PI/2-feet_h*angel_1/feet_l); break; case 2 : angel_1=track(t-50); send_pwm2=angel_pwm(angel_1); send_pwm3=angel_pwm(

8、M_PI/2-feet_h*angel_1/feet_l); angel_1=track(t-100); send_pwm4=angel_pwm(angel_1); send_pwm5=angel_pwm(M_PI/2-feet_h*angel_1/feet_l); break; case 3 : angel_1=track(t-100); send_pwm4=angel_pwm(angel_1); send_pwm5=angel_pwm(M_PI/2-feet_h*angel_1/feet_l); angel_1=track(t-150); send_pwm6=angel_pwm(angel

9、_1); send_pwm7=angel_pwm(M_PI/2-feet_h*angel_1/feet_l); break; case 4 : angel_1=track(t-150); send_pwm6=angel_pwm(angel_1); send_pwm7=angel_pwm(M_PI/2-feet_h*angel_1/feet_l); break; */ /* *工程名：DOG *文件名：ad.c *功能：传感器 A/D 采集 *CPU： M16 7.3728MHZ 晶振 *作者：YY *当前版本：1.0 *日期：2008-5-7 */ #include “ad.h“ static

10、 unsigned int g_aAdValue8; void ad_init(void) / ADMUX=(1max_value) max_value=g_aAdValuei-ret; max_id=i; else if(ret-g_aAdValueimin_value) min_value=ret-g_aAdValuei; min_id=i; /去掉第一个和最大最小值后的平均值 ret=0; for(i=1;imax_value) max_value=g_aAdValuei-ret; max_id=i; else if(ret-g_aAdValueimin_value) min_value

11、=ret-g_aAdValuei; min_id=i; /去掉第一个和最大最小值后的平均值 ret=0; for(i=1;i87) return(255); else i+=1; delay_nms(10); if(eye()100) being=1; */ for(i=0;i100) being=pgm_read_byte(scan_n+i); if(0=being) return 255; /无目标 else return being; /返回角度值 /检测有无目标存在 unsigned char eye(void) unsigned char eye_i,eye_data; for(ey

12、e_i=0;eye_i250;eye_i+) if(0=(PINB return eye_data; /返回检测结果 /*小狗行为动作总控函数* void find_do(void) unsigned char azimuth=0,t_back,scan_again; /* unsigned char i_test; for(i_test=0;i_test250;i_test+) glow(255-i_test,i_test); delay_nms(3); for(i_test=0;i_test50) scan_again=1; /目标在右侧，相应动作代码区 glow(5,250); if(a

13、zimuth30 while(scan_again); /若无目标或目标不在前方，则继续搜索 scan_again=0; t_back=along(10); if(t_back!=0) /若返回非零，则后退到前一步 glow(250,5); back(t_back); else glow(5,250); while(1); void glow(unsigned char red,unsigned char bice) OCR2=bice; OCR1A=red; unsigned char vola(void) unsigned char ad_i,foux=0; for(ad_i=0;ad_i

14、2;ad_i+) if(ad(ad_i+1)0x133) /小于设定值时，说明已出界 foux|=(1ad_i); return(foux); /* *工程名：DOG *文件名：usart.c *功能：USART 通信,上位机程序。 *CPU： M16 7.3728MHZ 晶振 *作者：YY *当前版本：1.0 *日期：2008-4-2 */ /* *当前版本：1.1 *更改数据串格式，注销 revise()，其功能转下位机执行 *日期：2008-4-15 */ #include “usart.h“ /*检查脉宽参数，并计算剩余时间 * void revise(void) unsigned c

15、har i; unsigned int temp=0; for(i=0;ipwm_top|send_pwmipwm_min) /检查脉宽系数， 若大于 3000， /则认为错误，并强制修改为 1500 send_pwmi=1500; temp+=send_pwmi; send_pwm10=65500-temp; 4-11 注销此函数，此功能由下位机实现 */ /*USART 初始化* void usart_init(void) UBRRH=(0URSEL)|0; UBRRL=31; /14.4k 波特率 / UBRRL=47; /9600 波特率 UCSRA|=(0U2X); /倍速, 误差

16、0.2% UCSRB=(1RXCIE)|(1RXEN)|(0TXEN)|(0UCSZ2); /开启接收，发送功能用 到时开启 UCSRC=(1URSEL)|(0UMSEL)|(1UPM1)|(0UPM0)|(1USBS)|(1UCSZ1)|(1 UCSZ0); /*发送数组 send_pwm* *发送数据格式：S+(a1+b1)+(a2+b2)+.(an+bn).+(a9+b9)+P * S 为起始码 P 为停止码 * an-bn 为 unsignend char 型数据 * (an+bn)组成一个 unsigned int 型的数据 * an 低 8 位，bn 高 8 位 */ void u

17、sart_send_pwm(void) unsigned char i; UCSRB=(1TXEN); /开启发送使能 UDR=S; /发送起始码 S for(i=0;i9;i+) while(0=(UCSRA UDR=send_pwmi; /* temp=(unsigned char)(send_pwmi); UDR=temp; asm (“nop“); asm (“nop“); asm (“nop“); asm (“nop“); while(0=(UCSRA UDR=temp; */ while(0=(UCSRA UDR=P; /发送停止码 P while(0=(UCSRA /等待发送完毕后 UCSRB=(0TXEN); /关闭发送使能