|
/*************************************************************** 库文件的申明，包括伺服电机控制必须要用到的头文件MegaServo.h number规定要用到的舵机的数目，first是规定第一个舵机所接的pwm端口 比如说：first=4就是说第一个舵机接的端口是PWM4端口 ***************************************************************/ #include<MegaServo.h> #definenumber 6 /*number设为6*/ #definefirst 4 /*fitst设为4*/ MegaServoservo[number]; /*舵机号的申明*/ /******************************************************** 变量的初始化，pos1到pos6的初始化是设定6个舵机的初始位置 Kaishi为1时开始执行的动作，为0就等待，用到的是I/O38也就是下面所提到的button,其为低电平开始，即Kaishi=1 *********************************************************/ intpos1=90,pos2=90,pos3=90,pos4=90,pos5=90,pos6=90,k=0; intbe=5; /* */ inttm=10; /*后面用到的延时时间*/ intflag=0; /* */ intk1=0; /* k1是一个全局变量,在没翻跟头前是0,翻完跟头置1 */ intbutton=38; /*定义button为38*/ intkaishi; /* button为LOW时kaishi=1*/ /*********************************************** 端口的初始化，用到的函数 .attach是绑定PWM端口 pinMode(a,b)用于规定数字口输入输出方式，a是开发板的管脚， b是INPUT或者OUTPUT，必须大写 ************************************************/ void setup() { inti; for(i=0;i<number;i++) { servo.attach(i+first); /*将伺服电机1-6与PWM端口4-9绑定*/ } pinMode(button,INPUT); /*定义开发板管脚38(即单片机第50引脚)为输入*/ } /************************************************ 因为根据比赛规则必须要先走3步，而每一步必须要分解成多个子动作， 否则会显得生硬，下面是第一步的分解动作函数 ************************************************/ void first_step1() { to_pos2(2,4,5,79,90,78,18); /*to_pos2为三个舵机同时运行，转一个角度用18ms*/ to_pos3(0,1,3,4,95,100,105,96,18); /* to_pos3为四个舵机同时运行*/ to_pos3(0,1,3,4,98,102,107,98,18); to_pos3(0,1,3,4,108,104,110,104,18); to_pos1(2,5,94,95,18); /* to_pos1为两个舵机同时运行*/ } /*************************************************** 第三步分解动作函数 ***************************************************/ void first_step2() { to_pos2(2,4,5,75,71,78,18); to_pos3(0,1,3,4,71,78,75,74,18); to_pos3(0,1,3,4,92,92,96,96,18); to_pos3(0,1,3,4,98,96,103,100,18); to_pos3(0,1,3,4,102,98,108,103,18); to_pos3(0,1,3,4,108,104,110,104,18); to_pos1(2,5,94,95,18); } /******************************************************* 第二步分解动作函数 *******************************************************/ void second_step() { to_pos2(1,2,5,103,108,110,18); to_pos3(0,1,3,4,100,97,105,100,18); to_pos3(0,1,3,4,94,90,96,95,18); to_pos3(0,1,3,4,83,86,85,84,18); to_pos3(0,1,3,4,76,79,78,77,18); to_pos3(0,1,3,4,68,75,71,74,18); to_pos1(2,5,94,95,18); } /********************************************************* 三步走程序 init_end()函数是立正 *********************************************************/ void three_step() { first_step1(); /*第一步*/ second_step(); /*第二步*/ first_step2(); /*第三步*/ init_end(); /*立正*/ } /****************************************** 走步程序，k1是一个全局变量，在没有翻跟头之前是0翻完跟头置1 ******************************************/ void qianjin() { if(k1==0) first_step1(); elsefirst_step2(); second_step(); k1=1; } /************************************ 初始化角度 *********************************/ void init_start() { to_pos2(1,2,4,90,94,88,15); to_pos2(0,3,5,93,90,92,15); } /********************************************* 立正 *********************************************/ void init_end() { to_pos2(1,2,5,103,108,110,18); to_pos3(0,1,3,4,100,97,105,100,18); to_pos3(0,1,3,4,93,93,90,88,18); to_pos1(2,5,94,95,18); } /********************************************** 检测开始按钮，如果按下即button(38)=0，则开始 **********************************************/ void detect() { intks=HIGH; ks=digitalRead(button); /*数字IO口读输入电平函数，结果为HIGH或LOW*/ if(ks==LOW) { kaishi=1; } } /************************************************************ 主函数 ************************************************************/ void loop() { inti; detect(); /*检测是否按下开始按钮*/ init_start(); /*初始化角度*/ while(kaishi==1) { if(k==0) { init_start(); to_pos2(2,4,5,73,88,77,8); to_pos3(0,1,3,4,96,94,94,89,20); delay(10); to_pos3(0,1,3,4,102,100,103,95,10); to_pos3(0,1,3,4,107,104,108,97,10); to_pos3(0,1,3,4,111,107,113,101,20); to_pos1(2,5,100,98,8); delay(10); to_pos3(1,2,4,5,108,109,101,107,8); to_pos3(0,1,3,4,104,102,104,96,20); delay(3); to_pos3(0,1,3,4,94,93,95,92,10); to_pos3(0,1,3,4,87,86,87,85,10); to_pos3(0,1,3,4,78,80,79,80,11); to_pos3(0,1,3,4,70,72,73,72,20); to_pos1(2,5,94,95,8); delay(10); to_pos3(1,2,4,5,70,73,76,77,8); delay(3); to_pos3(0,1,3,4,78,81,80,78,20); to_pos3(0,1,3,4,86,88,87,84,10); to_pos3(0,1,3,4,94,94,94,90,10); to_pos3(0,1,3,4,102,100,103,95,10); to_pos3(0,1,3,4,107,104,108,97,11); to_pos3(0,1,3,4,111,107,113,101,20); to_pos1(2,5,100,98,8); delay(10); for(i=0;i<10;i++) { to_pos3(1,2,4,5,108,109,101,107,8); to_pos3(0,1,3,4,104,102,104,96,20); delay(3); to_pos3(0,1,3,4,94,93,95,92,10); to_pos3(0,1,3,4,87,86,87,85,10); to_pos3(0,1,3,4,78,80,79,80,11); to_pos3(0,1,3,4,70,72,73,72,20); to_pos1(2,5,94,95,8); delay(10); to_pos3(1,2,4,5,70,73,76,77,8); delay(3); to_pos3(0,1,3,4,78,81,80,78,20); to_pos3(0,1,3,4,86,88,87,84,10); to_pos3(0,1,3,4,94,94,94,90,10); to_pos3(0,1,3,4,102,100,103,95,10); to_pos3(0,1,3,4,107,104,108,97,11); to_pos3(0,1,3,4,111,107,113,101,20); to_pos1(2,5,100,98,8); delay(10); } k=1; } } } /******************************************************** 正翻3次分解动作，如果要合理必须在主函数中一个一个角度调用 ********************************************************/ void zhengfang() { inti; for(i=0;i<3;i++) { init_start(); delay(100); to_pos3(0,1,3,4,20,152,139,22,13); to_pos3(0,1,3,4,0,165,165,10,12); to_angle(3,150,6); to_pos1(3,4,40,150,6); to_pos1(3,4,13,165,6); to_angle(0,30,6); to_pos1(0,1,145,30,6); to_pos1(0,1,173,12,6); to_pos3(0,1,3,4,93,93,91,94,13); init_start(); } } /********************************************************* 倒翻3次的分解动作，调角度可按上提方法进行 *********************************************************/ void daofang() { inti; for(i=0;i<3;i++) { init_start(); delay(100); to_pos3(0,1,3,4,158,30,30,153,18); to_pos3(0,1,3,4,179,10,10,166,18); to_angle(0,160,10); to_pos1(0,1,40,160,10); to_pos1(0,1,0,168,10); to_angle(3,40,10); to_pos1(3,4,140,23,10); to_pos1(3,4,165,10,10); to_pos3(0,1,3,4,93,93,91,94,18); init_start(); } } /***************************************************************************** 目标角度与当前角度比较，返回比较差值，分段进行等待时间设定 *****************************************************************************/ int compare(int angle1,int angle2) { intk2=0; k2=abs(angle1-angle2); returnk2; } /************************************************************ 这个函数的作用是2个舵机等待时间的分别设定，可以节省比赛所用时间 ************************************************************/ void min_angle1(int n1,int n2,int angle1,int angle2,int time) { intk1=0,k2=0; inta1=0,a2=0; intk3=0,k4=0; intflag=0; if(n1==2|| n1==5) flag=1; k1=pos_select(n1); k2=pos_select(n2); a1=abs(angle1-k1); a2=abs(angle2-k2); k3=min(a1,a2); if(k3==a1) { if(a1<be&& a1!=0) delay(time); elseif(a1==0) { if(a2<be&& a2!=0) delay(time); elseif(a2==0) return; elsedelay(tm); } elsedelay(tm); } else { if(a2<be&& a2!=0) delay(time); elseif(a2==0) { if(a1<be&& a1!=0) delay(time); elseif(a1==0) return; elsedelay(tm); } elsedelay(tm); } } /********************************************************** 作用和上个函数的作用一样 **********************************************************/ |
