附录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_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<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
*文件名: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,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,
21,21,21,22,22,22,22,22,22,22,22,22,22,23,23,23,23,23,23,23,23
};
/*************将直行相应数据装入待发送的缓冲区********************/ void up_data(unsigned char time)
{
send_pwm[0]=pgm_read_byte(t_a1+time);
send_pwm[1]=pgm_read_byte(t_a2+time);
send_pwm[2]=pgm_read_byte(t_a3+time);
send_pwm[3]=pgm_read_byte(t_a4+time)-5; //修正机械误差
send_pwm[4]=pgm_read_byte(t_a5+time);
send_pwm[5]=pgm_read_byte(t_a6+time);
send_pwm[6]=pgm_read_byte(t_a7+time);
send_pwm[7]=pgm_read_byte(t_a8+time);
}
/**********将右转相应数据装入待发送的缓冲区****************/ void right_data(unsigned char time)
{
/* send_pwm[0]=pgm_read_byte(right_a1+time);
send_pwm[1]=pgm_read_byte(right_a2+time);
send_pwm[2]=pgm_read_byte(right_b1+time);
send_pwm[3]=pgm_read_byte(right_b2+time)-5;
send_pwm[4]=pgm_read_byte(right_c1+time);
send_pwm[5]=pgm_read_byte(right_c2+time);
send_pwm[6]=pgm_read_byte(right_d1+time);
send_pwm[7]=pgm_read_byte(right_d2+time); */
}
//******前进指定步数,参数n为前进步数*******
unsigned char along(unsigned char n)
{
unsigned char n_temp,along_error=0;
n_temp=n;
TCCR0|=0x05; //开启定时器
while(n_temp>0)
{
if(1==up_t)
{
up_t=0;
up_data(t);
usart_send_pwm();
}
if(0==t%10)
{
if(vola()>0) //检测脚底环境
{
n_temp=0; //检测到出界,提前结束
along_error=1;
}
}
if(1==one_T) //已完成一周期
{
one_T=0;
n_temp--;
}
}
TCCR0&=0xfa; //关闭定时器
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_temp>0)
{
if(1==up_t)
{
up_t=0;
up_data(n_temp-t);
usart_send_pwm();
}
if(0==n_temp-t)
{
n_temp=0;
}
}
TCCR0&=0xfa; //关闭定时器
}
void go_R(unsigned char n)
{
unsigned char n_temp;
n_temp=n;
TCCR0|=0x05; //开启定时器
while(n_temp>0)
{
if(1==up_t)
{
up_t=0;
right_data(t);
usart_send_pwm();
}
if(1==one_T)
{
one_T=0;
n_temp--;
}
}
TCCR0&=0xfa; //关闭定时器
}
void go_L(void )
{
}
/*4-10注销这段代码,这些功能由MA TLAB计算,并预存在t_pwm[]数组中******* 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,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_pwm[0]=angel_pwm(angel_1); //计算上肢角度对应pwm值
send_pwm[1]=angel_pwm(M_PI/2-feet_h*angel_1/feet_l);
break;
case 1 :
angel_1=track(t);
send_pwm[0]=angel_pwm(angel_1);
send_pwm[1]=angel_pwm(M_PI/2-feet_h*angel_1/feet_l);
angel_1=track(t-50);
send_pwm[2]=angel_pwm(angel_1);
send_pwm[3]=angel_pwm(M_PI/2-feet_h*angel_1/feet_l);
break;
case 2 :
angel_1=track(t-50);
send_pwm[2]=angel_pwm(angel_1);
send_pwm[3]=angel_pwm(M_PI/2-feet_h*angel_1/feet_l);
angel_1=track(t-100);
send_pwm[4]=angel_pwm(angel_1);
send_pwm[5]=angel_pwm(M_PI/2-feet_h*angel_1/feet_l);
break;
case 3 :
angel_1=track(t-100);
send_pwm[4]=angel_pwm(angel_1);
send_pwm[5]=angel_pwm(M_PI/2-feet_h*angel_1/feet_l);
angel_1=track(t-150);
send_pwm[6]=angel_pwm(angel_1);
send_pwm[7]=angel_pwm(M_PI/2-feet_h*angel_1/feet_l);
break;
case 4 :
angel_1=track(t-150);
send_pwm[6]=angel_pwm(angel_1);
send_pwm[7]=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 unsigned int g_aAdV alue[8];
void ad_init(void)
{
//
ADMUX=(1<ret)
{
if(g_aAdV alue[i]-ret>max_value)
{
max_value=g_aAdV alue[i]-ret;
max_id=i;
}
}
else
{
if(ret-g_aAdV alue[i]>min_value)
{
min_value=ret-g_aAdV alue[i];
min_id=i;
}
}
}
//去掉第一个和最大最小值后的平均值
ret=0;
for(i=1;i<8;i++)
{
if((i!=min_id)&&(i!=max_id))
{
ret+=g_aAdV alue[i];
}
}
if(min_id!=max_id)
{
ret/=5;
}
else ret/=6;
ADCSRA=0;//关闭ADC
return ret;
}*/
unsigned int ad(unsigned char ad_x)
{
unsigned char i;
unsigned int ret;
unsigned char max_id,min_id,max_value,min_value;
switch(ad_x)
{
case 0:ADMUX=0x40;break;
case 1: ADMUX=0x41;break;
case 2:ADMUX=0x42;break;
case 3:ADMUX=0x43;break;
}
// ADMUX=0xc1; //内部2.56V 参考电压,0 通道ADCSRA=_BV(ADEN); //使能ADC,单次转换模式
for(i=0;i<8;i++) //连续转换8 次
{
ADCSRA|=_BV(ADSC);
_delay_loop_1(60);
while(ADCSRA&_BV(ADSC))
_delay_loop_1(60);
ret=ADCL;
ret|=(unsigned int)(ADCH<<8);
g_aAdV alue[i]=ret;
}
ret=0;
for(i=1;i<8;i++)
ret+=g_aAdV alue[i];
//找到最大和最小值索引
ret/=7;
max_id=min_id=1;
max_value=min_value=0;
for(i=1;i<8;i++)
{
if(g_aAdV alue[i]>ret)
{
if(g_aAdV alue[i]-ret>max_value)
{
max_value=g_aAdV alue[i]-ret;
max_id=i;
}
}
else
{
if(ret-g_aAdV alue[i]>min_value)
{
min_value=ret-g_aAdV alue[i];
min_id=i;
}
}
}
//去掉第一个和最大最小值后的平均值
ret=0;
for(i=1;i<8;i++)
{
if((i!=min_id)&&(i!=max_id))
{
ret+=g_aAdV alue[i];
}
}
if(min_id!=max_id)
{
ret/=5;
}
else ret/=6;
ADCSRA=0;//关闭ADC
return ret;
}
/***********************************
*工程名:DOG
*文件名:think.c
*功能:传感器检测及动作决策函数
*当前版本:1.0
*日期:2008-5-1
************************************/
/***********************************
*当前版本:1.1
*修改传感器检测函数
*日期:2008-5-9
************************************/
#include "think.h"
const prog_uchar scan_n[]={
44,46,48,50,52,54,56,58,60,62,
64,66,68,70,72,74,76,78,80,82,
84,86,86,84,82,80,78,76,74,72,
70,68,66,64,62,60,58,56,54,52,
50,48,46,44,42,40,38,36,34,32,
30,28,26,24,22,20,18,16,14,12,
10,8,6,4,2,0,0,2,4,6,
8,10,12,14,16,18,20,22,24,26,
28,30,32,34,36,38,40,42,44
};
/************头部传感器扫描函数,返回目标相对角度值*****
********************************************************/ unsigned char scan(void)
{
unsigned char i=0;
unsigned being=0; //存放目标位置
/* while(0==being)
{
send_pwm[8]=pgm_read_byte(scan_n+i);
usart_send_pwm();
if(i>87)
{
return(255);
}
else
{
i+=1;
}
delay_nms(10);
if(eye()>100)
{
being=1;
}
} */
for(i=0;i<87;i++)
{
send_pwm[8]=pgm_read_byte(scan_n+i);
usart_send_pwm();
delay_nms(10);
if(eye()>100)
{
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(eye_i=0;eye_i<250;eye_i++)
{
if(0==(PINB&(1<50)
{
scan_again=1; //目标在右侧,相应动作代码区
glow(5,250);
}
if(azimuth<40)
{
scan_again=1; //目标在左侧,相应动作代码区
glow(250,5);
}
if(azimuth==255) //无目标
{
scan_again=1;
glow(1,1);
}
if(azimuth>30&&azimuth<60) //目标在前方
{
scan_again=0;
}
}
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<2;ad_i++)
{
if(ad(ad_i+1)<0x133) //小于设定值时,说明已出界{
foux|=(1<pwm_top||send_pwm[i]>8);
UDR=temp;
*/
}
while(0==(UCSRA&(1<
本文档为【附录3：四足步态算法及其控制代码】，请使用软件OFFICE或WPS软件打开。作品中的文字与图均可以修改和编辑，
图片更改请在作品中右键图片并更换，文字修改请直接点击文字进行修改，也可以新增和删除文档中的内容。
该文档来自用户分享，如有侵权行为请发邮件ishare@vip.sina.com联系网站客服，我们会及时删除。
[版权声明] 本站所有资料为用户分享产生，若发现您的权利被侵害，请联系客服邮件isharekefu@iask.cn，我们尽快处理。
本作品所展示的图片、画像、字体、音乐的版权可能需版权方额外授权，请谨慎使用。
网站提供的党政主题相关内容(国旗、国徽、党徽..)目的在于配合国家政策宣传，仅限个人学习分享使用，禁止用于任何广告和商用目的。
浙公网安备 33021202002483