春ロボ1班(元F3RC4班+)
3/20 2:04
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "EC.h" 00002 #include "R1370P.h" 00003 #include "move4wheel.h" 00004 #include "mbed.h" 00005 #include "math.h" 00006 #include "PathFollowing.h" 00007 #include "movement.h" 00008 #include "maxonsetting.h" 00009 #include "manual.h" 00010 #include "can.h" 00011 00012 #define PI 3.141592 00013 00014 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義 00015 00016 int go_waitmode = 0; 00017 00018 /*------------運動時の各速度の設定----------*/ 00019 double st_speed = 500; // 移動開始時の速度 00020 double max_speed = 2500; // 直線移動時の最大速度 00021 double mid_speed = 2000; // 直線移動時の中間速度 00022 double turn_speed = 1000; // 旋回時の速度 00023 double end_speed = 200; // 停止直前の速度 00024 double fruit_speed = 200; // 果物を取る時の速度 00025 /*--------------------------------------*/ 00026 00027 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// 00028 //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp 00029 //DigitalOut canread_led(LED2); //canread -> on //can.cpp 00030 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp 00031 00032 //////////////////////////////////////////////////////////////以下main文///////////////////////////////////////////////////////////////// 00033 00034 int main() 00035 { 00036 UserLoopSetting_maxon(); 00037 UserLoopSetting_sensor(); 00038 UserLoopSetting_can(); 00039 00040 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 00041 00042 printf("ソースファイル名 : %s¥n", __FILE__); 00043 printf("作成日付 : %s¥n", __DATE__); 00044 printf("作成時刻 : %s¥n", __TIME__); 00045 00046 #endif 00047 00048 while(1) { 00049 //id1_value[0] = 1; 00050 switch(id1_value[0]) { 00051 //-----auto mode----------------------------------------------------------------------------------------------------------------------// 00052 case 1: 00053 //id1_value[6] = 0; 00054 switch(id1_value[6]) { 00055 case 0: 00056 //-----right mode-------------------------------------------------------------------------------------------------------------// 00057 //未修正 00058 00059 flag = 0; 00060 UserLoopSetting_enc_right(); 00061 if(T1 == 0) { //スタート位置からみかんの木まで移動 00062 00063 gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00064 gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00065 purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00066 purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00067 set_cond(2,1,1050,1,1076); 00068 gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00069 MaxonControl(0,0,0,0); 00070 pos_correction(1672,1600,0,0,0,25); 00071 enc_correction(1,1); 00072 gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00073 MaxonControl(0,0,0,0); 00074 00075 //printf("t1 = 0\n\r"); 00076 //wait(1); 00077 T1++; 00078 } 00079 if(T1 == 1) { 00080 while(1) { 00081 wait(0.1); 00082 //calc_xy(0,0,0); 00083 if(T1 == 2) { 00084 // enc_correction(1,1); 00085 break; 00086 } 00087 } 00088 } 00089 if(T1 == 2) { //みかんの木から三宝置き場まで移動 00090 00091 gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00092 purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00093 purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90); 00094 gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90); 00095 gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90); 00096 MaxonControl(0,0,0,0); 00097 set_cond(2,1,2462,0,6000); 00098 pos_correction(2962,4837.5,90,0,0,10); 00099 gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90); 00100 MaxonControl(0,0,0,0); 00101 enc_correction(1,1); 00102 00103 //printf("t1 = 2\n\r"); 00104 //wait(1); 00105 T1++; 00106 } 00107 if(T1 == 3) { 00108 while(1) { 00109 wait(0.1); 00110 //calc_xy(90,0,0); 00111 if(T1 == 4) { 00112 // enc_correction(1,1); 00113 break; 00114 } 00115 } 00116 } 00117 if(T1 == 4) { //三宝置き場からりんごの木まで移動 00118 00119 gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90); 00120 purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90); 00121 purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90); 00122 purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90); 00123 gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90); 00124 MaxonControl(0,0,0,0); 00125 set_cond(1,0,0,1,6038); 00126 pos_correction(2500,6610,90,1,0,8); 00127 set_cond(2,0,3162,1,6038); 00128 gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90); 00129 MaxonControl(0,0,0,0); 00130 enc_correction(1,1); 00131 00132 // printf("t1 = 4\n\r"); 00133 // wait(1); 00134 T1++; 00135 } 00136 if(T1 == 5) { 00137 while(1) { 00138 wait(0.1); 00139 //calc_xy(90,0,0); 00140 if(T1 == 6) { 00141 // enc_correction(1,1); 00142 break; 00143 } 00144 } 00145 } 00146 if(T1 == 6) { //りんごの木からお供え台まで移動 00147 00148 set_cond(1,0,0,0,6962); 00149 gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90); 00150 gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90); 00151 gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90); 00152 enc_correction(0,1); 00153 set_cond(2,1,19,0,6962); //要修正 00154 purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正 00155 enc_correction(1,0); 00156 set_cond(0,1,19,0,0); 00157 gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180); 00158 gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180); 00159 set_cond(2,0,1000,1,4000); 00160 gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180); 00161 MaxonControl(0,0,0,0); 00162 pos_correction(620,4370,180,0,0,30); 00163 enc_correction(1,1); 00164 MaxonControl(0,0,0,0); 00165 00166 // printf("t1 = 6\n\r"); 00167 // wait(1); 00168 T1++; 00169 } 00170 if(T1 == 7) { 00171 while(1) { 00172 printf("ashi finished\n\r"); 00173 MaxonControl(0,0,0,0); 00174 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00175 if(id1_value[6] != flag)break; 00176 } 00177 } 00178 break; 00179 00180 case 1: 00181 //-----left mode--------------------------------------------------------------------------------------------------------------// 00182 flag = 1; 00183 printf("start\n\r"); 00184 UserLoopSetting_enc_left(); 00185 if(T1 == 0) { //スタート位置からみかんの木まで移動 00186 00187 gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00188 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00189 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00190 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00191 set_cond(2,0,-1050,1,1076); 00192 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00193 MaxonControl(0,0,0,0); 00194 pos_correction(-1672,1600,0,0,0,25); 00195 enc_correction(1,1); 00196 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00197 MaxonControl(0,0,0,0); 00198 00199 T1++; 00200 } 00201 if(T1 == 1) { 00202 while(1) { 00203 wait(0.1); 00204 calc_xy(0,0,0); 00205 if(T1 == 2) { 00206 enc_correction(1,1); 00207 break; 00208 } 00209 } 00210 } 00211 if(T1 == 2) { //みかんの木から三宝置き場まで移動 00212 gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00213 purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00214 purecurve(4,1,1,-2317,2500,-2962,3000,9,turn_speed,5,0.1,10,0.1,600,-90); //purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); 00215 gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); 00216 gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90); 00217 MaxonControl(0,0,0,0); 00218 set_cond(2,0,-2462,0,6000); 00219 pos_correction(-2962,4837.5,-90,0,0,10); 00220 gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90); 00221 MaxonControl(0,0,0,0); 00222 enc_correction(1,1); 00223 00224 T1++; 00225 } 00226 if(T1 == 3) { 00227 while(1) { 00228 wait(0.1); 00229 calc_xy(-90,0,0); 00230 if(T1 == 4) { 00231 enc_correction(1,1); 00232 break; 00233 } 00234 } 00235 } 00236 if(T1 == 4) { //三宝置き場からりんごの木まで移動 00237 gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); 00238 purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); 00239 purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); 00240 purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); 00241 gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90); 00242 MaxonControl(0,0,0,0); 00243 set_cond(1,0,0,1,6038); 00244 pos_correction(-2500,6610,-90,1,0,8); 00245 set_cond(2,1,-3162,1,6038); 00246 gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); 00247 MaxonControl(0,0,0,0); 00248 enc_correction(1,1); 00249 00250 T1++; 00251 } 00252 if(T1 == 5) { 00253 while(1) { 00254 wait(0.1); 00255 calc_xy(-90,0,0); 00256 if(T1 == 6) { 00257 enc_correction(1,1); 00258 break; 00259 } 00260 } 00261 } 00262 if(T1 == 6) { //りんごの木からお供え台まで移動 00263 set_cond(1,0,0,0,6962); 00264 gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); 00265 gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); 00266 00267 gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); 00268 enc_correction(0,1); 00269 set_cond(2,0,-19,0,6962); 00270 purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正 00271 enc_correction(1,0); 00272 set_cond(0,0,-19,0,0); 00273 gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); 00274 gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); 00275 set_cond(2,1,-1000,1,4000); 00276 gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); 00277 MaxonControl(0,0,0,0); 00278 pos_correction(-620,4370,-180,0,0,30); 00279 enc_correction(1,1); 00280 MaxonControl(0,0,0,0); 00281 00282 T1++; 00283 } 00284 if(T1 == 7) { 00285 while(1) { 00286 printf("ashi finished\n\r"); 00287 MaxonControl(0,0,0,0); 00288 00289 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00290 if(id1_value[6] != flag)break; 00291 } 00292 } 00293 00294 00295 00296 break; 00297 } 00298 //-----wait mode----------------------------------------------------------------------------------------------------------------------// 00299 case 0: 00300 00301 // printf("wait\n\r"); 00302 calc_xy(0,1,1); 00303 ashi_led(); 00304 MaxonControl(0,0,0,0); 00305 go_waitmode = 0; 00306 00307 break; 00308 //-----manual mode--------------------------------------------------------------------------------------------------------------------// 00309 case 2: 00310 switch(id1_value[6]) { 00311 case 0: 00312 //-----right mode-------------------------------------------------------------------------------------------------------------// 00313 //未修正 00314 00315 flag = 0; 00316 UserLoopSetting_enc_right(); 00317 if(T1 == 0) { //スタート位置からみかんの木まで移動 00318 00319 gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00320 gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00321 purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00322 purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00323 set_cond(2,1,1050,1,1076); 00324 gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00325 MaxonControl(0,0,0,0); 00326 pos_correction(1672,1600,0,0,0,25); 00327 enc_correction(1,1); 00328 gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00329 MaxonControl(0,0,0,0); 00330 00331 //printf("t1 = 0\n\r"); 00332 //wait(1); 00333 T1++; 00334 } 00335 if(T1 == 1) { 00336 while(1) { 00337 ManualOut(250,100,500,200); 00338 //calc_xy(0,0,0); 00339 if(T1 == 2) { 00340 // enc_correction(1,1); 00341 break; 00342 } 00343 } 00344 } 00345 if(T1 == 2) { //みかんの木から三宝置き場まで移動 00346 00347 gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00348 purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00349 purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90); 00350 gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90); 00351 gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90); 00352 MaxonControl(0,0,0,0); 00353 set_cond(2,1,2462,0,6000); 00354 pos_correction(2962,4837.5,90,0,0,10); 00355 gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90); 00356 MaxonControl(0,0,0,0); 00357 enc_correction(1,1); 00358 00359 //printf("t1 = 2\n\r"); 00360 //wait(1); 00361 T1++; 00362 } 00363 if(T1 == 3) { 00364 while(1) { 00365 ManualOut(250,100,500,200); 00366 //calc_xy(90,0,0); 00367 if(T1 == 4) { 00368 // enc_correction(1,1); 00369 break; 00370 } 00371 } 00372 } 00373 if(T1 == 4) { //三宝置き場からりんごの木まで移動 00374 00375 gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90); 00376 purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90); 00377 purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90); 00378 purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90); 00379 gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90); 00380 MaxonControl(0,0,0,0); 00381 set_cond(1,0,0,1,6038); 00382 pos_correction(2500,6610,90,1,0,8); 00383 set_cond(2,0,3162,1,6038); 00384 gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90); 00385 MaxonControl(0,0,0,0); 00386 enc_correction(1,1); 00387 00388 // printf("t1 = 4\n\r"); 00389 // wait(1); 00390 T1++; 00391 } 00392 if(T1 == 5) { 00393 while(1) { 00394 ManualOut(250,100,500,200); 00395 //calc_xy(90,0,0); 00396 if(T1 == 6) { 00397 // enc_correction(1,1); 00398 break; 00399 } 00400 } 00401 } 00402 if(T1 == 6) { //りんごの木からお供え台まで移動 00403 00404 set_cond(1,0,0,0,6962); 00405 gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90); 00406 gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90); 00407 gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90); 00408 enc_correction(0,1); 00409 set_cond(2,1,19,0,6962); //要修正 00410 purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正 00411 enc_correction(1,0); 00412 set_cond(0,1,19,0,0); 00413 gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180); 00414 gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180); 00415 set_cond(2,0,1000,1,4000); 00416 gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180); 00417 MaxonControl(0,0,0,0); 00418 pos_correction(620,4370,180,0,0,30); 00419 enc_correction(1,1); 00420 MaxonControl(0,0,0,0); 00421 00422 // printf("t1 = 6\n\r"); 00423 // wait(1); 00424 T1++; 00425 } 00426 if(T1 == 7) { 00427 while(1) { 00428 ManualOut(250,100,500,200); 00429 //printf("ashi finished\n\r"); 00430 //MaxonControl(0,0,0,0); 00431 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00432 if(id1_value[6] != flag)break; 00433 } 00434 } 00435 break; 00436 00437 case 1: 00438 //-----left mode--------------------------------------------------------------------------------------------------------------// 00439 flag = 1; 00440 printf("start\n\r"); 00441 UserLoopSetting_enc_left(); 00442 if(T1 == 0) { //スタート位置からみかんの木まで移動 00443 00444 gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00445 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00446 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00447 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00448 set_cond(2,0,-1050,1,1076); 00449 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00450 MaxonControl(0,0,0,0); 00451 pos_correction(-1672,1600,0,0,0,25); 00452 enc_correction(1,1); 00453 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00454 MaxonControl(0,0,0,0); 00455 00456 T1++; 00457 } 00458 00459 if(T1 == 1) { 00460 while(1) { 00461 ManualOut(250,100,500,200); 00462 if(T1 == 2) { 00463 break; 00464 } 00465 } 00466 } 00467 if(T1 == 2) { //みかんの木から三宝置き場まで移動 00468 gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00469 purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00470 purecurve(4,1,1,-2317,2500,-2962,3000,9,turn_speed,5,0.1,10,0.1,600,-90); //purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); 00471 gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); 00472 gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90); 00473 MaxonControl(0,0,0,0); 00474 set_cond(2,0,-2462,0,6000); 00475 pos_correction(-2962,4837.5,-90,0,0,10); 00476 gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90); 00477 MaxonControl(0,0,0,0); 00478 enc_correction(1,1); 00479 00480 T1++; 00481 } 00482 if(T1 == 3) { 00483 while(1) { 00484 ManualOut(250,100,500,200); 00485 if(T1 == 4) { 00486 break; 00487 } 00488 } 00489 } 00490 if(T1 == 4) { //三宝置き場からりんごの木まで移動 00491 gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); 00492 purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); 00493 purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); 00494 purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); 00495 gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90); 00496 MaxonControl(0,0,0,0); 00497 set_cond(1,0,0,1,6038); 00498 pos_correction(-2500,6610,-90,1,0,8); 00499 set_cond(2,1,-3162,1,6038); 00500 gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); 00501 MaxonControl(0,0,0,0); 00502 enc_correction(1,1); 00503 00504 T1++; 00505 } 00506 if(T1 == 5) { 00507 while(1) { 00508 ManualOut(250,100,500,200); 00509 if(T1 == 6) { 00510 break; 00511 } 00512 } 00513 } 00514 if(T1 == 6) { //りんごの木からお供え台まで移動 00515 set_cond(1,0,0,0,6962); 00516 gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); 00517 gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); 00518 gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); 00519 enc_correction(0,1); 00520 set_cond(2,0,-19,0,6962); 00521 purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正 00522 enc_correction(1,0); 00523 set_cond(0,0,-19,0,0); 00524 gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); 00525 gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); 00526 set_cond(2,1,-1000,1,4000); 00527 gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); 00528 MaxonControl(0,0,0,0); 00529 pos_correction(-620,4370,-180,0,0,30); 00530 enc_correction(1,1); 00531 MaxonControl(0,0,0,0); 00532 00533 T1++; 00534 } 00535 if(T1 == 7) { 00536 while(1) { 00537 //printf("ashi finished\n\r"); 00538 ManualOut(250,100,500,200); 00539 //MaxonControl(0,0,0,0); 00540 00541 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00542 if(id1_value[6] != flag)break; 00543 } 00544 } 00545 00546 00547 00548 break; 00549 } 00550 // ManualOut(250,100,500,200); 00551 //go_waitmode = 0; 00552 00553 break; 00554 } 00555 //------------------------------------------------------------------------------------------------------------------------------------// 00556 } 00557 }
Generated on Wed Jul 20 2022 13:26:37 by
1.7.2