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

include include unsigned int phase 0 unsigned int ServoPos 6 97 89 94 88 94 96 unsigned int HomePos 6 97 89 94 88 94 96 define FGDD 10 翻跟头等待时间 define CBDD 100 迈步步等待时间 define XS 10 斜身幅度 define MBFDY 15 迈步幅度 1 define MBFDE MBFDY 2 迈步幅度 2 define FGFD 58 翻跟头幅度 define MBSD 15 迈步速度 define FGSD 12 翻跟头速度 define JBHWSD 50 后脚板回位速度 计时器计数器 1 16 bit 比较匹配中断 固定周期 2 5ms 中断 每次中断后把 PC 中一 个引脚置高 8 次中断正好 20ms 8 2 5 20ms SIGNAL SIG OUTPUT COMPARE1A PORTC 1 phase 把 PC 口的第 phase 个引脚置高 phase 0 1 2 3 4 5 6 7 TCNT1 0 x00 计时器计数器 1 置 0 OCR0 ServoPos phase 当 TCNT0 增加到和 ServoPos phase 相 等时产生计时器计数器 0 中断 ServoPos phase 的值决定了高电平的时间 也就是脉冲的 宽度 TCCR0 1 5 phase 0 1 2 3 4 5 6 7 分别对应了 PC0 PC1 PC2 PC7 phase 0 计时器计数器 0 8 bit 比较匹配中断 中断的时间根据 OCR0 中设置的 ServoPos phase 值决定 中断后把 PC 口 8 个引脚全部降低 SIGNAL SIG OUTPUT COMPARE0 PORTC 发生中 断时让 PC 0 PC 7 全部置低 TCCR0 反复用途计数器变数 for 句子用使用 TCCR2 1 CS22 在 64 分频内计时器 us 一次 计 数器 2 开始 for count 0 count msec count 只反复 msec 的次数 1 mS 生成 TCNT2 0 x00 计数器 置 0 while TCNT2 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 void step2 void 行进中抬右脚迈右腿右脚落地 unsigned char i 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 void step3 void 行进中抬左脚迈左腿左脚落地 unsigned char i 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 void step4 void 迈右腿停止 unsigned char i 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 void step5 void 1 头前趴 unsigned char i 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 void step6 void 脚往前趴 unsigned char i 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 0 z dong 90 ServoPos 0 ServoPos 0 1 wait ms FGSD wait ms 50 for i FGFD i 0 i 3 dong 90 ServoPos 3 ServoPos 3 1 wait ms FGSD wait ms 50 for i FGFD i 0 i 4 dong 180 ServoPos 4 ServoPos 4 2 wait ms FGSD for i FGFD i 0 i 3 z dong 90 ServoPos 3 ServoPos 3 1 wait ms FGSD void step7 void 头抬起 unsigned char i for i FGFD i 0 i ServoPos 1 ServoPos 1 1 ServoPos 4 ServoPos 4 1 wait ms FGSD wait ms 50 for i FGFD i 0 i ServoPos 0 ServoPos 0 1 ServoPos 3 ServoPos 3 1 wait ms FGSD void step8 void 2 头后趴 unsigned char i 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 void step9 void 脚往后趴 unsigned char i for i FGFD i 0 i 3 动 90 ServoPos 3 ServoPos 3 1 wait ms FGSD wait ms 50 for i FGFD i 0 i 4 动 180 ServoPos 4 ServoPos 4 2 wait ms FGSD for i FGFD i 0 i 3 z dong 90 ServoPos 3 ServoPos 3 1 wait ms FGSD wait ms 50 for i FGFD i 0 i 0 dong 90 ServoPos 0 ServoPos 0 1 wait ms FGSD wait ms 50 for i FGFD i 0 i 1 dong 180 ServoPos 1 ServoPos 1 2 wait ms FGSD for i FGFD i 0 i 3 z dong 90 ServoPos 0 ServoPos 0 1 wait ms FGSD void step10 void 头抬起 unsigned char i for i FGFD i 0 i ServoPos 1 ServoPos 1 1 ServoPos 4 ServoPos 4 1 wait ms FGSD wait ms 50 for i FGFD i 0 i ServoPos 0 ServoPos 0 1 ServoPos 3 ServoPos 3 1 wait ms FGSD void step11 void 行进中抬右脚迈右腿右脚落地 unsigned char i 脚底板转 身体向右倾斜 抬左脚 为迈腿准备 迈左腿 全幅 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 2 ServoPos 2 1 ServoPos 5 ServoPos 5 1 wait ms JBHWSD void homeP void ServoPos 0 HomePos 0 ServoPos 1 HomePos 1 ServoPos 2 HomePos 2 ServoPos 3 HomePos 3 ServoPos 4 HomePos 4 ServoPos 5 HomePos 5 int main void int l PC 口的设定 DDRC 1 PC0 1 PC1 1 PC2 1 PC3 1 PC4 1 PC5 PORTC 计时器计数器 1 的设定 TIMSK 1 OCIE1A 计时器计数器 1 比较匹配中断 允许 TCCR1A 1 WGM12 计时器计数器 1 CTC 比较匹配时清零定时器 CTC 模式 OCR1A 625 TCNT1 计数到 625 产生中断 共 625 4 2500us 2 5MS 计时器计数器 0 的设定 TIMSK 1 OCIE0 计时器计数器 0 的比较匹配中断 允许 SREG 1 SREG I TCCR1B 1 CS11 1 CS10 选择分频系数 并启动定时器 homeP wait ms 2000 li zheng tai bu zou 1 step1 wait ms CBDD step2 wait ms CBDD step3 wait ms CBDD step4 wait ms CBDD zou jie shu for l 0 l 3 l qian fan step5 wait ms FGDD step6 wait ms FGDD step7 wait ms FGDD step1 tai bu zou 2 wait m