3/2
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
main.cpp@5:6cebe1c458a9, 2019-03-02 (annotated)
- Committer:
- yuki0701
- Date:
- Sat Mar 02 07:18:38 2019 +0000
- Revision:
- 5:6cebe1c458a9
- Parent:
- 4:317c53a674fa
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
la00noix | 0:c61c6e4775ca | 1 | #include "EC.h" |
la00noix | 0:c61c6e4775ca | 2 | #include "R1370P.h" |
la00noix | 0:c61c6e4775ca | 3 | #include "move4wheel.h" |
la00noix | 0:c61c6e4775ca | 4 | #include "mbed.h" |
la00noix | 0:c61c6e4775ca | 5 | #include "math.h" |
la00noix | 0:c61c6e4775ca | 6 | #include "PathFollowing.h" |
la00noix | 0:c61c6e4775ca | 7 | #include "movement.h" |
la00noix | 0:c61c6e4775ca | 8 | #include "maxonsetting.h" |
la00noix | 0:c61c6e4775ca | 9 | #include "manual.h" |
la00noix | 0:c61c6e4775ca | 10 | #include "can.h" |
la00noix | 0:c61c6e4775ca | 11 | |
la00noix | 0:c61c6e4775ca | 12 | #define PI 3.141592 |
la00noix | 0:c61c6e4775ca | 13 | |
la00noix | 0:c61c6e4775ca | 14 | //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義 |
la00noix | 0:c61c6e4775ca | 15 | |
la00noix | 0:c61c6e4775ca | 16 | #define HARUROBO_TEST_MODE //テスト自動プログラム(練習・動作確認用)使用時に定義 |
la00noix | 0:c61c6e4775ca | 17 | |
la00noix | 0:c61c6e4775ca | 18 | int go_waitmode = 0; |
la00noix | 0:c61c6e4775ca | 19 | |
la00noix | 0:c61c6e4775ca | 20 | //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// |
la00noix | 0:c61c6e4775ca | 21 | //DigitalOut canread_led(LED1); //canread -> on //can.cpp |
la00noix | 0:c61c6e4775ca | 22 | //DigitalOut cansend_led(LED2); //cansend -> on //can.cpp |
la00noix | 0:c61c6e4775ca | 23 | //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp |
la00noix | 0:c61c6e4775ca | 24 | |
la00noix | 0:c61c6e4775ca | 25 | //////////////////////////////////////////////////////////////以下main文///////////////////////////////////////////////////////////////// |
la00noix | 0:c61c6e4775ca | 26 | |
la00noix | 0:c61c6e4775ca | 27 | int main(){ |
la00noix | 0:c61c6e4775ca | 28 | |
la00noix | 0:c61c6e4775ca | 29 | UserLoopSetting_maxon(); |
la00noix | 0:c61c6e4775ca | 30 | UserLoopSetting_sensor(); |
la00noix | 0:c61c6e4775ca | 31 | UserLoopSetting_can(); |
la00noix | 0:c61c6e4775ca | 32 | |
la00noix | 0:c61c6e4775ca | 33 | #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 |
la00noix | 0:c61c6e4775ca | 34 | |
la00noix | 0:c61c6e4775ca | 35 | printf("ソースファイル名 : %s¥n", __FILE__); |
la00noix | 0:c61c6e4775ca | 36 | printf("作成日付 : %s¥n", __DATE__); |
la00noix | 0:c61c6e4775ca | 37 | printf("作成時刻 : %s¥n", __TIME__); |
la00noix | 0:c61c6e4775ca | 38 | |
la00noix | 0:c61c6e4775ca | 39 | #endif |
la00noix | 0:c61c6e4775ca | 40 | |
la00noix | 0:c61c6e4775ca | 41 | while(1){ |
la00noix | 0:c61c6e4775ca | 42 | |
la00noix | 2:7cba05e70367 | 43 | id1_value[0] = 1; |
yuki0701 | 1:3c11e07da92a | 44 | |
la00noix | 0:c61c6e4775ca | 45 | switch(id1_value[0]){ |
la00noix | 0:c61c6e4775ca | 46 | |
la00noix | 0:c61c6e4775ca | 47 | //-----auto mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 48 | case 1: |
la00noix | 0:c61c6e4775ca | 49 | |
la00noix | 0:c61c6e4775ca | 50 | //-----テスト自動プログラム(練習・動作確認用--------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 51 | #ifdef HARUROBO_TEST_MODE |
la00noix | 0:c61c6e4775ca | 52 | |
la00noix | 0:c61c6e4775ca | 53 | if(go_waitmode == 0){ |
yuki0701 | 1:3c11e07da92a | 54 | |
yuki0701 | 5:6cebe1c458a9 | 55 | while(1); |
la00noix | 2:7cba05e70367 | 56 | //can_start(); |
la00noix | 2:7cba05e70367 | 57 | //set_cond(2,1,-700,1,-700); |
la00noix | 4:317c53a674fa | 58 | gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0); |
la00noix | 4:317c53a674fa | 59 | purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0); |
la00noix | 4:317c53a674fa | 60 | purecurve(8,1,1,-2317,2500,-1610,2000,9,1000,5,0.1,10,0.1,600,0); |
la00noix | 4:317c53a674fa | 61 | gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0); |
yuki0701 | 1:3c11e07da92a | 62 | MaxonControl(0,0,0,0); |
la00noix | 4:317c53a674fa | 63 | set_cond(2,0,-1243,1,1080); |
la00noix | 4:317c53a674fa | 64 | pos_correction(-1610,1250,0,1,0); |
yuki0701 | 1:3c11e07da92a | 65 | wait(0.5); |
yuki0701 | 1:3c11e07da92a | 66 | |
la00noix | 4:317c53a674fa | 67 | gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0); |
la00noix | 4:317c53a674fa | 68 | purecurve(3,1,1,-1610,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 3:8a0faa3b08c3 | 69 | purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); |
yuki0701 | 3:8a0faa3b08c3 | 70 | gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); |
yuki0701 | 3:8a0faa3b08c3 | 71 | gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90); |
yuki0701 | 1:3c11e07da92a | 72 | MaxonControl(0,0,0,0); |
yuki0701 | 3:8a0faa3b08c3 | 73 | pos_correction(-2962,4500,-90,1,1); |
yuki0701 | 1:3c11e07da92a | 74 | |
yuki0701 | 1:3c11e07da92a | 75 | set_cond(0,0,-2462,0,0); |
la00noix | 4:317c53a674fa | 76 | gogo_straight(1,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); |
yuki0701 | 1:3c11e07da92a | 77 | MaxonControl(0,0,0,0); |
la00noix | 4:317c53a674fa | 78 | pos_correction(-2850,4500,-90,1,1); |
yuki0701 | 1:3c11e07da92a | 79 | wait(0.5); |
yuki0701 | 1:3c11e07da92a | 80 | |
yuki0701 | 3:8a0faa3b08c3 | 81 | gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 3:8a0faa3b08c3 | 82 | purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 3:8a0faa3b08c3 | 83 | purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 3:8a0faa3b08c3 | 84 | purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 3:8a0faa3b08c3 | 85 | gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90); |
la00noix | 4:317c53a674fa | 86 | set_cond(2,1,-3500,1,6324); |
la00noix | 4:317c53a674fa | 87 | gogo_straight(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90); |
yuki0701 | 1:3c11e07da92a | 88 | MaxonControl(0,0,0,0); |
la00noix | 4:317c53a674fa | 89 | pos_correction(-2700,6647,-90,1,1); |
yuki0701 | 1:3c11e07da92a | 90 | wait(0.5); |
yuki0701 | 1:3c11e07da92a | 91 | |
yuki0701 | 3:8a0faa3b08c3 | 92 | gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90); |
la00noix | 4:317c53a674fa | 93 | gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90); |
la00noix | 4:317c53a674fa | 94 | purecurve(8,1,1,-1000,6647,-350,6000,9,1000,5,0.1,10,0.1,600,-180); |
la00noix | 4:317c53a674fa | 95 | gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180); |
la00noix | 4:317c53a674fa | 96 | set_cond(2,1,1243,1,4000); |
la00noix | 4:317c53a674fa | 97 | gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180); |
yuki0701 | 1:3c11e07da92a | 98 | MaxonControl(0,0,0,0); |
la00noix | 4:317c53a674fa | 99 | pos_correction(-400,4500,-180,1,1); |
yuki0701 | 1:3c11e07da92a | 100 | |
yuki0701 | 1:3c11e07da92a | 101 | go_waitmode = 1; |
la00noix | 0:c61c6e4775ca | 102 | |
la00noix | 0:c61c6e4775ca | 103 | }else if(go_waitmode == 1){ |
la00noix | 0:c61c6e4775ca | 104 | |
la00noix | 0:c61c6e4775ca | 105 | MaxonControl(0,0,0,0); |
la00noix | 0:c61c6e4775ca | 106 | |
la00noix | 0:c61c6e4775ca | 107 | } |
la00noix | 0:c61c6e4775ca | 108 | |
la00noix | 0:c61c6e4775ca | 109 | |
la00noix | 0:c61c6e4775ca | 110 | #endif |
la00noix | 0:c61c6e4775ca | 111 | //--------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 112 | |
la00noix | 0:c61c6e4775ca | 113 | |
la00noix | 0:c61c6e4775ca | 114 | |
la00noix | 0:c61c6e4775ca | 115 | |
la00noix | 0:c61c6e4775ca | 116 | //-----本番用自動プログラム(右側フィールド用)-------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 117 | #ifdef HARUROBO_RIGHT_MODE |
la00noix | 0:c61c6e4775ca | 118 | |
la00noix | 0:c61c6e4775ca | 119 | //ここに本番用自動プログラム(右側フィールド用)を書く |
la00noix | 0:c61c6e4775ca | 120 | |
la00noix | 0:c61c6e4775ca | 121 | #endif |
la00noix | 0:c61c6e4775ca | 122 | //---------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 123 | |
la00noix | 0:c61c6e4775ca | 124 | |
la00noix | 0:c61c6e4775ca | 125 | |
la00noix | 0:c61c6e4775ca | 126 | //-----本番用自動プログラム(左側フィールド用)-------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 127 | #ifdef HARUROBO_LEFT_MODE // |
la00noix | 0:c61c6e4775ca | 128 | |
la00noix | 0:c61c6e4775ca | 129 | //ここに本番用自動プログラム(左側フィールド用)を書く |
la00noix | 0:c61c6e4775ca | 130 | |
la00noix | 0:c61c6e4775ca | 131 | #endif |
la00noix | 0:c61c6e4775ca | 132 | //---------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 133 | MaxonControl(0,0,0,0); |
la00noix | 0:c61c6e4775ca | 134 | break; |
la00noix | 0:c61c6e4775ca | 135 | |
la00noix | 0:c61c6e4775ca | 136 | //-----wait mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 137 | case 0: |
la00noix | 0:c61c6e4775ca | 138 | |
la00noix | 0:c61c6e4775ca | 139 | calc_xy(0,1,1); |
la00noix | 0:c61c6e4775ca | 140 | ashi_led(); |
la00noix | 0:c61c6e4775ca | 141 | MaxonControl(0,0,0,0); |
yuki0701 | 1:3c11e07da92a | 142 | printf("now_angle: %f, now_x: %f, now_y: %f\n\r",now_angle,now_x,now_y); |
la00noix | 0:c61c6e4775ca | 143 | |
la00noix | 0:c61c6e4775ca | 144 | break; |
la00noix | 0:c61c6e4775ca | 145 | |
la00noix | 0:c61c6e4775ca | 146 | //-----manual mode--------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 147 | case 2: |
la00noix | 0:c61c6e4775ca | 148 | |
la00noix | 0:c61c6e4775ca | 149 | ManualOut(250,100,500,200); |
la00noix | 0:c61c6e4775ca | 150 | |
la00noix | 0:c61c6e4775ca | 151 | break; |
la00noix | 0:c61c6e4775ca | 152 | //------------------------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 153 | } |
la00noix | 0:c61c6e4775ca | 154 | } |
la00noix | 0:c61c6e4775ca | 155 | } |