改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
main.cpp@9:63924280272d, 2019-03-07 (annotated)
- Committer:
- yuki0701
- Date:
- Thu Mar 07 05:47:18 2019 +0000
- Revision:
- 9:63924280272d
- Parent:
- 8:2ba338b4590e
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 //プログラム使用時に使用プログラムの情報を最初に表示する際に定義 |
yuki0701 | 9:63924280272d | 15 | #define HARUROBO_TEST_MODE |
yuki0701 | 9:63924280272d | 16 | //#define HARUROBO_MAIN_MODE |
la00noix | 0:c61c6e4775ca | 17 | |
la00noix | 0:c61c6e4775ca | 18 | int go_waitmode = 0; |
la00noix | 0:c61c6e4775ca | 19 | |
la00noix | 0:c61c6e4775ca | 20 | //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// |
la00noix | 6:26724c287387 | 21 | //DigitalOut cansend_led(LED1); //canread -> on //can.cpp |
la00noix | 6:26724c287387 | 22 | //DigitalOut canread_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 | 6:26724c287387 | 27 | int main() |
la00noix | 6:26724c287387 | 28 | { |
la00noix | 0:c61c6e4775ca | 29 | UserLoopSetting_maxon(); |
la00noix | 0:c61c6e4775ca | 30 | UserLoopSetting_sensor(); |
la00noix | 0:c61c6e4775ca | 31 | UserLoopSetting_can(); |
la00noix | 6:26724c287387 | 32 | |
la00noix | 0:c61c6e4775ca | 33 | #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 |
la00noix | 0:c61c6e4775ca | 34 | |
la00noix | 6:26724c287387 | 35 | printf("ソースファイル名 : %s¥n", __FILE__); |
la00noix | 6:26724c287387 | 36 | printf("作成日付 : %s¥n", __DATE__); |
la00noix | 6:26724c287387 | 37 | printf("作成時刻 : %s¥n", __TIME__); |
la00noix | 6:26724c287387 | 38 | |
la00noix | 0:c61c6e4775ca | 39 | #endif |
la00noix | 0:c61c6e4775ca | 40 | |
yuki0701 | 9:63924280272d | 41 | #ifdef HARUROBO_TEST_MODE |
yuki0701 | 9:63924280272d | 42 | |
yuki0701 | 9:63924280272d | 43 | gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 44 | purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 45 | purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 46 | gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 47 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 48 | set_cond(2,0,-1243,1,1076); |
yuki0701 | 9:63924280272d | 49 | pos_correction(-1672,1400,0,0,0); |
yuki0701 | 9:63924280272d | 50 | enc_correction(1,1); |
yuki0701 | 9:63924280272d | 51 | wait(0.5); |
yuki0701 | 9:63924280272d | 52 | |
yuki0701 | 9:63924280272d | 53 | gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 54 | purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 55 | purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); |
yuki0701 | 9:63924280272d | 56 | gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); |
yuki0701 | 9:63924280272d | 57 | gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90); |
yuki0701 | 9:63924280272d | 58 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 59 | pos_correction(-2962,4500,-90,1,1); |
yuki0701 | 9:63924280272d | 60 | |
yuki0701 | 9:63924280272d | 61 | set_cond(0,0,-2462,0,0); |
yuki0701 | 9:63924280272d | 62 | gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 63 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 64 | pos_correction(-2850,4500,-90,0,1); |
yuki0701 | 9:63924280272d | 65 | enc_correction(1,0); |
yuki0701 | 9:63924280272d | 66 | wait(0.5); |
yuki0701 | 9:63924280272d | 67 | |
yuki0701 | 9:63924280272d | 68 | gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 69 | purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 70 | purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 71 | purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 72 | gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 73 | set_cond(2,1,-3500,1,6324); |
yuki0701 | 9:63924280272d | 74 | gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 75 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 76 | pos_correction(-2700,6647,-90,0,0); |
yuki0701 | 9:63924280272d | 77 | enc_correction(1,1); |
yuki0701 | 9:63924280272d | 78 | wait(0.5); |
yuki0701 | 9:63924280272d | 79 | |
yuki0701 | 9:63924280272d | 80 | gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 81 | gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 82 | purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 9:63924280272d | 83 | gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 9:63924280272d | 84 | set_cond(2,1,1243,1,4000); |
yuki0701 | 9:63924280272d | 85 | gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180); |
yuki0701 | 9:63924280272d | 86 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 87 | pos_correction(-519,4500,-180,0,0); |
yuki0701 | 9:63924280272d | 88 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 89 | |
yuki0701 | 9:63924280272d | 90 | #endif |
yuki0701 | 9:63924280272d | 91 | |
yuki0701 | 9:63924280272d | 92 | #ifdef HARUROBO_MAIN_MODE |
la00noix | 6:26724c287387 | 93 | while(1) { |
la00noix | 6:26724c287387 | 94 | |
la00noix | 6:26724c287387 | 95 | switch(id1_value[0]) { |
la00noix | 0:c61c6e4775ca | 96 | //-----auto mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 97 | case 1: |
la00noix | 6:26724c287387 | 98 | |
la00noix | 6:26724c287387 | 99 | switch(id1_value[6]) { |
la00noix | 6:26724c287387 | 100 | case 0: |
la00noix | 6:26724c287387 | 101 | //-----right mode-------------------------------------------------------------------------------------------------------------// |
la00noix | 8:2ba338b4590e | 102 | if(T1 == 0) { |
la00noix | 6:26724c287387 | 103 | |
la00noix | 8:2ba338b4590e | 104 | //スタート位置からみかんの木まで移動 |
la00noix | 8:2ba338b4590e | 105 | wait(0.5); |
la00noix | 8:2ba338b4590e | 106 | T1++; |
la00noix | 8:2ba338b4590e | 107 | |
la00noix | 8:2ba338b4590e | 108 | } |
la00noix | 8:2ba338b4590e | 109 | if(T1 == 1) { |
la00noix | 8:2ba338b4590e | 110 | while(1) { |
la00noix | 8:2ba338b4590e | 111 | wait(0.5); |
la00noix | 8:2ba338b4590e | 112 | if(T1 == 2) { |
la00noix | 8:2ba338b4590e | 113 | break; |
la00noix | 8:2ba338b4590e | 114 | } |
la00noix | 8:2ba338b4590e | 115 | } |
la00noix | 8:2ba338b4590e | 116 | } |
la00noix | 8:2ba338b4590e | 117 | if(T1 == 2) { |
la00noix | 8:2ba338b4590e | 118 | |
la00noix | 8:2ba338b4590e | 119 | //みかんの木から三宝置き場まで移動 |
la00noix | 8:2ba338b4590e | 120 | T1++; |
la00noix | 8:2ba338b4590e | 121 | } |
la00noix | 8:2ba338b4590e | 122 | if(T1 == 3) { |
la00noix | 8:2ba338b4590e | 123 | while(1) { |
la00noix | 8:2ba338b4590e | 124 | wait(0.5); |
la00noix | 8:2ba338b4590e | 125 | if(T1 == 4) { |
la00noix | 8:2ba338b4590e | 126 | break; |
la00noix | 8:2ba338b4590e | 127 | } |
la00noix | 8:2ba338b4590e | 128 | } |
la00noix | 8:2ba338b4590e | 129 | } |
la00noix | 8:2ba338b4590e | 130 | if(T1 == 4) { |
la00noix | 8:2ba338b4590e | 131 | |
la00noix | 8:2ba338b4590e | 132 | //三宝置き場からりんごの木まで移動 |
la00noix | 8:2ba338b4590e | 133 | wait(0.5); |
la00noix | 8:2ba338b4590e | 134 | T1++; |
la00noix | 8:2ba338b4590e | 135 | } |
la00noix | 8:2ba338b4590e | 136 | if(T1 == 5) { |
la00noix | 8:2ba338b4590e | 137 | while(1) { |
la00noix | 8:2ba338b4590e | 138 | |
la00noix | 8:2ba338b4590e | 139 | wait(0.5); |
la00noix | 8:2ba338b4590e | 140 | if(T1 == 6) { |
la00noix | 8:2ba338b4590e | 141 | break; |
la00noix | 8:2ba338b4590e | 142 | } |
la00noix | 8:2ba338b4590e | 143 | } |
la00noix | 8:2ba338b4590e | 144 | } |
la00noix | 8:2ba338b4590e | 145 | if(T1 == 6) { |
la00noix | 8:2ba338b4590e | 146 | |
la00noix | 8:2ba338b4590e | 147 | //りんごの木からお供え台まで移動 |
la00noix | 8:2ba338b4590e | 148 | wait(0.5); |
la00noix | 8:2ba338b4590e | 149 | T1++; |
la00noix | 8:2ba338b4590e | 150 | } |
la00noix | 6:26724c287387 | 151 | break; |
la00noix | 6:26724c287387 | 152 | case 1: |
la00noix | 6:26724c287387 | 153 | //-----left mode--------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 154 | |
la00noix | 6:26724c287387 | 155 | break; |
la00noix | 6:26724c287387 | 156 | } |
la00noix | 6:26724c287387 | 157 | //-----wait mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 158 | case 0: |
la00noix | 6:26724c287387 | 159 | |
la00noix | 6:26724c287387 | 160 | calc_xy(0,1,1); |
la00noix | 6:26724c287387 | 161 | ashi_led(); |
la00noix | 6:26724c287387 | 162 | MaxonControl(0,0,0,0); |
la00noix | 7:44ce34007499 | 163 | go_waitmode = 0; |
la00noix | 6:26724c287387 | 164 | |
la00noix | 6:26724c287387 | 165 | break; |
la00noix | 6:26724c287387 | 166 | //-----manual mode--------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 167 | case 2: |
la00noix | 6:26724c287387 | 168 | |
la00noix | 6:26724c287387 | 169 | ManualOut(250,100,500,200); |
la00noix | 7:44ce34007499 | 170 | go_waitmode = 0; |
la00noix | 6:26724c287387 | 171 | |
la00noix | 6:26724c287387 | 172 | break; |
la00noix | 0:c61c6e4775ca | 173 | } |
la00noix | 0:c61c6e4775ca | 174 | //------------------------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 175 | } |
yuki0701 | 9:63924280272d | 176 | |
yuki0701 | 9:63924280272d | 177 | #endif |
yuki0701 | 9:63924280272d | 178 | |
la00noix | 0:c61c6e4775ca | 179 | } |