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

#include #include unsigned int phase = 0; unsigned int ServoPos6 = 97,89,94,88,94,96; unsigned int HomePos6 = 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 |= (1phase);/* 把PC口的第phase个引脚置高 phase=0,1,2,3,4,5,6,7*/ TCNT1 = 0x00; /* 计时器计数器1 置0 */OCR0 = ServoPosphase; /* 当TCNT0增加到和ServoPosphase相等时产生计时器计数器 0中断，ServoPosphase的值决定了高电平的时间，也就是脉冲的宽度 */ TCCR0 |= (1 5)/*phase=0,1,2,3,4,5,6,7,分别对应了PC0，PC1，PC2，.PC7*/phase = 0; /*-计时器计数器 0(8 bit) 比较匹配中断（中断的时间根据OCR0中设置的 ServoPosphase值决定，中断后把PC口8个引脚全部降低）-*/SIGNAL(SIG_OUTPUT_COMPARE0)PORTC &= (1PC0) | (1PC1) | (1PC2) | (1PC3) | (1PC4) | (1PC5); /* 发生中断时让 PC 0-PC 7 全部置低*/TCCR0 &= (1 90*/* 1-1.5*/*15-22.5*/*20- 30*/*30- 45*/*用T2中断做的 mS 延时函数-用于每个动作之间的延时*/void wait_ms(int msec)int count; /* 反复用途计数器变数 （for 句子用使用 )*/TCCR2 |= (1CS22); /* 在 64分频内计时器us一次，计数器 2 开始 */for (count = 0; count msec; count+)/* 只反复 msec 的次数 */*1 mS 生成 */TCNT2 = 0x00;/* 计数器置0 */ while(TCNT2 0;i-)ServoPos2 = ServoPos2 + 2;ServoPos5 = ServoPos5 + 1;wait_ms(JBHWSD);for(i=XS;i0;i-)ServoPos2 = ServoPos2 - 1;wait_ms(JBHWSD);for(i=MBFDY;i0;i-)ServoPos0 = ServoPos0 - 1;ServoPos1 = ServoPos1 - 1;ServoPos3 = ServoPos3 - 1;ServoPos4 = ServoPos4 - 1;wait_ms(MBSD);for(i=XS;i0;i-)ServoPos2 = ServoPos2 - 1;ServoPos5 = ServoPos5 - 1;wait_ms(JBHWSD);void step2(void)/行进中抬右脚迈右腿右脚落地unsigned char i;for(i = XS;i0;i-)ServoPos2 = ServoPos2 - 1;ServoPos5 = ServoPos5 - 2;wait_ms(JBHWSD);for(i = XS;i0;i-)ServoPos5 = ServoPos5 + 1;wait_ms(JBHWSD);for(i =MBFDE;i0;i-)ServoPos0 = ServoPos0 + 1;ServoPos1 = ServoPos1 + 1;ServoPos3 = ServoPos3 + 1;ServoPos4 = ServoPos4 + 1;wait_ms(MBSD);for(i = XS;i0;i-)ServoPos5 = ServoPos5 + 1;ServoPos2 = ServoPos2 + 1;wait_ms(JBHWSD);void step3(void)/行进中抬左脚迈左腿左脚落地unsigned char i;for(i =XS;i0;i-)ServoPos2 = ServoPos2 + 2;ServoPos5 = ServoPos5 + 1;wait_ms(JBHWSD);for(i =XS;i0;i-)ServoPos2 = ServoPos2 - 1;wait_ms(JBHWSD);for(i = MBFDE;i0;i-)ServoPos0 = ServoPos0 - 1;ServoPos1 = ServoPos1 - 1;ServoPos3 = ServoPos3 - 1;ServoPos4 = ServoPos4 - 1;wait_ms(MBSD);/脚底板回位,左脚落地for(i = XS;i0;i-)ServoPos2 = ServoPos2 - 1;ServoPos5 = ServoPos5 - 1;wait_ms(JBHWSD);void step4(void)/迈右腿停止unsigned char i;for(i=XS;i0;i-)ServoPos2 = ServoPos2 - 1;ServoPos5 = ServoPos5 - 2;wait_ms(JBHWSD);for(i=XS;i0;i-)ServoPos5 = ServoPos5 + 1;wait_ms(JBHWSD);for(i=MBFDY;i0;i-)ServoPos0 = ServoPos0 + 1;ServoPos1 = ServoPos1 + 1;ServoPos3 = ServoPos3 + 1;ServoPos4 = ServoPos4 + 1;wait_ms(MBSD);for(i = XS;i0;i-)ServoPos5 = ServoPos5 + 1;ServoPos2 = ServoPos2 + 1;wait_ms(JBHWSD);*/void step5(void)/1 头前趴 unsigned char i;for(i=FGFD;i0;i-)ServoPos3 = ServoPos3 + 1;ServoPos0 = ServoPos0 - 1;wait_ms(FGSD);for(i=FGFD;i0;i-)ServoPos1 = ServoPos1 + 1;ServoPos4 = ServoPos4 - 1;wait_ms(FGSD);void step6(void)/脚往前趴，unsigned char i;for(i=FGFD;i0;i-)/0 抬起90 ServoPos0 = ServoPos0 + 1; wait_ms(FGSD); wait_ms(50); for(i=FGFD;i0;i-)/1动180 ServoPos1 = ServoPos1 - 2; wait_ms(FGSD); for(i=FGFD;i0;i-)/0 z dong90 ServoPos0 = ServoPos0 + 1; wait_ms(FGSD); wait_ms(50);for(i=FGFD;i0;i-)/3 dong 90 ServoPos3 = ServoPos3 - 1; wait_ms(FGSD); wait_ms(50); for(i=FGFD;i0;i-)/ 4 dong 180 ServoPos4 = ServoPos4 + 2; wait_ms(FGSD);for(i=FGFD;i0;i-)/3 z dong 90 ServoPos3 = ServoPos3 - 1; wait_ms(FGSD); void step7(void)/头抬起unsigned char i;for(i=FGFD;i0;i-)ServoPos1 = ServoPos1 + 1;ServoPos4 = ServoPos4 - 1;wait_ms(FGSD);wait_ms(50);for(i=FGFD;i0;i-)ServoPos0 = ServoPos0 - 1;ServoPos3 = ServoPos3 + 1;wait_ms(FGSD);void step8(void)/2 头后趴 unsigned char i;for(i=FGFD;i0;i-)ServoPos3 = ServoPos3 - 1;ServoPos0 = ServoPos0 + 1;wait_ms(FGSD);for(i=FGFD;i0;i-)ServoPos1 = ServoPos1 - 1;ServoPos4 = ServoPos4 + 1;wait_ms(FGSD);void step9(void)/脚往后趴，unsigned char i;for(i=FGFD;i0;i-)/3 动 90 ServoPos3 = ServoPos3 + 1; wait_ms(FGSD); wait_ms(50); for(i=FGFD;i0;i-)/4 动180 ServoPos4 = ServoPos4 - 2; wait_ms(FGSD); for(i=FGFD;i0;i-)/3 z dong90 ServoPos3 = ServoPos3 + 1; wait_ms(FGSD); wait_ms(50);for(i=FGFD;i0;i-)/0 dong 90 ServoPos0 = ServoPos0 - 1; wait_ms(FGSD); wait_ms(50); for(i=FGFD;i0;i-)/ 1 dong 180 ServoPos1 = ServoPos1 + 2; wait_ms(FGSD);for(i=FGFD;i0;i-)/3 z dong 90 ServoPos0 = ServoPos0 - 1; wait_ms(FGSD); void step10(void)/头抬起unsigned char i;for(i=FGFD;i0;i-)ServoPos1 = ServoPos1 - 1;ServoPos4 = ServoPos4 + 1;wait_ms(FGSD);wait_ms(50);for(i=FGFD;i0;i-)ServoPos0 = ServoPos0 + 1;ServoPos3 = ServoPos3 - 1;wait_ms(FGSD);/*void step11(void)/行进中抬右脚迈右腿右脚落地unsigned char i;/脚底板转，身体向右倾斜，抬左脚，为迈腿准备/迈左腿（全幅）for(i=XS;i0;i-)ServoPos2 = ServoPos2 - 1;ServoPos5 = ServoPos5 -2 ;wait_ms(JBHWSD); for(i =XS;i0;i-) ServoPos5 = ServoPos5 + 1; wait_ms(JBHWSD); for(i = MBFDE;i0;i-)ServoPos0 = ServoPos0 + 1;ServoPos1 = ServoPos1 + 1;ServoPos3 = ServoPos3 + 1;ServoPos4 = ServoPos4 + 1;wait_ms(MBSD);/脚底板回位,左脚落地for(i = XS;i0;i-)ServoPos2 = ServoPos2 + 1;ServoPos5 = ServoPos5 + 1;wait_ms(JBHWSD);*/void homeP(void)ServoPos0 = HomePos0;ServoPos1 = HomePos1;ServoPos2 = HomePos2;ServoPos3 = HomePos3;ServoPos4 = HomePos4;ServoPos5 = HomePos5;int main(void) int l;/* PC口的设定 */DDRC |= (1PC0) | (1PC1) | (1PC2) | (1PC3) | (1PC4) | (1PC5); PORTC &= (1PC0) | (1PC1) | (1PC2) | (1PC3) | (1PC4) | (1PC5);/* 计时器计数器 1 的设定 */TIMSK |= (1OCIE1A); /* 计时器计数器 1 比较匹配中断 允许 */TCCR1A |= (1WGM12); /* 计时器计数器 1 CTC 比较匹配时清零定时器(CTC) 模式*/OCR1A =625; /* TCNT1计数到625产生中断 共625*4=2500us=2.5MS*/* 计时器计数器 0 的设定 */TIMSK |= (1OCIE0); /* 计时器计数器 0 的比较匹配中断 允许 */ SREG |= (1SREG_I); TCCR1B |= (1CS11) | (1CS10); /* 选择分频系数，并启动定时器 */hom