|
/*************************************************************** 庫文件的申明，包括伺服電機控制必須要用到的頭文件MegaServo.h number規(guī)定要用到的舵機的數(shù)目，first是規(guī)定第一個舵機所接的pwm端口 比如說：first=4就是說第一個舵機接的端口是PWM4端口 ***************************************************************/ #include<MegaServo.h> #definenumber 6 /*number設(shè)為6*/ #definefirst 4 /*fitst設(shè)為4*/ MegaServoservo[number]; /*舵機號的申明*/ /******************************************************** 變量的初始化，pos1到pos6的初始化是設(shè)定6個舵機的初始位置 Kaishi為1時開始執(zhí)行的動作，為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*/ /*********************************************** 端口的初始化，用到的函數(shù) .attach是綁定PWM端口 pinMode(a,b)用于規(guī)定數(shù)字口輸入輸出方式，a是開發(fā)板的管腳， b是INPUT或者OUTPUT，必須大寫 ************************************************/ void setup() { inti; for(i=0;i<number;i++) { servo.attach(i+first); /*將伺服電機1-6與PWM端口4-9綁定*/ } pinMode(button,INPUT); /*定義開發(fā)板管腳38(即單片機第50引腳)為輸入*/ } /************************************************ 因為根據(jù)比賽規(guī)則必須要先走3步，而每一步必須要分解成多個子動作， 否則會顯得生硬，下面是第一步的分解動作函數(shù) ************************************************/ void first_step1() { to_pos2(2,4,5,79,90,78,18); /*to_pos2為三個舵機同時運行，轉(zhuǎn)一個角度用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為兩個舵機同時運行*/ } /*************************************************** 第三步分解動作函數(shù) ***************************************************/ 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); } /******************************************************* 第二步分解動作函數(shù) *******************************************************/ 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()函數(shù)是立正 *********************************************************/ 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); /*數(shù)字IO口讀輸入電平函數(shù)，結(jié)果為HIGH或LOW*/ if(ks==LOW) { kaishi=1; } } /************************************************************ 主函數(shù) ************************************************************/ 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次分解動作，如果要合理必須在主函數(shù)中一個一個角度調(diào)用 ********************************************************/ 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次的分解動作，調(diào)角度可按上提方法進行 *********************************************************/ 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(); } } /***************************************************************************** 目標角度與當前角度比較，返回比較差值，分段進行等待時間設(shè)定 *****************************************************************************/ int compare(int angle1,int angle2) { intk2=0; k2=abs(angle1-angle2); returnk2; } /************************************************************ 這個函數(shù)的作用是2個舵機等待時間的分別設(shè)定，可以節(jié)省比賽所用時間 ************************************************************/ 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); } } /********************************************************** 作用和上個函數(shù)的作用一樣 **********************************************************/ |
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
