改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: main.cpp
- Revision:
- 10:cf5c159e729a
- Parent:
- 9:63924280272d
--- a/main.cpp Thu Mar 07 05:47:18 2019 +0000 +++ b/main.cpp Fri Mar 08 07:09:33 2019 +0000 @@ -40,53 +40,85 @@ #ifdef HARUROBO_TEST_MODE + id1_value[0] = 1; + + //while(1); + 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,-1672,2000,-1672,1400,1000,200,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); MaxonControl(0,0,0,0); - set_cond(2,0,-1243,1,1076); pos_correction(-1672,1400,0,0,0); enc_correction(1,1); wait(0.5); 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); - gogo_straight(1,1,-2962,3000,-2962,4000,1000,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); //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(0,0,-2462,0,0); - gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); + 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); MaxonControl(0,0,0,0); - pos_correction(-2850,4500,-90,0,1); - enc_correction(1,0); + pos_correction(-2850,4500,-90,0,0); + enc_correction(1,1); wait(0.5); 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,6647,9,1000,5,0.1,10,0.1,800,-90); - gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90); - set_cond(2,1,-3500,1,6324); - gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,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); MaxonControl(0,0,0,0); - pos_correction(-2700,6647,-90,0,0); + pos_correction(-2700,6600,-90,0,0); enc_correction(1,1); wait(0.5); - gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90); - gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90); - purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180); + 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(2,1,1243,1,4000); + 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); MaxonControl(0,0,0,0); pos_correction(-519,4500,-180,0,0); + enc_correction(1,1); MaxonControl(0,0,0,0); +//以下自動で初期位置に戻るプログラム + wait(3); + + 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); + 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); + enc_correction(1,1); + 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(-3000,3500,0,0,0); + enc_correction(1,1); + MaxonControl(0,0,0,0); + #endif #ifdef HARUROBO_MAIN_MODE