涼太郎 中村 / Mbed 2 deprecated 6th33222_copy

a

Dependencies:   Locate Move Servo button mbed

Fork of 4thcomp by 涼太郎 中村

main.cpp@17:5b9fbb6133ad, 2016-09-10 (annotated)

Committer:
choutin
Date:
Sat Sep 10 13:32:25 2016 +0000
Revision:
17:5b9fbb6133ad
Parent:
16:a9885a66048f
Child:
18:aa8c08f6f474
?

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 17:5b9fbb6133ad 14 const int slidedist=75;
choutin 17:5b9fbb6133ad 15 const int space=0;//壁やばそうなときのよゆう
choutin 17:5b9fbb6133ad 16 const float btime=1.7;
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
choutin 17:5b9fbb6133ad 21 int main(){
choutin 17:5b9fbb6133ad 22 setup();
choutin 17:5b9fbb6133ad 23 initmotor();
choutin 17:5b9fbb6133ad 24 close_arm();
choutin 17:5b9fbb6133ad 25 while(1){
choutin 17:5b9fbb6133ad 26 pmove(600,0);
choutin 17:5b9fbb6133ad 27 pmove2(300,600);
choutin 17:5b9fbb6133ad 28 pmove2(0,0);
choutin 17:5b9fbb6133ad 29 }
choutin 17:5b9fbb6133ad 30
choutin 17:5b9fbb6133ad 31 }
choutin 17:5b9fbb6133ad 32
choutin 17:5b9fbb6133ad 33 /*
choutin 12:456d651ef4f0 34
sakanakuuun 4:1604d599d40f 35 int main()
sakanakuuun 4:1604d599d40f 36 {
choutin 17:5b9fbb6133ad 37
choutin 17:5b9fbb6133ad 38 int team=1,phase=0;
choutin 17:5b9fbb6133ad 39 team=teamLED();
sakanakuuun 4:1604d599d40f 40 setup();
sakanakuuun 4:1604d599d40f 41 initmotor();
choutin 17:5b9fbb6133ad 42 close_arm();
sakanakuuun 8:21f40f92215f 43
choutin 17:5b9fbb6133ad 44 while(start()){}
choutin 14:0629ab54765c 45 int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
choutin 15:a2cdcb373d3d 46 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 47 int goalx=0,goaly1=1200+150,goaly2=900+slidedist,goaly3=600+slidedist,goaly4=300+slidedist;
choutin 14:0629ab54765c 48
choutin 17:5b9fbb6133ad 49 phase=phaseSW();
choutin 17:5b9fbb6133ad 50
choutin 17:5b9fbb6133ad 51 //phaseごとの初期位置修正
choutin 16:a9885a66048f 52
choutin 17:5b9fbb6133ad 53
choutin 17:5b9fbb6133ad 54 //
choutin 16:a9885a66048f 55
choutin 14:0629ab54765c 56 //オブジェクト0,1,2回収
choutin 17:5b9fbb6133ad 57 if(phase==0){
choutin 14:0629ab54765c 58 pmove(targetx[0],0);
choutin 16:a9885a66048f 59 open_hand();
choutin 14:0629ab54765c 60 pmove(targetx[0],targety[0]);
choutin 16:a9885a66048f 61 close_hand();
choutin 16:a9885a66048f 62 beltup();
choutin 17:5b9fbb6133ad 63 wait(btime);
choutin 16:a9885a66048f 64 beltstop();
choutin 16:a9885a66048f 65 open_hand();
choutin 14:0629ab54765c 66 //回収
choutin 14:0629ab54765c 67 pmove(targetx[1],targety[1]);
choutin 16:a9885a66048f 68 close_hand();
choutin 16:a9885a66048f 69 beltup();
choutin 17:5b9fbb6133ad 70 wait(btime);
choutin 16:a9885a66048f 71 beltstop();
choutin 16:a9885a66048f 72 open_hand();
choutin 14:0629ab54765c 73 //回収
choutin 14:0629ab54765c 74 pmove(targetx[2],targety[2]);
choutin 14:0629ab54765c 75 //回収
choutin 17:5b9fbb6133ad 76 close_hand();
choutin 17:5b9fbb6133ad 77 pmove(600,1200+slidedist-space*3);//次につなげるハブ
choutin 14:0629ab54765c 78 pmove(goalx,goaly1);
choutin 14:0629ab54765c 79 //リリース
choutin 17:5b9fbb6133ad 80 open_arm();
choutin 17:5b9fbb6133ad 81 open_hand();
choutin 14:0629ab54765c 82 back300();//(300,1200)なう
choutin 17:5b9fbb6133ad 83 turnrad_ccw(2*PI);
choutin 17:5b9fbb6133ad 84 phase=1;
choutin 14:0629ab54765c 85
choutin 17:5b9fbb6133ad 86 }
choutin 17:5b9fbb6133ad 87
choutin 17:5b9fbb6133ad 88 if(phase==1){
choutin 14:0629ab54765c 89 //オブジェクト3,4回収
choutin 17:5b9fbb6133ad 90 open_arm();
choutin 17:5b9fbb6133ad 91 pmove(targetx[3],targety[3]);
choutin 17:5b9fbb6133ad 92 close_arm();
choutin 17:5b9fbb6133ad 93 close_hand();
choutin 17:5b9fbb6133ad 94 beltup();
choutin 17:5b9fbb6133ad 95 wait(btime);
choutin 17:5b9fbb6133ad 96 beltstop();
choutin 17:5b9fbb6133ad 97 pmove(1200,900+slidedist);//ハブ
choutin 17:5b9fbb6133ad 98 open_hand();
choutin 14:0629ab54765c 99 pmove(targetx[4],targety[4]);
choutin 17:5b9fbb6133ad 100
choutin 14:0629ab54765c 101 pmove(goalx,goaly2);
choutin 17:5b9fbb6133ad 102 open_arm();
choutin 17:5b9fbb6133ad 103 open_hand();
choutin 14:0629ab54765c 104 //リリース
choutin 14:0629ab54765c 105 back300();
choutin 17:5b9fbb6133ad 106
choutin 17:5b9fbb6133ad 107 phase=2;
choutin 17:5b9fbb6133ad 108 }
choutin 17:5b9fbb6133ad 109
choutin 17:5b9fbb6133ad 110 if(phase==2){
choutin 17:5b9fbb6133ad 111 //オブジェクト5,6回収
choutin 17:5b9fbb6133ad 112 open_arm();
choutin 15:a2cdcb373d3d 113 pmove(1200,900+slidedist);//ハブ
choutin 14:0629ab54765c 114 pmove(targetx[5],targety[5]);
choutin 14:0629ab54765c 115 //回収
choutin 17:5b9fbb6133ad 116 pmove(targetx[6],targety[6]);
choutin 14:0629ab54765c 117 //回収?
choutin 17:5b9fbb6133ad 118
choutin 17:5b9fbb6133ad 119 pmove(goalx,goaly3);
choutin 14:0629ab54765c 120 back300();
choutin 17:5b9fbb6133ad 121 phase=3;
choutin 17:5b9fbb6133ad 122 }
choutin 17:5b9fbb6133ad 123
choutin 17:5b9fbb6133ad 124 if(phase==3){
choutin 17:5b9fbb6133ad 125 //オブジェクト7,8回収
choutin 17:5b9fbb6133ad 126 pmove(1200,600+slidedist);//ハブ
choutin 17:5b9fbb6133ad 127
choutin 17:5b9fbb6133ad 128 pmove(targetx[7],targety[7]);
choutin 14:0629ab54765c 129 //回収
choutin 17:5b9fbb6133ad 130 pmove(900,0);
choutin 15:a2cdcb373d3d 131 //回収
choutin 17:5b9fbb6133ad 132 pmove(targetx[8],targety[8]);
choutin 14:0629ab54765c 133 //回収?
choutin 17:5b9fbb6133ad 134 pmove(goalx,300+slidedist+30);
choutin 14:0629ab54765c 135 //リリース
choutin 14:0629ab54765c 136 back300();
choutin 14:0629ab54765c 137
choutin 17:5b9fbb6133ad 138 phase=4;
choutin 17:5b9fbb6133ad 139 }
choutin 14:0629ab54765c 140
choutin 14:0629ab54765c 141
choutin 17:5b9fbb6133ad 142 }*/