涼太郎 中村
a
Dependencies: Locate Move Servo button mbed
Diff: main.cpp
- Revision:
- 20:4b8ce61a00b2
- Parent:
- 19:e3383efa483b
- Child:
- 21:7df0f8cd2737
diff -r e3383efa483b -r 4b8ce61a00b2 main.cpp
--- a/main.cpp Sun Sep 11 12:05:39 2016 +0000
+++ b/main.cpp Fri Sep 16 08:51:16 2016 +0000
@@ -11,16 +11,18 @@
const int dist=150;//タイヤ、アーム間の距離
-const int slidedist=102-20;
+const int slidedist=0; //102-20;
const int space=50;//壁やばそうなときのよゆう
-const float btime=1.9;
+//btimeはservoに移動
const float allowdegree=0.2;
const float allowlength=0.2;
+const int unit=583;
+
int main()
-{
-
+{
+
int teams=1,phase=0;
teams=teamLED();
int team = 1;
@@ -29,59 +31,102 @@
open_arm();
wait(2);
close_arm();
+ buzzer(1);
+ wait(1);
+ buzzer(0);
- while(start()){}
int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
int targety[9] = { team*(300+slidedist),team*(600+slidedist),team*(900+slidedist),team*(1200+slidedist),team*(900+slidedist),team*(600+slidedist),team*(600+slidedist),team*(0+slidedist),team*(300+slidedist) };
- int goalx=150,goaly=1200+(slidedist)*0;//yへのslidediatを無視
+ int goalx=220,goaly=1200-30+(slidedist)*0;//yへのslidediatを無視
phase=phaseSW();
- //
-
+ if(phase==0){
+ buzzer(1);
+ wait(0.4);
+ buzzer(0);
+ wait(0.2);
+ buzzer(1);
+ wait(0.4);
+ buzzer(0);
+ wait(0.2);
+ buzzer(1);
+ wait(0.4);
+ buzzer(0);}
+
+ while(start()){}
+ if(phase==15){
+ movelength(900);
+ }
+
+ if(phase==14){
+ while(1){
+ turnrad_cw(-2*PI,1);
+ turnrad_ccw(0,1);
+ }
+ }
+ if(phase==13){
+ nxback300();
+ }
+
+ if(phase==12){
+ while(1){
+ open_arm();
+ wait(3);
+ close_arm();
+ wait(3);
+ }
+ }
+
+ if(phase==11){
+ while(1){
+ close_hand();
+ wait(0.3);
+ lift();
+ wait(0.2);
+ open_hand();
+ wait(2);
+ }
+ }
//オブジェクト0,1,2回収
if(phase==0){
- pmove(targetx[0],0); //傾きをはじめから向くように
+
+ pmove2(targetx[0],-60);
open_hand();
pmove2(targetx[0],targety[0]);
close_hand();
wait(0.3);
- beltup();
- wait(btime);
- beltstop();
+ lift();
open_hand();
//回収
pmove2(targetx[1],targety[1]);
close_hand();
wait(0.3);
- beltup();
- wait(btime);
- beltstop();
+ lift();
open_hand();
//回収
- pmove(targetx[2],targety[2]);
+ pmove2(targetx[2],targety[2]);
//回収
close_hand();
- pmove2(goalx,goaly);
+ pmove2(goalx,goaly-40);
//リリース
turnrad_ccw(PI,team);
open_arm();
open_hand();
wait(0.5);
+
back300();//(300,1200)なう
turnrad_ccw(2*PI,team);
//--------------------------------------------------------------------
- open_arm();
- pmove(targetx[3],targety[3]);
+ open_hand();
+
+ pmove(targetx[3]-50,targety[3]);
+
close_arm();
close_hand();
turnrad_cw(PI,team);
- /*
- open_hanf();
- pmove(1800,team*(1200+slidedist));
- closehand();
- */
- pmove(150,team*(1200+slidedist-space));//ゴール
+
+ pmove2(150,team*(goaly));//ゴール
open_arm();
open_hand();
@@ -94,48 +139,52 @@
//また縦に回収
close_arm();
- pmove2(900,team*(1200+slidedist+space));
+ pmove2(900,team*(1200+30));
open_hand();
pmove2(targetx[4],targety[4]);
close_hand();
wait(0.3);
- beltup();
- wait(btime);
- beltstop();
+ lift();
open_hand();
//回収
- pmove(targetx[6],targety[6]);
+ pmove2(targetx[6],targety[6]);
close_hand();
wait(0.3);
- beltup();
- wait(btime);
- beltstop();
+ lift();
open_hand();
//回収
- pmove(targetx[8],targety[8]);
+ pmove2(targetx[8],targety[8]);
//回収
close_hand();
- pmove2(150,team*(900+slidedist));//斜め移動
+ pmove2(150,team*(600+slidedist));//斜め移動
turnrad_ccw(PI,team);
open_arm();
open_hand();
back300();
-
-
- pmove2(700,(600+slidedist));//ハブ
-
- pmove(targetx[5],targety[5]);
- close_arm();
- close_hand();
- nxback300();
-
- pmove2(150,team*(900+slidedist));
- open_arm();
- open_hand();
-
- back300();
+
+
+ pmove2(600,600);
+ pmove2(targetx[5],targety[5]);
+ close_arm();
+ close_hand();
+ nxback300();
+ pmove2(150,600+slidedist);
+ open_arm();
+ open_hand();
+ back300();
+ pmove2(900,50);
+ pmove2(1200-50,0);
+ close_arm();
+ close_hand();
+ nxback300();
+ pmove2(150,900+slidedist);
+ turnrad_ccw(PI,team);
+ open_arm();
+ open_hand();
+ back300();
+
while(1){
buzzer(1);
wait(0.5);
@@ -145,10 +194,12 @@
}
- if(phase==1){
+ if(phase==2){
open_arm();
+
open_hand();
- pmove(targetx[7],targety[7]);
+ pmove2(900,0);
+ pmove2(1200-80,0);//オブジェクト
close_arm();
close_hand();
nxback300();
@@ -158,8 +209,170 @@
open_hand();
back300();
- pmove2(2100,1200);
+ pmove2(600,600);
+ pmove2(targetx[5]-80,targety[5]);
+ close_arm();
+ close_hand();
+ nxback300();
+ pmove2(150,600+slidedist);
+ back300();
+
+ }
+
+ if(phase==1){
+
+ pmove(900,0);
+ open_hand();
+ pmove2(targetx[8],targety[8]);
+ close_hand();
+ wait(0.3);
+ lift();
+ open_hand();
+ //回収
+ pmove2(targetx[6],targety[6]);
+ close_hand();
+ wait(0.3);
+ lift();
+ open_hand();
+ //回収
+ pmove(targetx[4],targety[4]);
+ //回収
+ close_hand();
+ pmove2(goalx,goaly-40);
+ //リリース
+ turnrad_ccw(PI,team);
+ open_arm();
+ open_hand();
+ wait(0.5);
+ back300();//(300,1200)なう
}
+
+ if(phase==3){
+
+
+ pmove2(targetx[0],-60);
+ open_hand();
+ pmove2(targetx[0],targety[0]);
+ close_hand();
+ wait(0.3);
+ lift();
+ open_hand();
+ //回収
+ pmove2(targetx[1],targety[1]);
+ close_hand();
+ wait(0.3);
+ lift();
+ open_hand();
+ //回収
+ pmove2(targetx[2],targety[2]);
+ //回収
+ close_hand();
+ pmove2(goalx,goaly-40);
+ //リリース
+ turnrad_ccw(PI,team);
+ open_arm();
+ open_hand();
+ wait(0.5);
+ back300();//(300,1200)なう
+ turnrad_ccw(2*PI,team);
+
+
+ close_arm();
+ pmove2(900,team*(1200+30));
+
+ open_hand();
+ pmove2(targetx[4],targety[4]);
+ close_hand();
+ wait(0.3);
+ lift();
+ open_hand();
+ //回収
+ pmove2(targetx[6],targety[6]);
+ close_hand();
+ wait(0.3);
+ lift();
+ open_hand();
+ //回収
+ pmove2(targetx[8],targety[8]);
+ //回収
+ close_hand();
+ pmove2(150,team*(600+slidedist));//斜め移動
+
+ turnrad_ccw((-1)*PI,team);
+ open_arm();
+ open_hand();
+ back300();
+ }
+
+ if(phase == 10)
+ {buzzer(1);
+ turnrad(6*PI);
+ while(1) update();
+ }
+
+
+
+ if(phase == 8)
+ { open_arm();
+ open_hand();
+
+ pmove2(0,1000);
+ pmove2(600,1200);
+ pmove2(1200-50,1200);
+ close_hand();
+ close_arm();
+ turnrad_cw((-1)*PI,1);
+ pmove2(150,1200);
+ open_hand();
+ open_arm();
+ back300();
+
+ turnrad_ccw(0,1);
+ pmove2(1800,1200);
+ close_hand();
+ close_arm();
+ turnrad_cw((-1)*PI,1);
+ pmove2(150,1200);
+ open_hand();
+ open_arm();
+ back300();
+ }
+
+ if(phase == 7)
+ { open_arm();
+ open_hand();
+
+ pmove2(0,1000);
+ pmove2(600,1200);
+ pmove2(1800,1200);
+ close_hand();
+ close_arm();
+ turnrad_cw((-1)*PI,1);
+ pmove2(150,1200);
+ open_hand();
+ open_arm();
+ back300();
+
+
+ }
+
+ if(phase == 6)
+ {buzzer(1);
+ initbutton();
+ int command[3];
+ commandIn(command);
+
+ commandMove(command[0], command[1], command[2]);
+ }
+
+ if(phase == 5)
+ {buzzer(1);
+ initbutton();
+ int command[3];
+ commandIn(command);
+
+ commandMoveEnemy(command[0], command[1], command[2]);
+ }
}
\ No newline at end of file