3/19
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: main.cpp
- Revision:
- 10:c741191360de
- Parent:
- 9:7667dcfc7ce5
- Child:
- 11:9db93bce4eef
--- a/main.cpp Wed Mar 13 03:24:49 2019 +0000 +++ b/main.cpp Wed Mar 13 04:23:18 2019 +0000 @@ -46,6 +46,101 @@ 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); + MaxonControl(0,0,0,0); + pos_correction(1672,1400,0,0,0); + enc_correction(1,1); + wait(0.5); + T1++; + } + if(T1 == 1) { + while(1) { + wait(0.1); + if(T1 == 2) { + break; + } + } + } + if(T1 == 2) { //みかんの木から三宝置き場まで移動 + 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); + 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); + enc_correction(1,1); + wait(0.5); + T1++; + } + if(T1 == 3) { + while(1) { + wait(0.1); + if(T1 == 4) { + break; + } + } + } + if(T1 == 4) { //三宝置き場からりんごの木まで移動 + 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); + gogo_straight(1,1,2257,6550,2500,6550,1000,1000,5,0.1,10,0.1,800,90); + 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); + enc_correction(1,1); + wait(0.5); + T1++; + } + if(T1 == 5) { + while(1) { + wait(0.1); + if(T1 == 6) { + 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); + MaxonControl(0,0,0,0); + pos_correction(519,4500,180,0,0); + 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); + if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり + if(id1_value[6] != flag)break; + } + } + break; + + case 1: + //-----left mode--------------------------------------------------------------------------------------------------------------// + flag = 1; + 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); @@ -134,10 +229,6 @@ if(id1_value[6] != flag)break; } } - break; - case 1: - //-----left mode--------------------------------------------------------------------------------------------------------------// - flag = 1; break; }