涼太郎 中村 / Mbed 2 deprecated 6th33222_copy

a

Dependencies:   Locate Move Servo button mbed

Fork of 4thcomp by 涼太郎 中村

main.cpp@20:4b8ce61a00b2, 2016-09-16 (annotated)

Committer:
choutin
Date:
Fri Sep 16 08:51:16 2016 +0000
Revision:
20:4b8ce61a00b2
Parent:
19:e3383efa483b
Child:
21:7df0f8cd2737
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 20:4b8ce61a00b2 14 const int slidedist=0; //102-20;
choutin 18:aa8c08f6f474 15 const int space=50;//壁やばそうなときのよゆう
choutin 20:4b8ce61a00b2 16 //btimeはservoに移動
choutin 17:5b9fbb6133ad 17 const float allowdegree=0.2;
choutin 17:5b9fbb6133ad 18 const float allowlength=0.2;
choutin 17:5b9fbb6133ad 19
choutin 20:4b8ce61a00b2 20 const int unit=583;
choutin 20:4b8ce61a00b2 21
choutin 17:5b9fbb6133ad 22
sakanakuuun 4:1604d599d40f 23 int main()
choutin 20:4b8ce61a00b2 24 {
choutin 20:4b8ce61a00b2 25
choutin 18:aa8c08f6f474 26 int teams=1,phase=0;
choutin 18:aa8c08f6f474 27 teams=teamLED();
choutin 18:aa8c08f6f474 28 int team = 1;
choutin 18:aa8c08f6f474 29 setup(teams);
choutin 18:aa8c08f6f474 30 initmotor(teams);
choutin 18:aa8c08f6f474 31 open_arm();
choutin 18:aa8c08f6f474 32 wait(2);
choutin 17:5b9fbb6133ad 33 close_arm();
choutin 20:4b8ce61a00b2 34 buzzer(1);
choutin 20:4b8ce61a00b2 35 wait(1);
choutin 20:4b8ce61a00b2 36 buzzer(0);
sakanakuuun 8:21f40f92215f 37
choutin 14:0629ab54765c 38 int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
choutin 15:a2cdcb373d3d 39 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 20:4b8ce61a00b2 40 int goalx=220,goaly=1200-30+(slidedist)*0;//yへのslidediatを無視
choutin 14:0629ab54765c 41
choutin 17:5b9fbb6133ad 42 phase=phaseSW();
choutin 17:5b9fbb6133ad 43
choutin 20:4b8ce61a00b2 44 if(phase==0){
choutin 20:4b8ce61a00b2 45 buzzer(1);
choutin 20:4b8ce61a00b2 46 wait(0.4);
choutin 20:4b8ce61a00b2 47 buzzer(0);
choutin 20:4b8ce61a00b2 48 wait(0.2);
choutin 20:4b8ce61a00b2 49 buzzer(1);
choutin 20:4b8ce61a00b2 50 wait(0.4);
choutin 20:4b8ce61a00b2 51 buzzer(0);
choutin 20:4b8ce61a00b2 52 wait(0.2);
choutin 20:4b8ce61a00b2 53 buzzer(1);
choutin 20:4b8ce61a00b2 54 wait(0.4);
choutin 20:4b8ce61a00b2 55 buzzer(0);}
choutin 20:4b8ce61a00b2 56
choutin 20:4b8ce61a00b2 57 while(start()){}
choutin 20:4b8ce61a00b2 58 if(phase==15){
choutin 20:4b8ce61a00b2 59 movelength(900);
choutin 20:4b8ce61a00b2 60 }
choutin 20:4b8ce61a00b2 61
choutin 20:4b8ce61a00b2 62 if(phase==14){
choutin 20:4b8ce61a00b2 63 while(1){
choutin 20:4b8ce61a00b2 64 turnrad_cw(-2*PI,1);
choutin 20:4b8ce61a00b2 65 turnrad_ccw(0,1);
choutin 20:4b8ce61a00b2 66 }
choutin 20:4b8ce61a00b2 67 }
choutin 20:4b8ce61a00b2 68 if(phase==13){
choutin 20:4b8ce61a00b2 69 nxback300();
choutin 20:4b8ce61a00b2 70 }
choutin 20:4b8ce61a00b2 71
choutin 20:4b8ce61a00b2 72 if(phase==12){
choutin 20:4b8ce61a00b2 73 while(1){
choutin 20:4b8ce61a00b2 74 open_arm();
choutin 20:4b8ce61a00b2 75 wait(3);
choutin 20:4b8ce61a00b2 76 close_arm();
choutin 20:4b8ce61a00b2 77 wait(3);
choutin 20:4b8ce61a00b2 78 }
choutin 20:4b8ce61a00b2 79 }
choutin 20:4b8ce61a00b2 80
choutin 20:4b8ce61a00b2 81 if(phase==11){
choutin 20:4b8ce61a00b2 82 while(1){
choutin 20:4b8ce61a00b2 83 close_hand();
choutin 20:4b8ce61a00b2 84 wait(0.3);
choutin 20:4b8ce61a00b2 85 lift();
choutin 20:4b8ce61a00b2 86 wait(0.2);
choutin 20:4b8ce61a00b2 87 open_hand();
choutin 20:4b8ce61a00b2 88 wait(2);
choutin 20:4b8ce61a00b2 89 }
choutin 20:4b8ce61a00b2 90 }
choutin 14:0629ab54765c 91 //オブジェクト0,1,2回収
choutin 17:5b9fbb6133ad 92 if(phase==0){
choutin 20:4b8ce61a00b2 93
choutin 20:4b8ce61a00b2 94 pmove2(targetx[0],-60);
choutin 16:a9885a66048f 95 open_hand();
choutin 18:aa8c08f6f474 96 pmove2(targetx[0],targety[0]);
choutin 16:a9885a66048f 97 close_hand();
choutin 18:aa8c08f6f474 98 wait(0.3);
choutin 20:4b8ce61a00b2 99 lift();
choutin 16:a9885a66048f 100 open_hand();
choutin 14:0629ab54765c 101 //回収
choutin 18:aa8c08f6f474 102 pmove2(targetx[1],targety[1]);
choutin 16:a9885a66048f 103 close_hand();
choutin 18:aa8c08f6f474 104 wait(0.3);
choutin 20:4b8ce61a00b2 105 lift();
choutin 16:a9885a66048f 106 open_hand();
choutin 14:0629ab54765c 107 //回収
choutin 20:4b8ce61a00b2 108 pmove2(targetx[2],targety[2]);
choutin 14:0629ab54765c 109 //回収
choutin 17:5b9fbb6133ad 110 close_hand();
choutin 20:4b8ce61a00b2 111 pmove2(goalx,goaly-40);
choutin 14:0629ab54765c 112 //リリース
choutin 18:aa8c08f6f474 113 turnrad_ccw(PI,team);
choutin 17:5b9fbb6133ad 114 open_arm();
choutin 17:5b9fbb6133ad 115 open_hand();
choutin 18:aa8c08f6f474 116 wait(0.5);
choutin 20:4b8ce61a00b2 117
choutin 14:0629ab54765c 118 back300();//(300,1200)なう
choutin 18:aa8c08f6f474 119 turnrad_ccw(2*PI,team);
choutin 18:aa8c08f6f474 120 //--------------------------------------------------------------------
choutin 20:4b8ce61a00b2 121 open_hand();
choutin 20:4b8ce61a00b2 122
choutin 20:4b8ce61a00b2 123 pmove(targetx[3]-50,targety[3]);
choutin 20:4b8ce61a00b2 124
choutin 17:5b9fbb6133ad 125 close_arm();
choutin 17:5b9fbb6133ad 126 close_hand();
choutin 18:aa8c08f6f474 127 turnrad_cw(PI,team);
choutin 20:4b8ce61a00b2 128
choutin 20:4b8ce61a00b2 129 pmove2(150,team*(goaly));//ゴール
choutin 18:aa8c08f6f474 130
choutin 18:aa8c08f6f474 131 open_arm();
choutin 18:aa8c08f6f474 132 open_hand();
choutin 18:aa8c08f6f474 133 wait(0.3);
choutin 18:aa8c08f6f474 134
choutin 18:aa8c08f6f474 135
choutin 18:aa8c08f6f474 136 back300();
choutin 18:aa8c08f6f474 137 turnrad_ccw(2*PI,team);
choutin 18:aa8c08f6f474 138 phase=2;
choutin 18:aa8c08f6f474 139
choutin 18:aa8c08f6f474 140 //また縦に回収
choutin 18:aa8c08f6f474 141 close_arm();
choutin 20:4b8ce61a00b2 142 pmove2(900,team*(1200+30));
choutin 18:aa8c08f6f474 143
choutin 18:aa8c08f6f474 144 open_hand();
choutin 18:aa8c08f6f474 145 pmove2(targetx[4],targety[4]);
choutin 18:aa8c08f6f474 146 close_hand();
choutin 18:aa8c08f6f474 147 wait(0.3);
choutin 20:4b8ce61a00b2 148 lift();
choutin 18:aa8c08f6f474 149 open_hand();
choutin 18:aa8c08f6f474 150 //回収
choutin 20:4b8ce61a00b2 151 pmove2(targetx[6],targety[6]);
choutin 18:aa8c08f6f474 152 close_hand();
choutin 18:aa8c08f6f474 153 wait(0.3);
choutin 20:4b8ce61a00b2 154 lift();
choutin 17:5b9fbb6133ad 155 open_hand();
choutin 18:aa8c08f6f474 156 //回収
choutin 20:4b8ce61a00b2 157 pmove2(targetx[8],targety[8]);
choutin 18:aa8c08f6f474 158 //回収
choutin 18:aa8c08f6f474 159 close_hand();
choutin 20:4b8ce61a00b2 160 pmove2(150,team*(600+slidedist));//斜め移動
choutin 17:5b9fbb6133ad 161
choutin 18:aa8c08f6f474 162 turnrad_ccw(PI,team);
choutin 17:5b9fbb6133ad 163 open_arm();
choutin 17:5b9fbb6133ad 164 open_hand();
choutin 14:0629ab54765c 165 back300();
choutin 20:4b8ce61a00b2 166
choutin 20:4b8ce61a00b2 167
choutin 20:4b8ce61a00b2 168 pmove2(600,600);
choutin 20:4b8ce61a00b2 169 pmove2(targetx[5],targety[5]);
choutin 20:4b8ce61a00b2 170 close_arm();
choutin 20:4b8ce61a00b2 171 close_hand();
choutin 20:4b8ce61a00b2 172 nxback300();
choutin 20:4b8ce61a00b2 173 pmove2(150,600+slidedist);
choutin 20:4b8ce61a00b2 174 open_arm();
choutin 20:4b8ce61a00b2 175 open_hand();
choutin 20:4b8ce61a00b2 176 back300();
choutin 20:4b8ce61a00b2 177 pmove2(900,50);
choutin 20:4b8ce61a00b2 178 pmove2(1200-50,0);
choutin 20:4b8ce61a00b2 179 close_arm();
choutin 20:4b8ce61a00b2 180 close_hand();
choutin 20:4b8ce61a00b2 181 nxback300();
choutin 20:4b8ce61a00b2 182 pmove2(150,900+slidedist);
choutin 20:4b8ce61a00b2 183 turnrad_ccw(PI,team);
choutin 20:4b8ce61a00b2 184 open_arm();
choutin 20:4b8ce61a00b2 185 open_hand();
choutin 20:4b8ce61a00b2 186 back300();
choutin 20:4b8ce61a00b2 187
choutin 18:aa8c08f6f474 188 while(1){
choutin 18:aa8c08f6f474 189 buzzer(1);
choutin 18:aa8c08f6f474 190 wait(0.5);
choutin 18:aa8c08f6f474 191 buzzer(0);
choutin 18:aa8c08f6f474 192 wait(1);
choutin 18:aa8c08f6f474 193 }
choutin 17:5b9fbb6133ad 194 }
choutin 17:5b9fbb6133ad 195
choutin 18:aa8c08f6f474 196
choutin 20:4b8ce61a00b2 197 if(phase==2){
choutin 19:e3383efa483b 198 open_arm();
choutin 20:4b8ce61a00b2 199
choutin 19:e3383efa483b 200 open_hand();
choutin 20:4b8ce61a00b2 201 pmove2(900,0);
choutin 20:4b8ce61a00b2 202 pmove2(1200-80,0);//オブジェクト
choutin 19:e3383efa483b 203 close_arm();
choutin 19:e3383efa483b 204 close_hand();
choutin 19:e3383efa483b 205 nxback300();
choutin 19:e3383efa483b 206 pmove2(150,900+slidedist);
choutin 18:aa8c08f6f474 207 turnrad_ccw(PI,team);
choutin 18:aa8c08f6f474 208 open_arm();
choutin 18:aa8c08f6f474 209 open_hand();
choutin 18:aa8c08f6f474 210 back300();
choutin 18:aa8c08f6f474 211
choutin 20:4b8ce61a00b2 212 pmove2(600,600);
choutin 20:4b8ce61a00b2 213 pmove2(targetx[5]-80,targety[5]);
choutin 20:4b8ce61a00b2 214 close_arm();
choutin 20:4b8ce61a00b2 215 close_hand();
choutin 20:4b8ce61a00b2 216 nxback300();
choutin 20:4b8ce61a00b2 217 pmove2(150,600+slidedist);
choutin 20:4b8ce61a00b2 218 back300();
choutin 20:4b8ce61a00b2 219
choutin 20:4b8ce61a00b2 220 }
choutin 20:4b8ce61a00b2 221
choutin 20:4b8ce61a00b2 222 if(phase==1){
choutin 20:4b8ce61a00b2 223
choutin 20:4b8ce61a00b2 224 pmove(900,0);
choutin 20:4b8ce61a00b2 225 open_hand();
choutin 20:4b8ce61a00b2 226 pmove2(targetx[8],targety[8]);
choutin 20:4b8ce61a00b2 227 close_hand();
choutin 20:4b8ce61a00b2 228 wait(0.3);
choutin 20:4b8ce61a00b2 229 lift();
choutin 20:4b8ce61a00b2 230 open_hand();
choutin 20:4b8ce61a00b2 231 //回収
choutin 20:4b8ce61a00b2 232 pmove2(targetx[6],targety[6]);
choutin 20:4b8ce61a00b2 233 close_hand();
choutin 20:4b8ce61a00b2 234 wait(0.3);
choutin 20:4b8ce61a00b2 235 lift();
choutin 20:4b8ce61a00b2 236 open_hand();
choutin 20:4b8ce61a00b2 237 //回収
choutin 20:4b8ce61a00b2 238 pmove(targetx[4],targety[4]);
choutin 20:4b8ce61a00b2 239 //回収
choutin 20:4b8ce61a00b2 240 close_hand();
choutin 20:4b8ce61a00b2 241 pmove2(goalx,goaly-40);
choutin 20:4b8ce61a00b2 242 //リリース
choutin 20:4b8ce61a00b2 243 turnrad_ccw(PI,team);
choutin 20:4b8ce61a00b2 244 open_arm();
choutin 20:4b8ce61a00b2 245 open_hand();
choutin 20:4b8ce61a00b2 246 wait(0.5);
choutin 20:4b8ce61a00b2 247 back300();//(300,1200)なう
choutin 18:aa8c08f6f474 248
choutin 17:5b9fbb6133ad 249 }
choutin 17:5b9fbb6133ad 250
choutin 20:4b8ce61a00b2 251
choutin 20:4b8ce61a00b2 252 if(phase==3){
choutin 20:4b8ce61a00b2 253
choutin 20:4b8ce61a00b2 254
choutin 20:4b8ce61a00b2 255 pmove2(targetx[0],-60);
choutin 20:4b8ce61a00b2 256 open_hand();
choutin 20:4b8ce61a00b2 257 pmove2(targetx[0],targety[0]);
choutin 20:4b8ce61a00b2 258 close_hand();
choutin 20:4b8ce61a00b2 259 wait(0.3);
choutin 20:4b8ce61a00b2 260 lift();
choutin 20:4b8ce61a00b2 261 open_hand();
choutin 20:4b8ce61a00b2 262 //回収
choutin 20:4b8ce61a00b2 263 pmove2(targetx[1],targety[1]);
choutin 20:4b8ce61a00b2 264 close_hand();
choutin 20:4b8ce61a00b2 265 wait(0.3);
choutin 20:4b8ce61a00b2 266 lift();
choutin 20:4b8ce61a00b2 267 open_hand();
choutin 20:4b8ce61a00b2 268 //回収
choutin 20:4b8ce61a00b2 269 pmove2(targetx[2],targety[2]);
choutin 20:4b8ce61a00b2 270 //回収
choutin 20:4b8ce61a00b2 271 close_hand();
choutin 20:4b8ce61a00b2 272 pmove2(goalx,goaly-40);
choutin 20:4b8ce61a00b2 273 //リリース
choutin 20:4b8ce61a00b2 274 turnrad_ccw(PI,team);
choutin 20:4b8ce61a00b2 275 open_arm();
choutin 20:4b8ce61a00b2 276 open_hand();
choutin 20:4b8ce61a00b2 277 wait(0.5);
choutin 20:4b8ce61a00b2 278 back300();//(300,1200)なう
choutin 20:4b8ce61a00b2 279 turnrad_ccw(2*PI,team);
choutin 20:4b8ce61a00b2 280
choutin 20:4b8ce61a00b2 281
choutin 20:4b8ce61a00b2 282 close_arm();
choutin 20:4b8ce61a00b2 283 pmove2(900,team*(1200+30));
choutin 20:4b8ce61a00b2 284
choutin 20:4b8ce61a00b2 285 open_hand();
choutin 20:4b8ce61a00b2 286 pmove2(targetx[4],targety[4]);
choutin 20:4b8ce61a00b2 287 close_hand();
choutin 20:4b8ce61a00b2 288 wait(0.3);
choutin 20:4b8ce61a00b2 289 lift();
choutin 20:4b8ce61a00b2 290 open_hand();
choutin 20:4b8ce61a00b2 291 //回収
choutin 20:4b8ce61a00b2 292 pmove2(targetx[6],targety[6]);
choutin 20:4b8ce61a00b2 293 close_hand();
choutin 20:4b8ce61a00b2 294 wait(0.3);
choutin 20:4b8ce61a00b2 295 lift();
choutin 20:4b8ce61a00b2 296 open_hand();
choutin 20:4b8ce61a00b2 297 //回収
choutin 20:4b8ce61a00b2 298 pmove2(targetx[8],targety[8]);
choutin 20:4b8ce61a00b2 299 //回収
choutin 20:4b8ce61a00b2 300 close_hand();
choutin 20:4b8ce61a00b2 301 pmove2(150,team*(600+slidedist));//斜め移動
choutin 20:4b8ce61a00b2 302
choutin 20:4b8ce61a00b2 303 turnrad_ccw((-1)*PI,team);
choutin 20:4b8ce61a00b2 304 open_arm();
choutin 20:4b8ce61a00b2 305 open_hand();
choutin 20:4b8ce61a00b2 306 back300();
choutin 20:4b8ce61a00b2 307 }
choutin 20:4b8ce61a00b2 308
choutin 20:4b8ce61a00b2 309 if(phase == 10)
choutin 20:4b8ce61a00b2 310 {buzzer(1);
choutin 20:4b8ce61a00b2 311 turnrad(6*PI);
choutin 20:4b8ce61a00b2 312 while(1) update();
choutin 20:4b8ce61a00b2 313 }
choutin 20:4b8ce61a00b2 314
choutin 20:4b8ce61a00b2 315
choutin 20:4b8ce61a00b2 316
choutin 20:4b8ce61a00b2 317 if(phase == 8)
choutin 20:4b8ce61a00b2 318 { open_arm();
choutin 20:4b8ce61a00b2 319 open_hand();
choutin 20:4b8ce61a00b2 320
choutin 20:4b8ce61a00b2 321 pmove2(0,1000);
choutin 20:4b8ce61a00b2 322 pmove2(600,1200);
choutin 20:4b8ce61a00b2 323 pmove2(1200-50,1200);
choutin 20:4b8ce61a00b2 324 close_hand();
choutin 20:4b8ce61a00b2 325 close_arm();
choutin 20:4b8ce61a00b2 326 turnrad_cw((-1)*PI,1);
choutin 20:4b8ce61a00b2 327 pmove2(150,1200);
choutin 20:4b8ce61a00b2 328 open_hand();
choutin 20:4b8ce61a00b2 329 open_arm();
choutin 20:4b8ce61a00b2 330 back300();
choutin 20:4b8ce61a00b2 331
choutin 20:4b8ce61a00b2 332 turnrad_ccw(0,1);
choutin 20:4b8ce61a00b2 333 pmove2(1800,1200);
choutin 20:4b8ce61a00b2 334 close_hand();
choutin 20:4b8ce61a00b2 335 close_arm();
choutin 20:4b8ce61a00b2 336 turnrad_cw((-1)*PI,1);
choutin 20:4b8ce61a00b2 337 pmove2(150,1200);
choutin 20:4b8ce61a00b2 338 open_hand();
choutin 20:4b8ce61a00b2 339 open_arm();
choutin 20:4b8ce61a00b2 340 back300();
choutin 20:4b8ce61a00b2 341 }
choutin 20:4b8ce61a00b2 342
choutin 20:4b8ce61a00b2 343 if(phase == 7)
choutin 20:4b8ce61a00b2 344 { open_arm();
choutin 20:4b8ce61a00b2 345 open_hand();
choutin 20:4b8ce61a00b2 346
choutin 20:4b8ce61a00b2 347 pmove2(0,1000);
choutin 20:4b8ce61a00b2 348 pmove2(600,1200);
choutin 20:4b8ce61a00b2 349 pmove2(1800,1200);
choutin 20:4b8ce61a00b2 350 close_hand();
choutin 20:4b8ce61a00b2 351 close_arm();
choutin 20:4b8ce61a00b2 352 turnrad_cw((-1)*PI,1);
choutin 20:4b8ce61a00b2 353 pmove2(150,1200);
choutin 20:4b8ce61a00b2 354 open_hand();
choutin 20:4b8ce61a00b2 355 open_arm();
choutin 20:4b8ce61a00b2 356 back300();
choutin 20:4b8ce61a00b2 357
choutin 20:4b8ce61a00b2 358
choutin 20:4b8ce61a00b2 359 }
choutin 20:4b8ce61a00b2 360
choutin 20:4b8ce61a00b2 361 if(phase == 6)
choutin 20:4b8ce61a00b2 362 {buzzer(1);
choutin 20:4b8ce61a00b2 363 initbutton();
choutin 20:4b8ce61a00b2 364 int command[3];
choutin 20:4b8ce61a00b2 365 commandIn(command);
choutin 20:4b8ce61a00b2 366
choutin 20:4b8ce61a00b2 367 commandMove(command[0], command[1], command[2]);
choutin 20:4b8ce61a00b2 368 }
choutin 20:4b8ce61a00b2 369
choutin 20:4b8ce61a00b2 370 if(phase == 5)
choutin 20:4b8ce61a00b2 371 {buzzer(1);
choutin 20:4b8ce61a00b2 372 initbutton();
choutin 20:4b8ce61a00b2 373 int command[3];
choutin 20:4b8ce61a00b2 374 commandIn(command);
choutin 20:4b8ce61a00b2 375
choutin 20:4b8ce61a00b2 376 commandMoveEnemy(command[0], command[1], command[2]);
choutin 20:4b8ce61a00b2 377 }
choutin 18:aa8c08f6f474 378 }