RoboCup双足竞步狭窄足冠军比赛程序.docx

RoboCup双足竞步狭窄足冠军比赛程序.docx

《RoboCup双足竞步狭窄足冠军比赛程序.docx》由会员分享，可在线阅读，更多相关《RoboCup双足竞步狭窄足冠军比赛程序.docx（16页珍藏版）》请在冰豆网上搜索。

RoboCup双足竞步狭窄足冠军比赛程序

#include

#include

unsignedintphase=0;

unsignedintServoPos[6]={97,89,94,88,94,96};

unsignedintHomePos[6]={97,89,94,88,94,96};

#defineFGDD10//翻跟头等待时间

#defineCBDD100//迈步步等待时间

#defineXS10//斜身幅度

#defineMBFDY15//迈步幅度1

#defineMBFDEMBFDY*2//迈步幅度2

#defineFGFD58//翻跟头幅度

#defineMBSD15//迈步速度

#defineFGSD12//翻跟头速度

#defineJBHWSD50//后脚板回位速度

/*-----计时器计数器1（16bit）比较匹配中断（固定周期2.5ms中断，每次中断后把PC中一个引脚置高,8次中断正好20ms8*2.5=20ms）-----*/

SIGNAL（SIG_OUTPUT_COMPARE1A）

{

PORTC|=（1<

TCNT1=0x00;/*计时器计数器1置0*/

OCR0=ServoPos[phase];/*当TCNT0增加到和ServoPos[phase]相等时产生计时器计数器0中断，ServoPos[phase]的值决定了高电平的时间，也就是脉冲的宽度*/

TCCR0|=（1<

phase++;/*phase加1*/

if（phase>5）/*phase=0,1,2,3,4,5,6,7,分别对应了PC0，PC1，PC2，...PC7*/

{

phase=0;

}

}

/*-----计时器计数器0（8bit）比较匹配中断（中断的时间根据OCR0中设置的ServoPos[phase]值决定，中断后把PC口8个引脚全部降低）-----*/

SIGNAL（SIG_OUTPUT_COMPARE0）

{

PORTC&=~（（1<

TCCR0&=~（1<

TCNT0=0x00;/*计时器计数器0置0*/

}

/*-----步行关系-----*/

/*（94-60）*16=512us=0.512ms*/

/*（94-00）*16=1504us=1.504ms*/

/*（94+60）*16=2496us=2.496ms*/

/*62------>90°*/

/*1------>1.5°*/

/*15----->22.5°*/

/*20------>30°*/

/*30------>45°*/

/*用T2中断做的mS延时函数-----用于每个动作之间的延时*/

voidwait_ms（intmsec）

{

intcount;/*反复用途计数器变数（for句子用使用）*/

TCCR2|=（1<

for（count=0;count

{

/*1mS生成*/

TCNT2=0x00;/*计数器２　置0*/

while（TCNT2<250）{}/*直到250*4=1000us=1ms反复（等待）*/

}

}

/*voidstep1（void）//由立正抬左脚迈左腿左脚落地

{

unsignedchari;

for（i=XS;i>0;i--）

{

ServoPos[2]=ServoPos[2]+2;

ServoPos[5]=ServoPos[5]+1;

wait_ms（JBHWSD）;

}

for（i=XS;i>0;i--）

{

ServoPos[2]=ServoPos[2]-1;

wait_ms（JBHWSD）;

}

for（i=MBFDY;i>0;i--）

{

ServoPos[0]=ServoPos[0]-1;

ServoPos[1]=ServoPos[1]-1;

ServoPos[3]=ServoPos[3]-1;

ServoPos[4]=ServoPos[4]-1;

wait_ms（MBSD）;

}

for（i=XS;i>0;i--）

{ServoPos[2]=ServoPos[2]-1;

ServoPos[5]=ServoPos[5]-1;

wait_ms（JBHWSD）;

}

}

voidstep2（void）//行进中抬右脚迈右腿右脚落地

{

unsignedchari;

for（i=XS;i>0;i--）

{ServoPos[2]=ServoPos[2]-1;

ServoPos[5]=ServoPos[5]-2;

wait_ms（JBHWSD）;

}

for（i=XS;i>0;i--）

{ServoPos[5]=ServoPos[5]+1;

wait_ms（JBHWSD）;

}

for（i=MBFDE;i>0;i--）

{

ServoPos[0]=ServoPos[0]+1;

ServoPos[1]=ServoPos[1]+1;

ServoPos[3]=ServoPos[3]+1;

ServoPos[4]=ServoPos[4]+1;

wait_ms（MBSD）;

}

for（i=XS;i>0;i--）

{

ServoPos[5]=ServoPos[5]+1;

ServoPos[2]=ServoPos[2]+1;

wait_ms（JBHWSD）;

}

}

voidstep3（void）//行进中抬左脚迈左腿左脚落地

{

unsignedchari;

for（i=XS;i>0;i--）

{ServoPos[2]=ServoPos[2]+2;

ServoPos[5]=ServoPos[5]+1;

wait_ms（JBHWSD）;

}

for（i=XS;i>0;i--）

{

ServoPos[2]=ServoPos[2]-1;

wait_ms（JBHWSD）;

}

for（i=MBFDE;i>0;i--）

{

ServoPos[0]=ServoPos[0]-1;

ServoPos[1]=ServoPos[1]-1;

ServoPos[3]=ServoPos[3]-1;

ServoPos[4]=ServoPos[4]-1;

wait_ms（MBSD）;

}

//脚底板回位,左脚落地

for（i=XS;i>0;i--）

{ServoPos[2]=ServoPos[2]-1;

ServoPos[5]=ServoPos[5]-1;

wait_ms（JBHWSD）;

}

}

voidstep4（void）//迈右腿停止

{

unsignedchari;

for（i=XS;i>0;i--）

{

ServoPos[2]=ServoPos[2]-1;

ServoPos[5]=ServoPos[5]-2;

wait_ms（JBHWSD）;

}

for（i=XS;i>0;i--）

{

ServoPos[5]=ServoPos[5]+1;

wait_ms（JBHWSD）;

}

for（i=MBFDY;i>0;i--）

{

ServoPos[0]=ServoPos[0]+1;

ServoPos[1]=ServoPos[1]+1;

ServoPos[3]=ServoPos[3]+1;

ServoPos[4]=ServoPos[4]+1;

wait_ms（MBSD）;

}

for（i=XS;i>0;i--）

{

ServoPos[5]=ServoPos[5]+1;

ServoPos[2]=ServoPos[2]+1;

wait_ms（JBHWSD）;

}

}*/

voidstep5（void）//1头前趴

{

unsignedchari;

for（i=FGFD;i>0;i--）

{

ServoPos[3]=ServoPos[3]+1;

ServoPos[0]=ServoPos[0]-1;

wait_ms（FGSD）;

}

for（i=FGFD;i>0;i--）

{

ServoPos[1]=ServoPos[1]+1;

ServoPos[4]=ServoPos[4]-1;

wait_ms（FGSD）;

}

}

voidstep6（void）//脚往前趴，

{

unsignedchari;

for（i=FGFD;i>0;i--）//0抬起90

{

ServoPos[0]=ServoPos[0]+1;

wait_ms（FGSD）;

}

wait_ms（50）;

for（i=FGFD;i>0;i--）//1动180

{

ServoPos[1]=ServoPos[1]-2;

wait_ms（FGSD）;

}

for（i=FGFD;i>0;i--）//0zdong90

{

ServoPos[0]=Serv