涼太郎 中村
a
Dependencies: Locate Move Servo button mbed
Diff: main.cpp
- Revision:
- 21:7df0f8cd2737
- Parent:
- 20:4b8ce61a00b2
diff -r 4b8ce61a00b2 -r 7df0f8cd2737 main.cpp
--- a/main.cpp Fri Sep 16 08:51:16 2016 +0000
+++ b/main.cpp Sat Sep 17 23:23:08 2016 +0000
@@ -21,65 +21,79 @@
int main()
-{
-
+{
+ Serial pc(SERIAL_TX, SERIAL_RX); //pcと通信
int teams=1,phase=0;
teams=teamLED();
int team = 1;
setup(teams);
initmotor(teams);
+
open_arm();
wait(2);
close_arm();
buzzer(1);
wait(1);
buzzer(0);
-
- 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=220,goaly=1200-30+(slidedist)*0;//yへのslidediatを無視
-
+
+ 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=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){
+
+ 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);
+ buzzer(1);
+
+ while(1) {
+ update();
+ }
+ }
+
+ if(phase==14) {
+ int i = 1;
+ while(1)
+ {
+
+ turnrad(-2*PI*3*(i++));
+ wait(5);
}
- if(phase==14){
- while(1){
- turnrad_cw(-2*PI,1);
- turnrad_ccw(0,1);
- }
- }
- if(phase==13){
- nxback300();
+ while(1) {
+ update();
}
-
- if(phase==12){
- while(1){
+ }
+ if(phase==13) {
+ move(-70, -70);
+ }
+
+ if(phase==12) {
+ while(1) {
open_arm();
wait(3);
close_arm();
wait(3);
}
}
-
- if(phase==11){
- while(1){
+
+ if(phase==11) {
+ while(1) {
close_hand();
wait(0.3);
lift();
@@ -88,236 +102,257 @@
wait(2);
}
}
+
+ if(phase==10) {
+ while(1) {
+ move(70,70);
+ update();
+ }
+ }
+ if(phase==9) {
+ while(1) {
+ move(32,30);
+ while(1) {
+ update_np();
+ if(coordinateX()>900) {
+ break;
+ }
+ }
+ pc.printf("start");
+ update();
+ }
+ }
+
//オブジェクト0,1,2回収
- if(phase==0){
-
- pmove2(targetx[0],-60);
- open_hand();
- pmove2(targetx[0],targety[0]);
- close_hand();
- wait(0.3);
+ if(phase==0) {
+
+ 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);
+ 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)なう
+ 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);
//--------------------------------------------------------------------
- open_hand();
-
- pmove(targetx[3]-50,targety[3]);
-
- close_arm();
- close_hand();
- turnrad_cw(PI,team);
+ open_hand();
+ \
+ pmove2(targetx[3]-50,targety[3]);
- pmove2(150,team*(goaly));//ゴール
+ close_arm();
+ close_hand();
+ turnrad_cw(PI,team);
+
+ pmove2(300,team*(goaly));//ゴール
open_arm();
open_hand();
wait(0.3);
-
+
back300();
turnrad_ccw(2*PI,team);
phase=2;
-
- //また縦に回収
- close_arm();
- pmove2(900,team*(1200+30));
+
+ //また縦に回収
+ close_arm();
+ pmove2(900,team*(1200+30));
- open_hand();
- pmove2(targetx[4],targety[4]);
- close_hand();
- wait(0.3);
+ 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);
+ 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(PI,team);
- 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){
+ open_hand();
+ //回収
+ pmove2(targetx[8],targety[8]);
+ //回収
+ close_hand();
+ pmove2(150,team*(600+slidedist));//斜め移動
+
+ turnrad_ccw(PI,team);
+ open_arm();
+ open_hand();
+ back300();
+
+
+ pmove2(600,600);
+ pmove2(targetx[5]-50,targety[5]);
+ close_arm();
+ close_hand();
+ nxback300();
+ pmove2(150,600+slidedist);
+ open_arm();
+ open_hand();
+ back300();
+ pmove2(900-50,50);
+ pmove2(1200-90,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);
buzzer(0);
- wait(1);
+ wait(1);
}
}
-
-
- if(phase==2){
+
+
+ if(phase==2) {
open_arm();
-
+
open_hand();
- pmove2(900,0);
- pmove2(1200-80,0);//オブジェクト
- close_arm();
- close_hand();
- nxback300();
- pmove2(150,900+slidedist);
- turnrad_ccw(PI,team);
- open_arm();
- open_hand();
- back300();
-
- pmove2(600,600);
- pmove2(targetx[5]-80,targety[5]);
- close_arm();
- close_hand();
- nxback300();
- pmove2(150,600+slidedist);
- back300();
-
+ pmove2(900,0);
+ pmove2(1200-80,0);//オブジェクト
+ close_arm();
+ close_hand();
+ nxback300();
+ pmove2(150,900+slidedist);
+ turnrad_ccw(PI,team);
+ open_arm();
+ open_hand();
+ back300();
+
+ pmove2(600,600);
+ pmove2(targetx[5]-80,targety[5]);
+ close_arm();
+ close_hand();
+ nxback300();
+ pmove2(150,600+slidedist);
+ back300();
+
}
-
- if(phase==1){
+
+ if(phase==1) {
- pmove(900,0);
- open_hand();
- pmove2(targetx[8],targety[8]);
- close_hand();
- wait(0.3);
+ 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);
+ 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)なう
-
+ 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){
-
+
+
+ if(phase==3) {
+
- pmove2(targetx[0],-60);
- open_hand();
- pmove2(targetx[0],targety[0]);
- close_hand();
- wait(0.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);
+ 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)なう
+ 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));
+ pmove2(900,team*(1200+30));
- open_hand();
- pmove2(targetx[4],targety[4]);
- close_hand();
- wait(0.3);
+ 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);
+ 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();
+ 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);
+
+ if(phase == 10) {
+ buzzer(1);
turnrad(6*PI);
while(1) update();
}
-
-
-
- if(phase == 8)
- { open_arm();
+
+
+
+ if(phase == 8) {
+ open_arm();
open_hand();
-
+
pmove2(0,1000);
pmove2(600,1200);
pmove2(1200-50,1200);
@@ -328,7 +363,7 @@
open_hand();
open_arm();
back300();
-
+
turnrad_ccw(0,1);
pmove2(1800,1200);
close_hand();
@@ -339,11 +374,11 @@
open_arm();
back300();
}
-
- if(phase == 7)
- { open_arm();
+
+ if(phase == 7) {
+ open_arm();
open_hand();
-
+
pmove2(0,1000);
pmove2(600,1200);
pmove2(1800,1200);
@@ -354,25 +389,26 @@
open_hand();
open_arm();
back300();
-
-
+
+
}
-
- if(phase == 6)
- {buzzer(1);
+
+ if(phase == 6) {
+ buzzer(1);
initbutton();
int command[3];
commandIn(command);
-
+
commandMove(command[0], command[1], command[2]);
}
-
- if(phase == 5)
- {buzzer(1);
+
+ if(phase == 5) {
+ buzzer(1);
initbutton();
int command[3];
commandIn(command);
-
+
commandMoveEnemy(command[0], command[1], command[2]);
}
+
}
\ No newline at end of file