3/20 13:39
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: main.cpp
- Revision:
- 11:9db93bce4eef
- Parent:
- 10:c741191360de
- Child:
- 12:762061580168
--- a/main.cpp Wed Mar 13 04:23:18 2019 +0000 +++ b/main.cpp Tue Mar 19 02:30:34 2019 +0000 @@ -15,6 +15,15 @@ int go_waitmode = 0; +/*------------運動時の各速度の設定----------*/ +double st_speed = 500; // 移動開始時の速度 +double max_speed = 2500; // 直線移動時の最大速度 +double mid_speed = 2000; // 直線移動時の中間速度 +double turn_speed = 1000; // 旋回時の速度 +double end_speed = 200; // 停止直前の速度 +double fruit_speed = 200; // 果物を取る時の速度 +/*--------------------------------------*/ + //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp //DigitalOut canread_led(LED2); //canread -> on //can.cpp @@ -37,62 +46,74 @@ #endif while(1) { - + //id1_value[0] = 1; switch(id1_value[0]) { //-----auto mode----------------------------------------------------------------------------------------------------------------------// case 1: - + //id1_value[6] = 0; switch(id1_value[6]) { case 0: //-----right mode-------------------------------------------------------------------------------------------------------------// + //未修正 + flag = 0; UserLoopSetting_enc_right(); if(T1 == 0) { //スタート位置からみかんの木まで移動 + /* gogo_straight(1,1,2962,3500,2962,2900,200,1000,5,0.1,10,0.1,600,0); purecurve(6,1,1,2962,2900,2317,2500,9,1000,5,0.1,10,0.1,600,0); purecurve(5,1,1,2317,2500,1672,2000,9,1000,5,0.1,10,0.1,600,0); set_cond(2,1,1050,1,1076); - gogo_straight(0,0,1672,2000,1672,1400,1000,200,5,0.1,10,0.1,600,0); + gogo_straight(0,0,1672,2000,1672,1600,1000,200,5,0.1,10,0.1,600,0); MaxonControl(0,0,0,0); - pos_correction(1672,1400,0,0,0); + pos_correction(1672,1600,0,0,0,25); enc_correction(1,1); - wait(0.5); + gogo_straight(0,0,1672,1600,1672,1400,200,200,5,0.1,10,0.1,600,0); + */ + printf("t1 = 0\n\r"); + wait(1); T1++; } if(T1 == 1) { while(1) { wait(0.1); + //calc_xy(0,0,0); if(T1 == 2) { + // enc_correction(1,1); break; } } } if(T1 == 2) { //みかんの木から三宝置き場まで移動 - gogo_straight(1,1,1672,1400,1672,2000,200,1000,5,0.1,10,0.1,600,0); + /*gogo_straight(1,1,1672,1400,1672,2000,200,1000,5,0.1,10,0.1,600,0); purecurve(2,1,1,1672,2000,2317,2500,9,1000,5,0.1,10,0.1,600,0); purecurve(1,1,1,2317,2500,2962,3000,9,1000,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); gogo_straight(1,1,2962,3000,2962,4000,1000,1000,5,0.1,10,0.1,600,90); gogo_straight(1,1,2962,4000,2962,4500,1000,200,5,0.1,10,0.1,600,90); MaxonControl(0,0,0,0); - pos_correction(2962,4500,90,1,1); + // pos_correction(2962,4500,90,1,1); set_cond(2,1,2462,0,6000); gogo_straight(0,0,2962,4500,2850,4500,200,200,5,0.1,10,0.1,800,90); MaxonControl(0,0,0,0); - pos_correction(2850,4500,90,0,0); + pos_correction(2850,4500,90,0,0,10); enc_correction(1,1); - wait(0.5); + wait(0.5);*/ + printf("t1 = 2\n\r"); + wait(1); T1++; } if(T1 == 3) { while(1) { wait(0.1); + //calc_xy(90,0,0); if(T1 == 4) { + // enc_correction(1,1); break; } } } if(T1 == 4) { //三宝置き場からりんごの木まで移動 - gogo_straight(1,1,2850,4500,2850,5150,200,1000,5,0.1,10,0.1,800,90); + /*gogo_straight(1,1,2850,4500,2850,5150,200,1000,5,0.1,10,0.1,800,90); purecurve(3,1,1,2850,5150,2257,5500,9,1000,5,0.1,10,0.1,800,90); purecurve(4,1,1,2257,5500,1700,6000,9,1000,5,0.1,10,0.1,800,90); purecurve(2,1,1,1700,6000,2257,6550,9,1000,5,0.1,10,0.1,800,90); @@ -100,31 +121,38 @@ set_cond(2,0,3500,1,6050); gogo_straight(0,0,2500,6550,2700,6600,1000,200,5,0.1,10,0.1,800,90); MaxonControl(0,0,0,0); - pos_correction(2700,6600,90,0,0); + pos_correction(2700,6600,90,0,0,15); enc_correction(1,1); - wait(0.5); + wait(0.5);*/ + printf("t1 = 4\n\r"); + wait(1); T1++; } if(T1 == 5) { while(1) { wait(0.1); + //calc_xy(90,0,0); if(T1 == 6) { + // enc_correction(1,1); break; } } } if(T1 == 6) { //りんごの木からお供え台まで移動 - gogo_straight(1,1,2700,6600,2500,6550,200,1000,5,0.1,10,0.1,800,90); - gogo_straight(1,1,2500,6550,1300,6550,1000,1000,5,0.1,10,0.1,800,90); - gogo_straight(1,1,1300,6550,1000,6500,1000,1000,5,0.1,10,0.1,800,90); - purecurve(5,1,1,1000,6550,519,6000,9,1000,5,0.1,10,0.1,600,180); - gogo_straight(1,1,519,6000,519,4700,1000,1000,5,0.1,10,0.1,600,180); - set_cond(2,0,1000,1,4000); - gogo_straight(0,0,519,4700,519,4500,1000,200,5,0.1,10,0.1,800,180); + /*gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); + gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); + purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); + purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); + set_cond(2,0,-1050,1,1076); + gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); MaxonControl(0,0,0,0); - pos_correction(519,4500,180,0,0); + pos_correction(-1672,1600,0,0,0,25); enc_correction(1,1); + gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); MaxonControl(0,0,0,0); + */ + printf("t1 = 6\n\r"); + wait(1); T1++; } if(T1 == 7) { @@ -136,105 +164,154 @@ } } break; - + case 1: //-----left mode--------------------------------------------------------------------------------------------------------------// flag = 1; + printf("start\n\r"); UserLoopSetting_enc_left(); if(T1 == 0) { //スタート位置からみかんの木まで移動 - gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0); - purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0); - purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0); + + gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); + gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); + purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); + purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); set_cond(2,0,-1050,1,1076); - gogo_straight(0,0,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0); + gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); MaxonControl(0,0,0,0); - pos_correction(-1672,1400,0,0,0); + pos_correction(-1672,1600,0,0,0,25); enc_correction(1,1); - wait(0.5); + gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); + MaxonControl(0,0,0,0); + T1++; } if(T1 == 1) { while(1) { wait(0.1); + calc_xy(0,0,0); if(T1 == 2) { + enc_correction(1,1); break; } } } if(T1 == 2) { //みかんの木から三宝置き場まで移動 - gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0); - purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0); - purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,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); - gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); - gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90); + gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); + purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); + 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); + gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); + gogo_straight(1,1,-2962,4000,-2962,4820,max_speed,end_speed,5,0.1,10,0.1,600,-90); MaxonControl(0,0,0,0); - pos_correction(-2962,4500,-90,1,1); set_cond(2,0,-2462,0,6000); - gogo_straight(0,0,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); + pos_correction(-2962,4837.5,-90,0,0,7); + gogo_straight(0,0,-2962,4837.5,-2800,4837.5,300,200,5,0.1,10,0.1,800,-90); MaxonControl(0,0,0,0); - pos_correction(-2850,4500,-90,0,0); enc_correction(1,1); - wait(0.5); + T1++; } if(T1 == 3) { while(1) { wait(0.1); + calc_xy(-90,0,0); if(T1 == 4) { + enc_correction(1,1); break; } } } if(T1 == 4) { //三宝置き場からりんごの木まで移動 - gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90); - purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90); - purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90); - purecurve(3,1,1,-1700,6000,-2257,6550,9,1000,5,0.1,10,0.1,800,-90); - gogo_straight(1,1,-2257,6550,-2500,6550,1000,1000,5,0.1,10,0.1,800,-90); - set_cond(2,1,-3500,1,6050); - gogo_straight(0,0,-2500,6550,-2700,6600,1000,200,5,0.1,10,0.1,800,-90); + gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); + purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); + purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); + purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); + gogo_straight(1,1,-2257,6550,-2500,6593,turn_speed,end_speed,5,0.1,10,0.1,800,-90); MaxonControl(0,0,0,0); - pos_correction(-2700,6600,-90,0,0); + set_cond(1,0,0,1,6050); + pos_correction(-2500,6600,-90,1,0,8); + set_cond(2,1,-3162,1,6038); + gogo_straight(0,0,-2500,6600,-2700,6600,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); + MaxonControl(0,0,0,0); enc_correction(1,1); - wait(0.5); + T1++; } if(T1 == 5) { while(1) { wait(0.1); + calc_xy(-90,0,0); if(T1 == 6) { + enc_correction(1,1); break; } } } if(T1 == 6) { //りんごの木からお供え台まで移動 - gogo_straight(1,1,-2700,6600,-2500,6550,200,1000,5,0.1,10,0.1,800,-90); - gogo_straight(1,1,-2500,6550,-1300,6550,1000,1000,5,0.1,10,0.1,800,-90); - gogo_straight(1,1,-1300,6550,-1000,6500,1000,1000,5,0.1,10,0.1,800,-90); - purecurve(8,1,1,-1000,6550,-519,6000,9,1000,5,0.1,10,0.1,600,-180); - gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180); + set_cond(1,0,0,0,6962); + gogo_straight(1,0,-2700,6600,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); + gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); + gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); + enc_correction(0,1); + set_cond(2,0,-19,0,6962); + purecurve(8,0,0,-850,6550,-519,6000,9,turn_speed,10,0.1,10,0.1,600,-180); + enc_correction(1,0); + set_cond(0,0,-19,0,0); + gogo_straight(0,1,-519,6000,-519,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); + gogo_straight(0,1,-519,5500,-519,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); set_cond(2,1,-1000,1,4000); - gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180); + gogo_straight(0,0,-519,5000,-519,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); MaxonControl(0,0,0,0); - pos_correction(-519,4500,-180,0,0); + pos_correction(-519,4370,-180,0,0,30); enc_correction(1,1); MaxonControl(0,0,0,0); + T1++; } if(T1 == 7) { while(1) { printf("ashi finished\n\r"); MaxonControl(0,0,0,0); + + /*gogo_straight(1,1,-519,4500,-519,5000,200,1000,5,0.1,10,0.1,600,-180); + gogo_straight(1,1,-519,5000,-519,6000,1000,1000,5,0.1,10,0.1,600,-180); + gogo_straight(1,1,-519,6000,-519,6600,1000,200,5,0.1,10,0.1,600,-180); + MaxonControl(0,0,0,0); + set_cond(2,0,0,0,7000); + //pos_correction(-519,6600,-180,0,0,30); + calc_xy(-180,0,0); + enc_correction(1,1); + gogo_straight(1,1,-519,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180); + purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180); + purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180); + gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180); + MaxonControl(0,0,0,0); + set_cond(2,1,-3500,0,6000); + //pos_correction(-2962,5500,-180,0,0,30); + enc_correction(1,1); + calc_xy(-180,0,0); + gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90); + gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0); + gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0); + MaxonControl(0,0,0,0); + set_cond(2,1,-3500,1,2000); + pos_correction(-3112,3500,0,0,0,15); + enc_correction(1,1); + MaxonControl(0,0,0,0);*/ + if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり if(id1_value[6] != flag)break; } } + + break; } //-----wait mode----------------------------------------------------------------------------------------------------------------------// case 0: + // printf("wait\n\r"); calc_xy(0,1,1); ashi_led(); MaxonControl(0,0,0,0);