涼太郎 中村 / Mbed 2 deprecated 4thcomp

a

Dependencies:   Locate Move Servo button mbed

Fork of 3rdcompfixstart by 涼太郎 中村

main.cpp@18:aa8c08f6f474, 2016-09-11 (annotated)

Committer:
choutin
Date:
Sun Sep 11 11:40:17 2016 +0000
Revision:
18:aa8c08f6f474
Parent:
17:5b9fbb6133ad 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:f12d257b587e 1 #include "mbed.h"
choutin 3:56b034c41dc5 2 #include "math.h"
sakanakuuun 4:1604d599d40f 3 #include "locate.h"
sakanakuuun 4:1604d599d40f 4 #include "move.h"
choutin 12:456d651ef4f0 5 #include "servo.h"
choutin 12:456d651ef4f0 6 #include "button.h"
choutin 3:56b034c41dc5 7
choutin 16:a9885a66048f 8 DigitalOut ledg(PC_2); //green
choutin 16:a9885a66048f 9 DigitalOut ledy(PC_3); //yellow
choutin 16:a9885a66048f 10 DigitalOut ledr(PC_0); //red
choutin 16:a9885a66048f 11
choutin 16:a9885a66048f 12
choutin 17:5b9fbb6133ad 13 const int dist=150;//タイヤ、アーム間の距離
choutin 18:aa8c08f6f474 14 const int slidedist=102-20;
choutin 18:aa8c08f6f474 15 const int space=50;//壁やばそうなときのよゆう
choutin 18:aa8c08f6f474 16 const float btime=1.9;
choutin 17:5b9fbb6133ad 17 const float allowdegree=0.2;
choutin 17:5b9fbb6133ad 18 const float allowlength=0.2;
choutin 17:5b9fbb6133ad 19
choutin 17:5b9fbb6133ad 20
sakanakuuun 4:1604d599d40f 21 int main()
sakanakuuun 4:1604d599d40f 22 {
choutin 17:5b9fbb6133ad 23
choutin 18:aa8c08f6f474 24 int teams=1,phase=0;
choutin 18:aa8c08f6f474 25 teams=teamLED();
choutin 18:aa8c08f6f474 26 int team = 1;
choutin 18:aa8c08f6f474 27 setup(teams);
choutin 18:aa8c08f6f474 28 initmotor(teams);
choutin 18:aa8c08f6f474 29 open_arm();
choutin 18:aa8c08f6f474 30 wait(2);
choutin 17:5b9fbb6133ad 31 close_arm();
sakanakuuun 8:21f40f92215f 32
choutin 17:5b9fbb6133ad 33 while(start()){}
choutin 14:0629ab54765c 34 int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
choutin 15:a2cdcb373d3d 35 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) };
choutin 18:aa8c08f6f474 36 int goalx=150,goaly=1200+(slidedist)*0;//yへのslidediatを無視
choutin 14:0629ab54765c 37
choutin 17:5b9fbb6133ad 38 phase=phaseSW();
choutin 17:5b9fbb6133ad 39
choutin 17:5b9fbb6133ad 40 //
choutin 16:a9885a66048f 41
choutin 14:0629ab54765c 42 //オブジェクト0,1,2回収
choutin 17:5b9fbb6133ad 43 if(phase==0){
choutin 18:aa8c08f6f474 44 pmove(targetx[0],0); //傾きをはじめから向くように
choutin 16:a9885a66048f 45 open_hand();
choutin 18:aa8c08f6f474 46 pmove2(targetx[0],targety[0]);
choutin 16:a9885a66048f 47 close_hand();
choutin 18:aa8c08f6f474 48 wait(0.3);
choutin 16:a9885a66048f 49 beltup();
choutin 17:5b9fbb6133ad 50 wait(btime);
choutin 16:a9885a66048f 51 beltstop();
choutin 16:a9885a66048f 52 open_hand();
choutin 14:0629ab54765c 53 //回収
choutin 18:aa8c08f6f474 54 pmove2(targetx[1],targety[1]);
choutin 16:a9885a66048f 55 close_hand();
choutin 18:aa8c08f6f474 56 wait(0.3);
choutin 16:a9885a66048f 57 beltup();
choutin 17:5b9fbb6133ad 58 wait(btime);
choutin 16:a9885a66048f 59 beltstop();
choutin 16:a9885a66048f 60 open_hand();
choutin 14:0629ab54765c 61 //回収
choutin 14:0629ab54765c 62 pmove(targetx[2],targety[2]);
choutin 14:0629ab54765c 63 //回収
choutin 17:5b9fbb6133ad 64 close_hand();
choutin 18:aa8c08f6f474 65 pmove2(goalx,goaly);
choutin 14:0629ab54765c 66 //リリース
choutin 18:aa8c08f6f474 67 turnrad_ccw(PI,team);
choutin 17:5b9fbb6133ad 68 open_arm();
choutin 17:5b9fbb6133ad 69 open_hand();
choutin 18:aa8c08f6f474 70 wait(0.5);
choutin 14:0629ab54765c 71 back300();//(300,1200)なう
choutin 18:aa8c08f6f474 72 turnrad_ccw(2*PI,team);
choutin 18:aa8c08f6f474 73 //--------------------------------------------------------------------
choutin 17:5b9fbb6133ad 74 open_arm();
choutin 17:5b9fbb6133ad 75 pmove(targetx[3],targety[3]);
choutin 17:5b9fbb6133ad 76 close_arm();
choutin 17:5b9fbb6133ad 77 close_hand();
choutin 18:aa8c08f6f474 78 turnrad_cw(PI,team);
choutin 18:aa8c08f6f474 79 /*
choutin 18:aa8c08f6f474 80 open_hanf();
choutin 18:aa8c08f6f474 81 pmove(1800,team*(1200+slidedist));
choutin 18:aa8c08f6f474 82 closehand();
choutin 18:aa8c08f6f474 83 */
choutin 18:aa8c08f6f474 84 pmove(150,team*(1200+slidedist-space));//ゴール
choutin 18:aa8c08f6f474 85
choutin 18:aa8c08f6f474 86 open_arm();
choutin 18:aa8c08f6f474 87 open_hand();
choutin 18:aa8c08f6f474 88 wait(0.3);
choutin 18:aa8c08f6f474 89
choutin 18:aa8c08f6f474 90
choutin 18:aa8c08f6f474 91 back300();
choutin 18:aa8c08f6f474 92 turnrad_ccw(2*PI,team);
choutin 18:aa8c08f6f474 93 phase=2;
choutin 18:aa8c08f6f474 94
choutin 18:aa8c08f6f474 95 //また縦に回収
choutin 18:aa8c08f6f474 96 close_arm();
choutin 18:aa8c08f6f474 97 pmove2(900,team*(1200+slidedist+space));
choutin 18:aa8c08f6f474 98
choutin 18:aa8c08f6f474 99 open_hand();
choutin 18:aa8c08f6f474 100 pmove2(targetx[4],targety[4]);
choutin 18:aa8c08f6f474 101 close_hand();
choutin 18:aa8c08f6f474 102 wait(0.3);
choutin 18:aa8c08f6f474 103 beltup();
choutin 18:aa8c08f6f474 104 wait(btime);
choutin 18:aa8c08f6f474 105 beltstop();
choutin 18:aa8c08f6f474 106 open_hand();
choutin 18:aa8c08f6f474 107 //回収
choutin 18:aa8c08f6f474 108 pmove(targetx[6],targety[6]);
choutin 18:aa8c08f6f474 109 close_hand();
choutin 18:aa8c08f6f474 110 wait(0.3);
choutin 17:5b9fbb6133ad 111 beltup();
choutin 17:5b9fbb6133ad 112 wait(btime);
choutin 17:5b9fbb6133ad 113 beltstop();
choutin 17:5b9fbb6133ad 114 open_hand();
choutin 18:aa8c08f6f474 115 //回収
choutin 18:aa8c08f6f474 116 pmove(targetx[8],targety[8]);
choutin 18:aa8c08f6f474 117 //回収
choutin 18:aa8c08f6f474 118 close_hand();
choutin 18:aa8c08f6f474 119 pmove2(150,team*(900+slidedist));//斜め移動
choutin 17:5b9fbb6133ad 120
choutin 18:aa8c08f6f474 121 turnrad_ccw(PI,team);
choutin 17:5b9fbb6133ad 122 open_arm();
choutin 17:5b9fbb6133ad 123 open_hand();
choutin 14:0629ab54765c 124 back300();
choutin 17:5b9fbb6133ad 125
choutin 18:aa8c08f6f474 126
choutin 18:aa8c08f6f474 127 pmove2(700,(600+slidedist));//ハブ
choutin 18:aa8c08f6f474 128
choutin 18:aa8c08f6f474 129 pmove(targetx[5],targety[5]);
choutin 18:aa8c08f6f474 130 close_arm();
choutin 18:aa8c08f6f474 131 close_hand();
choutin 18:aa8c08f6f474 132 nxback300();
choutin 18:aa8c08f6f474 133
choutin 18:aa8c08f6f474 134 pmove2(150,team*(900+slidedist));
choutin 18:aa8c08f6f474 135 open_arm();
choutin 18:aa8c08f6f474 136 open_hand();
choutin 18:aa8c08f6f474 137
choutin 18:aa8c08f6f474 138 back300();
choutin 18:aa8c08f6f474 139 while(1){
choutin 18:aa8c08f6f474 140 buzzer(1);
choutin 18:aa8c08f6f474 141 wait(0.5);
choutin 18:aa8c08f6f474 142 buzzer(0);
choutin 18:aa8c08f6f474 143 wait(1);
choutin 18:aa8c08f6f474 144 }
choutin 17:5b9fbb6133ad 145 }
choutin 17:5b9fbb6133ad 146
choutin 18:aa8c08f6f474 147
choutin 18:aa8c08f6f474 148 if(phase==1){
choutin 18:aa8c08f6f474 149
choutin 18:aa8c08f6f474 150 pmove(targetx[7],targety[7]);
choutin 18:aa8c08f6f474 151 back300();
choutin 18:aa8c08f6f474 152 pmove2(goalx,goaly);
choutin 18:aa8c08f6f474 153 turnrad_ccw(PI,team);
choutin 18:aa8c08f6f474 154 open_arm();
choutin 18:aa8c08f6f474 155 open_hand();
choutin 18:aa8c08f6f474 156 back300();
choutin 18:aa8c08f6f474 157
choutin 18:aa8c08f6f474 158 pmove2(900,600+slidedist);
choutin 18:aa8c08f6f474 159
choutin 18:aa8c08f6f474 160 pmove(targetx[5],targety[5]);
choutin 18:aa8c08f6f474 161 pmove2(goalx,goaly);
choutin 18:aa8c08f6f474 162 turnrad_ccw(2*PI,team);
choutin 18:aa8c08f6f474 163 open_arm();
choutin 18:aa8c08f6f474 164 open_hand();
choutin 18:aa8c08f6f474 165 back300();
choutin 18:aa8c08f6f474 166
choutin 17:5b9fbb6133ad 167 }
choutin 17:5b9fbb6133ad 168
choutin 18:aa8c08f6f474 169 }