春ロボ1班(元F3RC4班+)
3/20 13:39
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 flag = 0; 00058 UserLoopSetting_enc_right(); 00059 00060 if(T1 == 0) { //スタート位置から少し移動 00061 00062 gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00063 T1++; 00064 } 00065 if(T1 == 1) { //みかんの木まで移動して掴む 00066 00067 gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00068 purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00069 purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00070 set_cond(2,1,1050,1,1076); 00071 gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00072 MaxonControl(0,0,0,0); 00073 pos_correction(1672,1600,0,0,0,25); 00074 enc_correction(1,1); 00075 gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00076 MaxonControl(0,0,0,0); 00077 T1++; 00078 } 00079 00080 if(T1 == 2) { //みかんを持ち上げる 00081 while(1) { 00082 wait(0.1); 00083 if(T1 == 3) { 00084 break; 00085 } 00086 } 00087 } 00088 if(T1 == 3) { //みかんの木から三宝置き場まで移動 00089 00090 gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00091 purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00092 purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90); 00093 gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90); 00094 gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90); 00095 MaxonControl(0,0,0,0); 00096 set_cond(2,1,2462,0,6000); 00097 pos_correction(2962,4837.5,90,0,0,10); 00098 gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90); 00099 MaxonControl(0,0,0,0); 00100 enc_correction(1,1); 00101 00102 00103 T1++; 00104 } 00105 if(T1 == 4) { //三宝を掴み機体内に取り込み、みかんを三宝に乗せる 00106 while(1) { 00107 wait(0.1); 00108 if(T1 == 5) { 00109 break; 00110 } 00111 } 00112 } 00113 if(T1 == 5) { //三宝置き場からりんごの木まで移動 00114 00115 gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90); 00116 purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90); 00117 purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90); 00118 purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90); 00119 gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90); 00120 MaxonControl(0,0,0,0); 00121 set_cond(1,0,0,1,6038); 00122 pos_correction(2500,6610,90,1,0,8); 00123 set_cond(2,0,3162,1,6038); 00124 gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90); 00125 MaxonControl(0,0,0,0); 00126 enc_correction(1,1); 00127 00128 T1++; 00129 } 00130 if(T1 == 6) { //りんごを持ち上げる 00131 while(1) { 00132 wait(0.1); 00133 if(T1 == 7) { 00134 break; 00135 } 00136 } 00137 } 00138 if(T1 == 7) { //りんごの木からお供え台まで移動 00139 00140 set_cond(1,0,0,0,6962); 00141 gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90); 00142 gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90); 00143 gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90); 00144 enc_correction(0,1); 00145 set_cond(2,1,19,0,6962); //要修正 00146 purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正 00147 enc_correction(1,0); 00148 set_cond(0,1,19,0,0); 00149 gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180); 00150 gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180); 00151 set_cond(2,0,1000,1,4000); 00152 gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180); 00153 MaxonControl(0,0,0,0); 00154 pos_correction(620,4370,180,0,0,30); 00155 enc_correction(1,1); 00156 MaxonControl(0,0,0,0); 00157 00158 T1++; 00159 } 00160 if(T1 == 8 || T1 == 9) { //三宝をお供え台に置く 00161 while(1) { 00162 printf("ashi finished\n\r"); 00163 MaxonControl(0,0,0,0); 00164 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00165 if(id1_value[6] != flag)break; 00166 } 00167 } 00168 break; 00169 case 1: 00170 //-----left mode--------------------------------------------------------------------------------------------------------------// 00171 flag = 1; 00172 UserLoopSetting_enc_left(); 00173 00174 if(T1 == 0) { //スタート位置から少し移動 00175 00176 gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00177 00178 T1++; 00179 } 00180 if(T1 == 1) { //みかんの木まで移動して掴む 00181 00182 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00183 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00184 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00185 set_cond(2,0,-1050,1,1076); 00186 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00187 MaxonControl(0,0,0,0); 00188 pos_correction(-1672,1600,0,0,0,25); 00189 enc_correction(1,1); 00190 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00191 MaxonControl(0,0,0,0); 00192 00193 T1++; 00194 } 00195 00196 if(T1 == 2) { //みかんを持ち上げる 00197 while(1) { 00198 wait(0.1); 00199 if(T1 == 3) { 00200 break; 00201 } 00202 } 00203 } 00204 if(T1 == 3) { //みかんの木から三宝置き場まで移動 00205 00206 gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00207 purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00208 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); 00209 gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); 00210 gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90); 00211 MaxonControl(0,0,0,0); 00212 set_cond(2,0,-2462,0,6000); 00213 pos_correction(-2962,4837.5,-90,0,0,10); 00214 gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90); 00215 MaxonControl(0,0,0,0); 00216 enc_correction(1,1); 00217 00218 00219 T1++; 00220 } 00221 if(T1 == 4) { //三宝を掴み機体内に取り込み、みかんを三宝に乗せる 00222 while(1) { 00223 wait(0.1); 00224 if(T1 == 5) { 00225 break; 00226 } 00227 } 00228 } 00229 if(T1 == 5) { //三宝置き場からりんごの木まで移動 00230 00231 gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); 00232 purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); 00233 purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); 00234 purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); 00235 gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90); 00236 MaxonControl(0,0,0,0); 00237 set_cond(1,0,0,1,6038); 00238 pos_correction(-2500,6610,-90,1,0,8); 00239 set_cond(2,1,-3162,1,6038); 00240 gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); 00241 MaxonControl(0,0,0,0); 00242 enc_correction(1,1); 00243 00244 T1++; 00245 } 00246 if(T1 == 6) { //りんごを持ち上げる 00247 while(1) { 00248 wait(0.1); 00249 if(T1 == 7) { 00250 break; 00251 } 00252 } 00253 } 00254 if(T1 == 7) { //りんごの木からお供え台まで移動 00255 00256 set_cond(1,0,0,0,6962); 00257 gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); 00258 gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); 00259 00260 gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); 00261 enc_correction(0,1); 00262 set_cond(2,0,-19,0,6962); 00263 purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正 00264 enc_correction(1,0); 00265 set_cond(0,0,-19,0,0); 00266 gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); 00267 gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); 00268 set_cond(2,1,-1000,1,4000); 00269 gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); 00270 MaxonControl(0,0,0,0); 00271 pos_correction(-620,4370,-180,0,0,30); 00272 enc_correction(1,1); 00273 MaxonControl(0,0,0,0); 00274 00275 T1++; 00276 } 00277 if(T1 == 8 || T1 == 9) { //三宝をお供え台に置く 00278 while(1) { 00279 printf("ashi finished\n\r"); 00280 MaxonControl(0,0,0,0); 00281 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00282 if(id1_value[6] != flag)break; 00283 } 00284 } 00285 break; 00286 } 00287 //-----wait mode----------------------------------------------------------------------------------------------------------------------// 00288 case 0: 00289 00290 // printf("wait\n\r"); 00291 calc_xy(0,1,1); 00292 ashi_led(); 00293 MaxonControl(0,0,0,0); 00294 go_waitmode = 0; 00295 00296 break; 00297 //-----manual mode--------------------------------------------------------------------------------------------------------------------// 00298 case 2: 00299 switch(id1_value[6]) { 00300 case 0: 00301 //-----right mode-------------------------------------------------------------------------------------------------------------// 00302 //未修正 00303 00304 flag = 0; 00305 UserLoopSetting_enc_right(); 00306 if(T1 == 0) { //スタート位置からみかんの木まで移動 00307 00308 gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00309 gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00310 purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00311 purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00312 set_cond(2,1,1050,1,1076); 00313 gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00314 MaxonControl(0,0,0,0); 00315 pos_correction(1672,1600,0,0,0,25); 00316 enc_correction(1,1); 00317 gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00318 MaxonControl(0,0,0,0); 00319 00320 //printf("t1 = 0\n\r"); 00321 //wait(1); 00322 T1++; 00323 } 00324 if(T1 == 1) { 00325 while(1) { 00326 ManualOut(250,100,500,200); 00327 //calc_xy(0,0,0); 00328 if(T1 == 2) { 00329 // enc_correction(1,1); 00330 break; 00331 } 00332 } 00333 } 00334 if(T1 == 2) { //みかんの木から三宝置き場まで移動 00335 00336 gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00337 purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00338 purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90); 00339 gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90); 00340 gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90); 00341 MaxonControl(0,0,0,0); 00342 set_cond(2,1,2462,0,6000); 00343 pos_correction(2962,4837.5,90,0,0,10); 00344 gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90); 00345 MaxonControl(0,0,0,0); 00346 enc_correction(1,1); 00347 00348 //printf("t1 = 2\n\r"); 00349 //wait(1); 00350 T1++; 00351 } 00352 if(T1 == 3) { 00353 while(1) { 00354 ManualOut(250,100,500,200); 00355 //calc_xy(90,0,0); 00356 if(T1 == 4) { 00357 // enc_correction(1,1); 00358 break; 00359 } 00360 } 00361 } 00362 if(T1 == 4) { //三宝置き場からりんごの木まで移動 00363 00364 gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90); 00365 purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90); 00366 purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90); 00367 purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90); 00368 gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90); 00369 MaxonControl(0,0,0,0); 00370 set_cond(1,0,0,1,6038); 00371 pos_correction(2500,6610,90,1,0,8); 00372 set_cond(2,0,3162,1,6038); 00373 gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90); 00374 MaxonControl(0,0,0,0); 00375 enc_correction(1,1); 00376 00377 // printf("t1 = 4\n\r"); 00378 // wait(1); 00379 T1++; 00380 } 00381 if(T1 == 5) { 00382 while(1) { 00383 ManualOut(250,100,500,200); 00384 //calc_xy(90,0,0); 00385 if(T1 == 6) { 00386 // enc_correction(1,1); 00387 break; 00388 } 00389 } 00390 } 00391 if(T1 == 6) { //りんごの木からお供え台まで移動 00392 00393 set_cond(1,0,0,0,6962); 00394 gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90); 00395 gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90); 00396 gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90); 00397 enc_correction(0,1); 00398 set_cond(2,1,19,0,6962); //要修正 00399 purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正 00400 enc_correction(1,0); 00401 set_cond(0,1,19,0,0); 00402 gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180); 00403 gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180); 00404 set_cond(2,0,1000,1,4000); 00405 gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180); 00406 MaxonControl(0,0,0,0); 00407 pos_correction(620,4370,180,0,0,30); 00408 enc_correction(1,1); 00409 MaxonControl(0,0,0,0); 00410 00411 // printf("t1 = 6\n\r"); 00412 // wait(1); 00413 T1++; 00414 } 00415 if(T1 == 7) { 00416 while(1) { 00417 ManualOut(250,100,500,200); 00418 //printf("ashi finished\n\r"); 00419 //MaxonControl(0,0,0,0); 00420 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00421 if(id1_value[6] != flag)break; 00422 } 00423 } 00424 break; 00425 00426 case 1: 00427 //-----left mode--------------------------------------------------------------------------------------------------------------// 00428 flag = 1; 00429 printf("start\n\r"); 00430 UserLoopSetting_enc_left(); 00431 if(T1 == 0) { //スタート位置からみかんの木まで移動 00432 00433 gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00434 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00435 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00436 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00437 set_cond(2,0,-1050,1,1076); 00438 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00439 MaxonControl(0,0,0,0); 00440 pos_correction(-1672,1600,0,0,0,25); 00441 enc_correction(1,1); 00442 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00443 MaxonControl(0,0,0,0); 00444 00445 T1++; 00446 } 00447 00448 if(T1 == 1) { 00449 while(1) { 00450 ManualOut(250,100,500,200); 00451 if(T1 == 2) { 00452 break; 00453 } 00454 } 00455 } 00456 if(T1 == 2) { //みかんの木から三宝置き場まで移動 00457 gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00458 purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00459 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); 00460 gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); 00461 gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90); 00462 MaxonControl(0,0,0,0); 00463 set_cond(2,0,-2462,0,6000); 00464 pos_correction(-2962,4837.5,-90,0,0,10); 00465 gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90); 00466 MaxonControl(0,0,0,0); 00467 enc_correction(1,1); 00468 00469 T1++; 00470 } 00471 if(T1 == 3) { 00472 while(1) { 00473 ManualOut(250,100,500,200); 00474 if(T1 == 4) { 00475 break; 00476 } 00477 } 00478 } 00479 if(T1 == 4) { //三宝置き場からりんごの木まで移動 00480 gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); 00481 purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); 00482 purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); 00483 purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); 00484 gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90); 00485 MaxonControl(0,0,0,0); 00486 set_cond(1,0,0,1,6038); 00487 pos_correction(-2500,6610,-90,1,0,8); 00488 set_cond(2,1,-3162,1,6038); 00489 gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); 00490 MaxonControl(0,0,0,0); 00491 enc_correction(1,1); 00492 00493 T1++; 00494 } 00495 if(T1 == 5) { 00496 while(1) { 00497 ManualOut(250,100,500,200); 00498 if(T1 == 6) { 00499 break; 00500 } 00501 } 00502 } 00503 if(T1 == 6) { //りんごの木からお供え台まで移動 00504 set_cond(1,0,0,0,6962); 00505 gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); 00506 gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); 00507 gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); 00508 enc_correction(0,1); 00509 set_cond(2,0,-19,0,6962); 00510 purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正 00511 enc_correction(1,0); 00512 set_cond(0,0,-19,0,0); 00513 gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); 00514 gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); 00515 set_cond(2,1,-1000,1,4000); 00516 gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); 00517 MaxonControl(0,0,0,0); 00518 pos_correction(-620,4370,-180,0,0,30); 00519 enc_correction(1,1); 00520 MaxonControl(0,0,0,0); 00521 00522 T1++; 00523 } 00524 if(T1 == 7) { 00525 while(1) { 00526 //printf("ashi finished\n\r"); 00527 ManualOut(250,100,500,200); 00528 //MaxonControl(0,0,0,0); 00529 00530 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00531 if(id1_value[6] != flag)break; 00532 } 00533 } 00534 00535 00536 00537 break; 00538 } 00539 // ManualOut(250,100,500,200); 00540 //go_waitmode = 0; 00541 00542 break; 00543 } 00544 //------------------------------------------------------------------------------------------------------------------------------------// 00545 } 00546 }
Generated on Mon Jul 18 2022 00:12:37 by
1.7.2