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

附录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=(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
*文件名：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注销这段代码，这些功能由MATLAB计算，并预存在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_aAdValue[8];
void ad_init(void)
{
// ADMUX=(1<<REFS1)|(1<<REFS0)|(0<<ADLAR)|(0<<MUX4)|(1<<MUX3)|(0<<MUX2)|(0<<MUX1)|(0<<MUX0);
// ADCSRA=(1<<ADEN)|(0<<ADSC)|(0<<ADATE)|(0<<ADIE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
}
/*
unsigned int ad(unsigned char ad_x)
{
unsigned char i;
unsigned int ret;
unsigned char max_id,min_id,max_value,min_value;
ADMUX=0xc1;//内部2.56V 参考电压，0 通道
ADCSRA=_BV(ADEN);//使能ADC，单次转换模式
//连续转换8 次
for(i=0;i<8;i++)
{
ADCSRA|=_BV(ADSC);
_delay_loop_1(60);
while(ADCSRA&_BV(ADSC))
_delay_loop_1(60);
ret=ADCL;
ret|=(unsigned int)(ADCH<<8);
g_aAdValue[i]=ret;
}
ret=0;
for(i=1;i<8;i++)
ret+=g_aAdValue[i];

ret/=7; //找到最大和最小值索引
max_id=min_id=1;
max_value=min_value=0;
for(i=1;i<8;i++)
{
if(g_aAdValue[i]>ret)
{
if(g_aAdValue[i]-ret>max_value)
{
max_value=g_aAdValue[i]-ret;
max_id=i;
}
}
else
{
if(ret-g_aAdValue[i]>min_value)
{
min_value=ret-g_aAdValue[i];
min_id=i;
}
}
}
//去掉第一个和最大最小值后的平均值
ret=0;
for(i=1;i<8;i++)
{
if((i!=min_id)&&(i!=max_id))
{
ret+=g_aAdValue[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_aAdValue[i]=ret;
}
ret=0;
for(i=1;i<8;i++)
ret+=g_aAdValue[i];
//找到最大和最小值索引
ret/=7;
max_id=min_id=1;
max_value=min_value=0;
for(i=1;i<8;i++)
{
if(g_aAdValue[i]>ret)
{
if(g_aAdValue[i]-ret>max_value)
{
max_value=g_aAdValue[i]-ret;
max_id=i;
}
}
else
{
if(ret-g_aAdValue[i]>min_value)
{
min_value=ret-g_aAdValue[i];
min_id=i;
}
}
}
//去掉第一个和最大最小值后的平均值
ret=0;
for(i=1;i<8;i++)
{
if((i!=min_id)&&(i!=max_id))
{
ret+=g_aAdValue[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<<PB0)))
{
eye_data++;
}
}
return eye_data; //返回检测结果
}

//*****小狗行为动作总控函数*********
void find_do(void)
{
unsigned char azimuth=0,t_back,scan_again;
/* unsigned char i_test;
for(i_test=0;i_test<250;i_test++)
{
glow(255-i_test,i_test);
delay_nms(3);
}
for(i_test=0;i_test<250;i_test++)
{
glow(i_test,255-i_test);
delay_nms(3);
} */
do
{
azimuth=scan();
if(azimuth>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<<ad_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 char i;
unsigned int temp=0;
for(i=0;i<8;i++)
{
if(send_pwm[i]>pwm_top||send_pwm[i]<pwm_min) //检查脉宽系数，若大于3000，
{ //则认为错误，并强制修改为1500
send_pwm[i]=1500;
}
temp+=send_pwm[i];
}
send_pwm[10]=65500-temp;
} 4-11注销此函数，此功能由下位机实现 */

//****