改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: main.cpp
- Revision:
- 6:26724c287387
- Parent:
- 5:6cebe1c458a9
- Child:
- 7:44ce34007499
diff -r 6cebe1c458a9 -r 26724c287387 main.cpp --- a/main.cpp Sat Mar 02 07:18:38 2019 +0000 +++ b/main.cpp Sat Mar 02 07:48:18 2019 +0000 @@ -13,143 +13,114 @@ //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義 -#define HARUROBO_TEST_MODE //テスト自動プログラム(練習・動作確認用)使用時に定義 - int go_waitmode = 0; //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// -//DigitalOut canread_led(LED1); //canread -> on //can.cpp -//DigitalOut cansend_led(LED2); //cansend -> on //can.cpp +//DigitalOut cansend_led(LED1); //canread -> on //can.cpp +//DigitalOut canread_led(LED2); //cansend -> on //can.cpp //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp //////////////////////////////////////////////////////////////以下main文///////////////////////////////////////////////////////////////// -int main(){ - +int main() +{ UserLoopSetting_maxon(); UserLoopSetting_sensor(); UserLoopSetting_can(); - + #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 - printf("ソースファイル名 : %s¥n", __FILE__); - printf("作成日付 : %s¥n", __DATE__); - printf("作成時刻 : %s¥n", __TIME__); - + printf("ソースファイル名 : %s¥n", __FILE__); + printf("作成日付 : %s¥n", __DATE__); + printf("作成時刻 : %s¥n", __TIME__); + #endif -while(1){ - - id1_value[0] = 1; - - switch(id1_value[0]){ - + while(1) { + + switch(id1_value[0]) { //-----auto mode----------------------------------------------------------------------------------------------------------------------// - case 1: + case 1: + + switch(id1_value[6]) { + case 0: + //-----right mode-------------------------------------------------------------------------------------------------------------// + + break; + case 1: + //-----left mode--------------------------------------------------------------------------------------------------------------// + + if(go_waitmode == 0) { -//-----テスト自動プログラム(練習・動作確認用--------------------------------------------------------------------// -#ifdef HARUROBO_TEST_MODE - - if(go_waitmode == 0){ - - while(1); - //can_start(); - //set_cond(2,1,-700,1,-700); - 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,-1610,2000,9,1000,5,0.1,10,0.1,600,0); - gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0); - MaxonControl(0,0,0,0); - set_cond(2,0,-1243,1,1080); - pos_correction(-1610,1250,0,1,0); - wait(0.5); + //can_start(); + //set_cond(2,1,-700,1,-700); + 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,-1610,2000,9,1000,5,0.1,10,0.1,600,0); + gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0); + MaxonControl(0,0,0,0); + set_cond(2,0,-1243,1,1080); + pos_correction(-1610,1250,0,1,0); + wait(0.5); + + gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0); + purecurve(3,1,1,-1610,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); + 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(1,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); + MaxonControl(0,0,0,0); + pos_correction(-2850,4500,-90,1,1); + wait(0.5); - gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0); - purecurve(3,1,1,-1610,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); - 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(1,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); - MaxonControl(0,0,0,0); - pos_correction(-2850,4500,-90,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(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90); - MaxonControl(0,0,0,0); - pos_correction(-2700,6647,-90,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,-350,6000,9,1000,5,0.1,10,0.1,600,-180); - gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180); - set_cond(2,1,1243,1,4000); - gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180); - MaxonControl(0,0,0,0); - pos_correction(-400,4500,-180,1,1); + 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(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90); + MaxonControl(0,0,0,0); + pos_correction(-2700,6647,-90,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,-350,6000,9,1000,5,0.1,10,0.1,600,-180); + gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180); + set_cond(2,1,1243,1,4000); + gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180); + MaxonControl(0,0,0,0); + pos_correction(-400,4500,-180,1,1); + + go_waitmode = 1; - go_waitmode = 1; - - }else if(go_waitmode == 1){ - - MaxonControl(0,0,0,0); - + } else if(go_waitmode == 1) { + + MaxonControl(0,0,0,0); + + } + break; + } +//-----wait mode----------------------------------------------------------------------------------------------------------------------// + case 0: + + calc_xy(0,1,1); + ashi_led(); + MaxonControl(0,0,0,0); + + break; +//-----manual mode--------------------------------------------------------------------------------------------------------------------// + case 2: + + ManualOut(250,100,500,200); + + break; } - - -#endif -//--------------------------------------------------------------------------------------------------------// - - - - -//-----本番用自動プログラム(右側フィールド用)-------------------------------------------------------------------// -#ifdef HARUROBO_RIGHT_MODE - - //ここに本番用自動プログラム(右側フィールド用)を書く - -#endif -//---------------------------------------------------------------------------------------------------------// - - - -//-----本番用自動プログラム(左側フィールド用)-------------------------------------------------------------------// -#ifdef HARUROBO_LEFT_MODE // - - //ここに本番用自動プログラム(左側フィールド用)を書く - -#endif -//---------------------------------------------------------------------------------------------------------// - MaxonControl(0,0,0,0); - break; - -//-----wait mode----------------------------------------------------------------------------------------------------------------------// - case 0: - - calc_xy(0,1,1); - ashi_led(); - MaxonControl(0,0,0,0); - printf("now_angle: %f, now_x: %f, now_y: %f\n\r",now_angle,now_x,now_y); - - break; - -//-----manual mode--------------------------------------------------------------------------------------------------------------------// - case 2: - - ManualOut(250,100,500,200); - - break; //------------------------------------------------------------------------------------------------------------------------------------// } -} } \ No newline at end of file