改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
main.cpp@10:cf5c159e729a, 2019-03-08 (annotated)
- Committer:
- yuki0701
- Date:
- Fri Mar 08 07:09:33 2019 +0000
- Revision:
- 10:cf5c159e729a
- Parent:
- 9:63924280272d
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 | 10:cf5c159e729a | 43 | id1_value[0] = 1; |
yuki0701 | 10:cf5c159e729a | 44 | |
yuki0701 | 10:cf5c159e729a | 45 | //while(1); |
yuki0701 | 10:cf5c159e729a | 46 | |
yuki0701 | 9:63924280272d | 47 | gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 48 | purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 49 | purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 10:cf5c159e729a | 50 | set_cond(2,0,-1050,1,1076); |
yuki0701 | 10:cf5c159e729a | 51 | gogo_straight(0,0,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 52 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 53 | pos_correction(-1672,1400,0,0,0); |
yuki0701 | 9:63924280272d | 54 | enc_correction(1,1); |
yuki0701 | 9:63924280272d | 55 | wait(0.5); |
yuki0701 | 9:63924280272d | 56 | |
yuki0701 | 9:63924280272d | 57 | gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 9:63924280272d | 58 | purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 10:cf5c159e729a | 59 | 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); |
yuki0701 | 10:cf5c159e729a | 60 | gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); |
yuki0701 | 9:63924280272d | 61 | gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90); |
yuki0701 | 9:63924280272d | 62 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 63 | pos_correction(-2962,4500,-90,1,1); |
yuki0701 | 10:cf5c159e729a | 64 | set_cond(2,0,-2462,0,6000); |
yuki0701 | 10:cf5c159e729a | 65 | gogo_straight(0,0,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 66 | MaxonControl(0,0,0,0); |
yuki0701 | 10:cf5c159e729a | 67 | pos_correction(-2850,4500,-90,0,0); |
yuki0701 | 10:cf5c159e729a | 68 | enc_correction(1,1); |
yuki0701 | 9:63924280272d | 69 | wait(0.5); |
yuki0701 | 9:63924280272d | 70 | |
yuki0701 | 9:63924280272d | 71 | gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 72 | purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 73 | purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 10:cf5c159e729a | 74 | purecurve(3,1,1,-1700,6000,-2257,6550,9,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 10:cf5c159e729a | 75 | gogo_straight(1,1,-2257,6550,-2500,6550,1000,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 10:cf5c159e729a | 76 | set_cond(2,1,-3500,1,6050); |
yuki0701 | 10:cf5c159e729a | 77 | gogo_straight(0,0,-2500,6550,-2700,6600,1000,200,5,0.1,10,0.1,800,-90); |
yuki0701 | 9:63924280272d | 78 | MaxonControl(0,0,0,0); |
yuki0701 | 10:cf5c159e729a | 79 | pos_correction(-2700,6600,-90,0,0); |
yuki0701 | 9:63924280272d | 80 | enc_correction(1,1); |
yuki0701 | 9:63924280272d | 81 | wait(0.5); |
yuki0701 | 9:63924280272d | 82 | |
yuki0701 | 10:cf5c159e729a | 83 | gogo_straight(1,1,-2700,6600,-2500,6550,200,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 10:cf5c159e729a | 84 | gogo_straight(1,1,-2500,6550,-1300,6550,1000,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 10:cf5c159e729a | 85 | gogo_straight(1,1,-1300,6550,-1000,6500,1000,1000,5,0.1,10,0.1,800,-90); |
yuki0701 | 10:cf5c159e729a | 86 | purecurve(8,1,1,-1000,6550,-519,6000,9,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 9:63924280272d | 87 | gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 10:cf5c159e729a | 88 | set_cond(2,1,-1000,1,4000); |
yuki0701 | 9:63924280272d | 89 | gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180); |
yuki0701 | 9:63924280272d | 90 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 91 | pos_correction(-519,4500,-180,0,0); |
yuki0701 | 10:cf5c159e729a | 92 | enc_correction(1,1); |
yuki0701 | 9:63924280272d | 93 | MaxonControl(0,0,0,0); |
yuki0701 | 9:63924280272d | 94 | |
yuki0701 | 10:cf5c159e729a | 95 | //以下自動で初期位置に戻るプログラム |
yuki0701 | 10:cf5c159e729a | 96 | wait(3); |
yuki0701 | 10:cf5c159e729a | 97 | |
yuki0701 | 10:cf5c159e729a | 98 | gogo_straight(1,1,-519,4500,-519,5000,200,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 10:cf5c159e729a | 99 | gogo_straight(1,1,-519,5000,-519,6000,1000,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 10:cf5c159e729a | 100 | gogo_straight(1,1,-519,6000,-519,6600,1000,200,5,0.1,10,0.1,600,-180); |
yuki0701 | 10:cf5c159e729a | 101 | MaxonControl(0,0,0,0); |
yuki0701 | 10:cf5c159e729a | 102 | set_cond(2,0,0,0,7000); |
yuki0701 | 10:cf5c159e729a | 103 | pos_correction(-519,6600,-180,0,0); |
yuki0701 | 10:cf5c159e729a | 104 | enc_correction(1,1); |
yuki0701 | 10:cf5c159e729a | 105 | gogo_straight(1,1,-519,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 10:cf5c159e729a | 106 | purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 10:cf5c159e729a | 107 | purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180); |
yuki0701 | 10:cf5c159e729a | 108 | gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180); |
yuki0701 | 10:cf5c159e729a | 109 | MaxonControl(0,0,0,0); |
yuki0701 | 10:cf5c159e729a | 110 | set_cond(2,1,-3500,0,6000); |
yuki0701 | 10:cf5c159e729a | 111 | pos_correction(-2962,5500,-180,0,0); |
yuki0701 | 10:cf5c159e729a | 112 | enc_correction(1,1); |
yuki0701 | 10:cf5c159e729a | 113 | gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90); |
yuki0701 | 10:cf5c159e729a | 114 | gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0); |
yuki0701 | 10:cf5c159e729a | 115 | gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0); |
yuki0701 | 10:cf5c159e729a | 116 | MaxonControl(0,0,0,0); |
yuki0701 | 10:cf5c159e729a | 117 | set_cond(2,1,-3500,1,2000); |
yuki0701 | 10:cf5c159e729a | 118 | pos_correction(-3000,3500,0,0,0); |
yuki0701 | 10:cf5c159e729a | 119 | enc_correction(1,1); |
yuki0701 | 10:cf5c159e729a | 120 | MaxonControl(0,0,0,0); |
yuki0701 | 10:cf5c159e729a | 121 | |
yuki0701 | 9:63924280272d | 122 | #endif |
yuki0701 | 9:63924280272d | 123 | |
yuki0701 | 9:63924280272d | 124 | #ifdef HARUROBO_MAIN_MODE |
la00noix | 6:26724c287387 | 125 | while(1) { |
la00noix | 6:26724c287387 | 126 | |
la00noix | 6:26724c287387 | 127 | switch(id1_value[0]) { |
la00noix | 0:c61c6e4775ca | 128 | //-----auto mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 129 | case 1: |
la00noix | 6:26724c287387 | 130 | |
la00noix | 6:26724c287387 | 131 | switch(id1_value[6]) { |
la00noix | 6:26724c287387 | 132 | case 0: |
la00noix | 6:26724c287387 | 133 | //-----right mode-------------------------------------------------------------------------------------------------------------// |
la00noix | 8:2ba338b4590e | 134 | if(T1 == 0) { |
la00noix | 6:26724c287387 | 135 | |
la00noix | 8:2ba338b4590e | 136 | //スタート位置からみかんの木まで移動 |
la00noix | 8:2ba338b4590e | 137 | wait(0.5); |
la00noix | 8:2ba338b4590e | 138 | T1++; |
la00noix | 8:2ba338b4590e | 139 | |
la00noix | 8:2ba338b4590e | 140 | } |
la00noix | 8:2ba338b4590e | 141 | if(T1 == 1) { |
la00noix | 8:2ba338b4590e | 142 | while(1) { |
la00noix | 8:2ba338b4590e | 143 | wait(0.5); |
la00noix | 8:2ba338b4590e | 144 | if(T1 == 2) { |
la00noix | 8:2ba338b4590e | 145 | break; |
la00noix | 8:2ba338b4590e | 146 | } |
la00noix | 8:2ba338b4590e | 147 | } |
la00noix | 8:2ba338b4590e | 148 | } |
la00noix | 8:2ba338b4590e | 149 | if(T1 == 2) { |
la00noix | 8:2ba338b4590e | 150 | |
la00noix | 8:2ba338b4590e | 151 | //みかんの木から三宝置き場まで移動 |
la00noix | 8:2ba338b4590e | 152 | T1++; |
la00noix | 8:2ba338b4590e | 153 | } |
la00noix | 8:2ba338b4590e | 154 | if(T1 == 3) { |
la00noix | 8:2ba338b4590e | 155 | while(1) { |
la00noix | 8:2ba338b4590e | 156 | wait(0.5); |
la00noix | 8:2ba338b4590e | 157 | if(T1 == 4) { |
la00noix | 8:2ba338b4590e | 158 | break; |
la00noix | 8:2ba338b4590e | 159 | } |
la00noix | 8:2ba338b4590e | 160 | } |
la00noix | 8:2ba338b4590e | 161 | } |
la00noix | 8:2ba338b4590e | 162 | if(T1 == 4) { |
la00noix | 8:2ba338b4590e | 163 | |
la00noix | 8:2ba338b4590e | 164 | //三宝置き場からりんごの木まで移動 |
la00noix | 8:2ba338b4590e | 165 | wait(0.5); |
la00noix | 8:2ba338b4590e | 166 | T1++; |
la00noix | 8:2ba338b4590e | 167 | } |
la00noix | 8:2ba338b4590e | 168 | if(T1 == 5) { |
la00noix | 8:2ba338b4590e | 169 | while(1) { |
la00noix | 8:2ba338b4590e | 170 | |
la00noix | 8:2ba338b4590e | 171 | wait(0.5); |
la00noix | 8:2ba338b4590e | 172 | if(T1 == 6) { |
la00noix | 8:2ba338b4590e | 173 | break; |
la00noix | 8:2ba338b4590e | 174 | } |
la00noix | 8:2ba338b4590e | 175 | } |
la00noix | 8:2ba338b4590e | 176 | } |
la00noix | 8:2ba338b4590e | 177 | if(T1 == 6) { |
la00noix | 8:2ba338b4590e | 178 | |
la00noix | 8:2ba338b4590e | 179 | //りんごの木からお供え台まで移動 |
la00noix | 8:2ba338b4590e | 180 | wait(0.5); |
la00noix | 8:2ba338b4590e | 181 | T1++; |
la00noix | 8:2ba338b4590e | 182 | } |
la00noix | 6:26724c287387 | 183 | break; |
la00noix | 6:26724c287387 | 184 | case 1: |
la00noix | 6:26724c287387 | 185 | //-----left mode--------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 186 | |
la00noix | 6:26724c287387 | 187 | break; |
la00noix | 6:26724c287387 | 188 | } |
la00noix | 6:26724c287387 | 189 | //-----wait mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 190 | case 0: |
la00noix | 6:26724c287387 | 191 | |
la00noix | 6:26724c287387 | 192 | calc_xy(0,1,1); |
la00noix | 6:26724c287387 | 193 | ashi_led(); |
la00noix | 6:26724c287387 | 194 | MaxonControl(0,0,0,0); |
la00noix | 7:44ce34007499 | 195 | go_waitmode = 0; |
la00noix | 6:26724c287387 | 196 | |
la00noix | 6:26724c287387 | 197 | break; |
la00noix | 6:26724c287387 | 198 | //-----manual mode--------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 199 | case 2: |
la00noix | 6:26724c287387 | 200 | |
la00noix | 6:26724c287387 | 201 | ManualOut(250,100,500,200); |
la00noix | 7:44ce34007499 | 202 | go_waitmode = 0; |
la00noix | 6:26724c287387 | 203 | |
la00noix | 6:26724c287387 | 204 | break; |
la00noix | 0:c61c6e4775ca | 205 | } |
la00noix | 0:c61c6e4775ca | 206 | //------------------------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 207 | } |
yuki0701 | 9:63924280272d | 208 | |
yuki0701 | 9:63924280272d | 209 | #endif |
yuki0701 | 9:63924280272d | 210 | |
la00noix | 0:c61c6e4775ca | 211 | } |