a
Dependencies: Locate Move Servo button mbed
Fork of 3rdcompfixstart by
main.cpp
00001 #include "mbed.h" 00002 #include "math.h" 00003 #include "locate.h" 00004 #include "move.h" 00005 #include "servo.h" 00006 #include "button.h" 00007 00008 DigitalOut ledg(PC_2); //green 00009 DigitalOut ledy(PC_3); //yellow 00010 DigitalOut ledr(PC_0); //red 00011 00012 00013 const int dist=150;//タイヤ、アーム間の距離 00014 const int slidedist=102-20; 00015 const int space=50;//壁やばそうなときのよゆう 00016 const float btime=1.9; 00017 const float allowdegree=0.2; 00018 const float allowlength=0.2; 00019 00020 00021 int main() 00022 { 00023 00024 int teams=1,phase=0; 00025 teams=teamLED(); 00026 int team = 1; 00027 setup(teams); 00028 initmotor(teams); 00029 open_arm(); 00030 wait(2); 00031 close_arm(); 00032 00033 while(start()){} 00034 int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 }; 00035 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) }; 00036 int goalx=150,goaly=1200+(slidedist)*0;//yへのslidediatを無視 00037 00038 phase=phaseSW(); 00039 00040 // 00041 00042 //オブジェクト0,1,2回収 00043 if(phase==0){ 00044 pmove(targetx[0],0); //傾きをはじめから向くように 00045 open_hand(); 00046 pmove2(targetx[0],targety[0]); 00047 close_hand(); 00048 wait(0.3); 00049 beltup(); 00050 wait(btime); 00051 beltstop(); 00052 open_hand(); 00053 //回収 00054 pmove2(targetx[1],targety[1]); 00055 close_hand(); 00056 wait(0.3); 00057 beltup(); 00058 wait(btime); 00059 beltstop(); 00060 open_hand(); 00061 //回収 00062 pmove(targetx[2],targety[2]); 00063 //回収 00064 close_hand(); 00065 pmove2(goalx,goaly); 00066 //リリース 00067 turnrad_ccw(PI,team); 00068 open_arm(); 00069 open_hand(); 00070 wait(0.5); 00071 back300();//(300,1200)なう 00072 turnrad_ccw(2*PI,team); 00073 //-------------------------------------------------------------------- 00074 open_arm(); 00075 pmove(targetx[3],targety[3]); 00076 close_arm(); 00077 close_hand(); 00078 turnrad_cw(PI,team); 00079 /* 00080 open_hanf(); 00081 pmove(1800,team*(1200+slidedist)); 00082 closehand(); 00083 */ 00084 pmove(150,team*(1200+slidedist-space));//ゴール 00085 00086 open_arm(); 00087 open_hand(); 00088 wait(0.3); 00089 00090 00091 back300(); 00092 turnrad_ccw(2*PI,team); 00093 phase=2; 00094 00095 //また縦に回収 00096 close_arm(); 00097 pmove2(900,team*(1200+slidedist+space)); 00098 00099 open_hand(); 00100 pmove2(targetx[4],targety[4]); 00101 close_hand(); 00102 wait(0.3); 00103 beltup(); 00104 wait(btime); 00105 beltstop(); 00106 open_hand(); 00107 //回収 00108 pmove(targetx[6],targety[6]); 00109 close_hand(); 00110 wait(0.3); 00111 beltup(); 00112 wait(btime); 00113 beltstop(); 00114 open_hand(); 00115 //回収 00116 pmove(targetx[8],targety[8]); 00117 //回収 00118 close_hand(); 00119 pmove2(150,team*(900+slidedist));//斜め移動 00120 00121 turnrad_ccw(PI,team); 00122 open_arm(); 00123 open_hand(); 00124 back300(); 00125 00126 00127 pmove2(700,(600+slidedist));//ハブ 00128 00129 pmove(targetx[5],targety[5]); 00130 close_arm(); 00131 close_hand(); 00132 nxback300(); 00133 00134 pmove2(150,team*(900+slidedist)); 00135 open_arm(); 00136 open_hand(); 00137 00138 back300(); 00139 while(1){ 00140 buzzer(1); 00141 wait(0.5); 00142 buzzer(0); 00143 wait(1); 00144 } 00145 } 00146 00147 00148 if(phase==1){ 00149 00150 pmove(targetx[7],targety[7]); 00151 back300(); 00152 pmove2(goalx,goaly); 00153 turnrad_ccw(PI,team); 00154 open_arm(); 00155 open_hand(); 00156 back300(); 00157 00158 pmove2(900,600+slidedist); 00159 00160 pmove(targetx[5],targety[5]); 00161 pmove2(goalx,goaly); 00162 turnrad_ccw(2*PI,team); 00163 open_arm(); 00164 open_hand(); 00165 back300(); 00166 00167 } 00168 00169 }
Generated on Sat Jul 16 2022 23:32:02 by
1.7.2
