涼太郎 中村 / Mbed 2 deprecated 4thcomp

a

Dependencies:   Locate Move Servo button mbed

Fork of 3rdcompfixstart by 涼太郎 中村

main.cpp@16:a9885a66048f, 2016-09-09 (annotated)

Committer:
choutin
Date:
Fri Sep 09 11:41:27 2016 +0000
Revision:
16:a9885a66048f
Parent:
15:a2cdcb373d3d
Child:
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 15:a2cdcb373d3d 13 const int dist=30;
choutin 15:a2cdcb373d3d 14 const int slidedist=150;
choutin 15:a2cdcb373d3d 15 const int space=100;
choutin 12:456d651ef4f0 16
sakanakuuun 4:1604d599d40f 17 int main()
sakanakuuun 4:1604d599d40f 18 {
choutin 14:0629ab54765c 19 int team=1;
sakanakuuun 4:1604d599d40f 20 setup();
sakanakuuun 4:1604d599d40f 21 initmotor();
sakanakuuun 8:21f40f92215f 22
choutin 14:0629ab54765c 23 int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
choutin 15:a2cdcb373d3d 24 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 15:a2cdcb373d3d 25 int goalx=0,goaly1=1200+150,goaly2=900+slidedist,goaly3=600+slidedist,goaly4=300+slidedist;
choutin 14:0629ab54765c 26
choutin 16:a9885a66048f 27
choutin 16:a9885a66048f 28
choutin 14:0629ab54765c 29 //オブジェクト0,1,2回収
choutin 16:a9885a66048f 30
choutin 14:0629ab54765c 31 pmove(targetx[0],0);
choutin 16:a9885a66048f 32 open_hand();
choutin 14:0629ab54765c 33 pmove(targetx[0],targety[0]);
choutin 16:a9885a66048f 34 close_hand();
choutin 16:a9885a66048f 35 beltup();
choutin 16:a9885a66048f 36 wait(5);
choutin 16:a9885a66048f 37 beltstop();
choutin 16:a9885a66048f 38 open_hand();
choutin 14:0629ab54765c 39 //回収
choutin 14:0629ab54765c 40 pmove(targetx[1],targety[1]);
choutin 16:a9885a66048f 41 close_hand();
choutin 16:a9885a66048f 42 beltup();
choutin 16:a9885a66048f 43 wait(5);
choutin 16:a9885a66048f 44 beltstop();
choutin 16:a9885a66048f 45 open_hand();
choutin 14:0629ab54765c 46 //回収
choutin 14:0629ab54765c 47 pmove(targetx[2],targety[2]);
choutin 14:0629ab54765c 48 //回収
choutin 14:0629ab54765c 49
choutin 15:a2cdcb373d3d 50 pmove(600,1200+slidedist);//次につなげるハブ
choutin 14:0629ab54765c 51
choutin 14:0629ab54765c 52 pmove(goalx,goaly1);
choutin 14:0629ab54765c 53 //リリース
choutin 14:0629ab54765c 54 back300();//(300,1200)なう
choutin 14:0629ab54765c 55 /****************/
choutin 14:0629ab54765c 56
choutin 14:0629ab54765c 57 //オブジェクト3,4回収
choutin 15:a2cdcb373d3d 58 pmove(targetx[3]+space,targety[3]);
choutin 14:0629ab54765c 59 //回収
choutin 15:a2cdcb373d3d 60 pmove(1200+space,900+slidedist);//ハブ
choutin 14:0629ab54765c 61 pmove(targetx[4],targety[4]);
choutin 14:0629ab54765c 62 //回収?
choutin 14:0629ab54765c 63 pmove(goalx,goaly2);
choutin 14:0629ab54765c 64 //リリース
choutin 14:0629ab54765c 65 back300();
choutin 14:0629ab54765c 66 /***************/
choutin 14:0629ab54765c 67
choutin 15:a2cdcb373d3d 68 //オブジェクト5,7回収
choutin 15:a2cdcb373d3d 69 pmove(1200,900+slidedist);//ハブ
choutin 14:0629ab54765c 70 pmove(targetx[5],targety[5]);
choutin 14:0629ab54765c 71 //回収
choutin 15:a2cdcb373d3d 72 pmove(targetx[7],targety[7]);
choutin 14:0629ab54765c 73 //回収?
choutin 15:a2cdcb373d3d 74 pmove(600,0+slidedist);
choutin 14:0629ab54765c 75 //リリース
choutin 15:a2cdcb373d3d 76 pmove(600,1200+slidedist);//ハブ
choutin 15:a2cdcb373d3d 77
choutin 15:a2cdcb373d3d 78 pmove(goalx,goaly1);
choutin 14:0629ab54765c 79 back300();
choutin 14:0629ab54765c 80 /**********************/
choutin 14:0629ab54765c 81
choutin 15:a2cdcb373d3d 82 //オブジェクト6,8回収
choutin 15:a2cdcb373d3d 83 pmove(300,0+slidedist);//ハブ
choutin 15:a2cdcb373d3d 84 pmove(900,0+slidedist);//ハブ
choutin 15:a2cdcb373d3d 85 pmove(targetx[8],targety[8]);
choutin 14:0629ab54765c 86 //回収
choutin 15:a2cdcb373d3d 87 pmove(targetx[6],targety[6]);
choutin 15:a2cdcb373d3d 88 //回収
choutin 15:a2cdcb373d3d 89 pmove(900,1200+slidedist);
choutin 14:0629ab54765c 90 //回収?
choutin 15:a2cdcb373d3d 91 pmove(goalx,goaly1);
choutin 14:0629ab54765c 92 //リリース
choutin 14:0629ab54765c 93 back300();
choutin 14:0629ab54765c 94 /**********************/
choutin 14:0629ab54765c 95
choutin 14:0629ab54765c 96
choutin 14:0629ab54765c 97
choutin 3:56b034c41dc5 98 }