2019NHK_teamA
/
2019_A_ver6-2_fixed_phase1
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
main.cpp@18:851f783ec516, 2019-09-07 (annotated)
- Committer:
- yuron
- Date:
- Sat Sep 07 13:17:32 2019 +0000
- Revision:
- 18:851f783ec516
- Parent:
- 17:de3bc1999ae7
- Child:
- 19:f17d2e585973
aa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuron | 14:ab89b6cd9719 | 1 | /* ------------------------------------------------------------------- */ |
yuron | 14:ab89b6cd9719 | 2 | /* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */ |
yuron | 14:ab89b6cd9719 | 3 | /* Nucleo Type: F446RE */ |
yuron | 14:ab89b6cd9719 | 4 | /* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */ |
yuron | 14:ab89b6cd9719 | 5 | /* Sensor: encorder*4 */ |
yuron | 14:ab89b6cd9719 | 6 | /* ------------------------------------------------------------------- */ |
yuron | 17:de3bc1999ae7 | 7 | /* 遠隔非常停止対応 & 移動時のバグを改善と */ |
yuron | 14:ab89b6cd9719 | 8 | /* ------------------------------------------------------------------- */ |
yuron | 0:f73c1b076ae4 | 9 | #include "mbed.h" |
yuron | 0:f73c1b076ae4 | 10 | #include "math.h" |
yuron | 0:f73c1b076ae4 | 11 | #include "QEI.h" |
yuron | 0:f73c1b076ae4 | 12 | #include "PID.h" |
yuron | 5:167327a82430 | 13 | |
yuron | 14:ab89b6cd9719 | 14 | //PIDGain of wheels |
yuron | 14:ab89b6cd9719 | 15 | #define Kp 4500000.0 |
yuron | 14:ab89b6cd9719 | 16 | //#define Kp 10000000.0 |
yuron | 14:ab89b6cd9719 | 17 | #define Ti 0.0 |
yuron | 14:ab89b6cd9719 | 18 | #define Td 0.0 |
yuron | 5:167327a82430 | 19 | |
yuron | 16:05b26003da50 | 20 | #define RED 0 |
yuron | 16:05b26003da50 | 21 | #define BLUE 1 |
yuron | 16:05b26003da50 | 22 | |
yuron | 14:ab89b6cd9719 | 23 | PID migimae(Kp, Ti, Td, 0.001); |
yuron | 14:ab89b6cd9719 | 24 | PID migiusiro(Kp, Ti, Td, 0.001); |
yuron | 14:ab89b6cd9719 | 25 | PID hidarimae(Kp, Ti, Td, 0.001); |
yuron | 14:ab89b6cd9719 | 26 | PID hidariusiro(Kp, Ti, Td, 0.001); |
yuron | 7:7f16fb8b0192 | 27 | |
yuron | 5:167327a82430 | 28 | //前進 |
yuron | 14:ab89b6cd9719 | 29 | PID front_migimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 30 | PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 31 | PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 32 | PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 33 | |
yuron | 5:167327a82430 | 34 | //後進 |
yuron | 14:ab89b6cd9719 | 35 | PID back_migimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 36 | PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 37 | PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 38 | PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 8:3df97287c825 | 39 | |
yuron | 14:ab89b6cd9719 | 40 | //右進 |
yuron | 17:de3bc1999ae7 | 41 | PID right_migimae(6000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 42 | PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 43 | PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 44 | PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001); |
yuron | 9:1359f0c813b1 | 45 | |
yuron | 14:ab89b6cd9719 | 46 | //左進 |
yuron | 17:de3bc1999ae7 | 47 | PID left_migimae(6000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 48 | PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 49 | PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 50 | PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001); |
yuron | 10:b672aa81b226 | 51 | |
yuron | 14:ab89b6cd9719 | 52 | //右旋回 |
yuron | 18:851f783ec516 | 53 | PID turn_right_migimae(30000000.0, 0.0, 0.0, 0.001); |
yuron | 18:851f783ec516 | 54 | PID turn_right_migiusiro(30000000.0, 0.0, 0.0, 0.001); |
yuron | 18:851f783ec516 | 55 | PID turn_right_hidarimae(30000000.0, 0.0, 0.0, 0.001); |
yuron | 18:851f783ec516 | 56 | PID turn_right_hidariusiro(30000000.0, 0.0, 0.0, 0.001); |
yuron | 4:df334779a69e | 57 | |
yuron | 14:ab89b6cd9719 | 58 | //左旋回 |
yuron | 18:851f783ec516 | 59 | PID turn_left_migimae(30000000.0, 0.0, 0.0, 0.001); |
yuron | 18:851f783ec516 | 60 | PID turn_left_migiusiro(30000000.0, 0.0, 0.0, 0.001); |
yuron | 18:851f783ec516 | 61 | PID turn_left_hidarimae(30000000.0, 0.0, 0.0, 0.001); |
yuron | 18:851f783ec516 | 62 | PID turn_left_hidariusiro(30000000.0, 0.0, 0.0, 0.001); |
yuron | 0:f73c1b076ae4 | 63 | |
yuron | 4:df334779a69e | 64 | //MDとの通信ポート |
yuron | 4:df334779a69e | 65 | I2C i2c(PB_9, PB_8); //SDA, SCL |
yuron | 14:ab89b6cd9719 | 66 | |
yuron | 4:df334779a69e | 67 | //PCとの通信ポート |
yuron | 4:df334779a69e | 68 | Serial pc(USBTX, USBRX); //TX, RX |
yuron | 4:df334779a69e | 69 | |
yuron | 17:de3bc1999ae7 | 70 | //特小モジュールとの通信ポート |
yuron | 17:de3bc1999ae7 | 71 | Serial pic(A0, A1); |
yuron | 17:de3bc1999ae7 | 72 | |
yuron | 18:851f783ec516 | 73 | //リミットスイッチ基板との通信ポート |
yuron | 18:851f783ec516 | 74 | Serial limit_serial(PC_12, PD_2); |
yuron | 18:851f783ec516 | 75 | |
yuron | 4:df334779a69e | 76 | //12V停止信号ピン |
yuron | 14:ab89b6cd9719 | 77 | DigitalOut emergency(D11); |
yuron | 4:df334779a69e | 78 | |
yuron | 16:05b26003da50 | 79 | DigitalOut USR_LED1(PB_7); |
yuron | 16:05b26003da50 | 80 | DigitalOut USR_LED2(PC_13); |
yuron | 16:05b26003da50 | 81 | DigitalOut USR_LED3(PC_2); |
yuron | 16:05b26003da50 | 82 | DigitalOut USR_LED4(PC_3); |
yuron | 16:05b26003da50 | 83 | |
yuron | 17:de3bc1999ae7 | 84 | //遠隔非常停止ユニットLED |
yuron | 17:de3bc1999ae7 | 85 | AnalogOut myled(A2); |
yuron | 17:de3bc1999ae7 | 86 | |
yuron | 16:05b26003da50 | 87 | DigitalIn start_switch(PB_12); |
yuron | 8:3df97287c825 | 88 | |
yuron | 14:ab89b6cd9719 | 89 | QEI wheel_x1(PA_8 , PA_6 , NC, 624); |
yuron | 14:ab89b6cd9719 | 90 | QEI wheel_x2(PB_14, PB_13, NC, 624); |
yuron | 14:ab89b6cd9719 | 91 | QEI wheel_y1(PB_1 , PB_15, NC, 624); |
yuron | 14:ab89b6cd9719 | 92 | QEI wheel_y2(PA_12, PA_11, NC, 624); |
yuron | 14:ab89b6cd9719 | 93 | //QEI wheel1(D2, D3, NC, 624); |
yuron | 14:ab89b6cd9719 | 94 | //QEI wheel2(D5, D4, NC, 624); |
yuron | 14:ab89b6cd9719 | 95 | |
yuron | 17:de3bc1999ae7 | 96 | Timer counter; |
yuron | 16:05b26003da50 | 97 | |
yuron | 14:ab89b6cd9719 | 98 | //エンコーダ値格納変数 |
yuron | 14:ab89b6cd9719 | 99 | int x_pulse1, x_pulse2, y_pulse1, y_pulse2; |
yuron | 14:ab89b6cd9719 | 100 | |
yuron | 14:ab89b6cd9719 | 101 | //操作の段階変数 |
yuron | 14:ab89b6cd9719 | 102 | unsigned int phase = 0; |
yuron | 16:05b26003da50 | 103 | unsigned int start_zone = 1; |
yuron | 16:05b26003da50 | 104 | bool zone = RED; |
yuron | 0:f73c1b076ae4 | 105 | |
yuron | 14:ab89b6cd9719 | 106 | //MD送信データ変数 |
yuron | 14:ab89b6cd9719 | 107 | char init_send_data[1]; |
yuron | 14:ab89b6cd9719 | 108 | char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1]; |
yuron | 14:ab89b6cd9719 | 109 | char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1]; |
yuron | 0:f73c1b076ae4 | 110 | |
yuron | 17:de3bc1999ae7 | 111 | //非常停止関連変数 |
yuron | 17:de3bc1999ae7 | 112 | char RDATA; |
yuron | 17:de3bc1999ae7 | 113 | char baff; |
yuron | 17:de3bc1999ae7 | 114 | int flug = 0; |
yuron | 17:de3bc1999ae7 | 115 | |
yuron | 18:851f783ec516 | 116 | int limit_data = 0; |
yuron | 18:851f783ec516 | 117 | |
yuron | 18:851f783ec516 | 118 | unsigned int start_switch_counter = 0; |
yuron | 18:851f783ec516 | 119 | |
yuron | 18:851f783ec516 | 120 | bool front_limit = 0; |
yuron | 18:851f783ec516 | 121 | bool right_limit = 0; |
yuron | 18:851f783ec516 | 122 | bool back_limit = 0; |
yuron | 18:851f783ec516 | 123 | |
yuron | 14:ab89b6cd9719 | 124 | //関数のプロトタイプ宣言 |
yuron | 14:ab89b6cd9719 | 125 | void init(void); |
yuron | 14:ab89b6cd9719 | 126 | void init_send(void); |
yuron | 17:de3bc1999ae7 | 127 | void get(void); |
yuron | 14:ab89b6cd9719 | 128 | void get_pulses(void); |
yuron | 14:ab89b6cd9719 | 129 | void print_pulses(void); |
yuron | 17:de3bc1999ae7 | 130 | void get_emergency(void); |
yuron | 18:851f783ec516 | 131 | void read_limit(void); |
yuron | 17:de3bc1999ae7 | 132 | void front(int target); |
yuron | 17:de3bc1999ae7 | 133 | void back(int target); |
yuron | 17:de3bc1999ae7 | 134 | void right(int target); |
yuron | 17:de3bc1999ae7 | 135 | void left(int target); |
yuron | 17:de3bc1999ae7 | 136 | void turn_right(int target); |
yuron | 17:de3bc1999ae7 | 137 | void turn_left(int target); |
yuron | 18:851f783ec516 | 138 | void stop(void); |
yuron | 17:de3bc1999ae7 | 139 | void front_PID(int target); |
yuron | 17:de3bc1999ae7 | 140 | void back_PID(int target); |
yuron | 17:de3bc1999ae7 | 141 | void right_PID(int target); |
yuron | 17:de3bc1999ae7 | 142 | void left_PID(int target); |
yuron | 17:de3bc1999ae7 | 143 | void turn_right_PID(int target); |
yuron | 17:de3bc1999ae7 | 144 | void turn_left_PID(int target); |
yuron | 14:ab89b6cd9719 | 145 | void dondonkasoku(void); |
yuron | 8:3df97287c825 | 146 | |
yuron | 14:ab89b6cd9719 | 147 | int main(void) { |
yuron | 14:ab89b6cd9719 | 148 | |
yuron | 14:ab89b6cd9719 | 149 | init(); |
yuron | 14:ab89b6cd9719 | 150 | init_send(); |
yuron | 18:851f783ec516 | 151 | zone = BLUE; |
yuron | 18:851f783ec516 | 152 | phase = 1; |
yuron | 16:05b26003da50 | 153 | |
yuron | 14:ab89b6cd9719 | 154 | while(1) { |
yuron | 18:851f783ec516 | 155 | |
yuron | 14:ab89b6cd9719 | 156 | get_pulses(); |
yuron | 14:ab89b6cd9719 | 157 | print_pulses(); |
yuron | 17:de3bc1999ae7 | 158 | get_emergency(); |
yuron | 18:851f783ec516 | 159 | read_limit(); |
yuron | 18:851f783ec516 | 160 | |
yuron | 18:851f783ec516 | 161 | if(zone == BLUE) { |
yuron | 18:851f783ec516 | 162 | switch(phase) { |
yuron | 18:851f783ec516 | 163 | case 0: |
yuron | 18:851f783ec516 | 164 | if(!start_switch) { |
yuron | 18:851f783ec516 | 165 | phase = 1; |
yuron | 18:851f783ec516 | 166 | } |
yuron | 18:851f783ec516 | 167 | case 1: |
yuron | 18:851f783ec516 | 168 | left(12000); |
yuron | 18:851f783ec516 | 169 | if((x_pulse1 > 12000) || (x_pulse2 > 12000)) { |
yuron | 18:851f783ec516 | 170 | phase = 2; |
yuron | 18:851f783ec516 | 171 | } |
yuron | 18:851f783ec516 | 172 | break; |
yuron | 18:851f783ec516 | 173 | case 2: |
yuron | 18:851f783ec516 | 174 | stop(); |
yuron | 18:851f783ec516 | 175 | counter.start(); |
yuron | 18:851f783ec516 | 176 | if(counter.read() > 1.0f) { |
yuron | 18:851f783ec516 | 177 | phase = 3; |
yuron | 18:851f783ec516 | 178 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 179 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 180 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 181 | wheel_y2.reset(); |
yuron | 18:851f783ec516 | 182 | } |
yuron | 18:851f783ec516 | 183 | break; |
yuron | 18:851f783ec516 | 184 | case 3: |
yuron | 18:851f783ec516 | 185 | counter.reset(); |
yuron | 18:851f783ec516 | 186 | turn_right(535); |
yuron | 18:851f783ec516 | 187 | if(x_pulse2 > 535) { |
yuron | 18:851f783ec516 | 188 | phase = 4; |
yuron | 18:851f783ec516 | 189 | } |
yuron | 18:851f783ec516 | 190 | break; |
yuron | 18:851f783ec516 | 191 | case 4: |
yuron | 18:851f783ec516 | 192 | stop(); |
yuron | 18:851f783ec516 | 193 | counter.start(); |
yuron | 18:851f783ec516 | 194 | if(counter.read() > 1.0f) { |
yuron | 18:851f783ec516 | 195 | //本当は5だけど今はリミットスイッチ無の為phase7まで飛ばす |
yuron | 18:851f783ec516 | 196 | //phase = 5; |
yuron | 18:851f783ec516 | 197 | phase = 7; |
yuron | 18:851f783ec516 | 198 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 199 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 200 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 201 | wheel_y2.reset(); |
yuron | 18:851f783ec516 | 202 | } |
yuron | 18:851f783ec516 | 203 | break; |
yuron | 18:851f783ec516 | 204 | case 5: |
yuron | 18:851f783ec516 | 205 | counter.reset(); |
yuron | 18:851f783ec516 | 206 | right(-500); |
yuron | 18:851f783ec516 | 207 | if((x_pulse1*-1 > 500) || (x_pulse2*-1 > 500)) { |
yuron | 18:851f783ec516 | 208 | phase = 6; |
yuron | 18:851f783ec516 | 209 | } |
yuron | 18:851f783ec516 | 210 | //if(!right_limit) { |
yuron | 18:851f783ec516 | 211 | //phase = 6; |
yuron | 18:851f783ec516 | 212 | //} |
yuron | 18:851f783ec516 | 213 | break; |
yuron | 18:851f783ec516 | 214 | case 6: |
yuron | 18:851f783ec516 | 215 | stop(); |
yuron | 18:851f783ec516 | 216 | counter.start(); |
yuron | 18:851f783ec516 | 217 | if(counter.read() > 1.0f) { |
yuron | 18:851f783ec516 | 218 | phase = 7; |
yuron | 18:851f783ec516 | 219 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 220 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 221 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 222 | wheel_y2.reset(); |
yuron | 18:851f783ec516 | 223 | } |
yuron | 18:851f783ec516 | 224 | break; |
yuron | 18:851f783ec516 | 225 | case 7: |
yuron | 18:851f783ec516 | 226 | counter.reset(); |
yuron | 18:851f783ec516 | 227 | front(3000); |
yuron | 18:851f783ec516 | 228 | if((y_pulse1 > 3000) || (y_pulse2 > 3000)) { |
yuron | 18:851f783ec516 | 229 | phase = 8; |
yuron | 18:851f783ec516 | 230 | } |
yuron | 18:851f783ec516 | 231 | break; |
yuron | 18:851f783ec516 | 232 | case 8: |
yuron | 18:851f783ec516 | 233 | stop(); |
yuron | 18:851f783ec516 | 234 | counter.start(); |
yuron | 18:851f783ec516 | 235 | if(counter.read() > 1.0f) { |
yuron | 18:851f783ec516 | 236 | phase = 9; |
yuron | 18:851f783ec516 | 237 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 238 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 239 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 240 | wheel_y2.reset(); |
yuron | 18:851f783ec516 | 241 | } |
yuron | 18:851f783ec516 | 242 | break; |
yuron | 18:851f783ec516 | 243 | case 9: |
yuron | 18:851f783ec516 | 244 | counter.reset(); |
yuron | 18:851f783ec516 | 245 | right(-3000); |
yuron | 18:851f783ec516 | 246 | if((x_pulse1*-1 > 3000) || (x_pulse2*-1 > 3000)) { |
yuron | 18:851f783ec516 | 247 | phase = 10; |
yuron | 18:851f783ec516 | 248 | } |
yuron | 18:851f783ec516 | 249 | //if(!right_limit) { |
yuron | 18:851f783ec516 | 250 | //phase = 10; |
yuron | 18:851f783ec516 | 251 | //} |
yuron | 18:851f783ec516 | 252 | break; |
yuron | 18:851f783ec516 | 253 | case 10: |
yuron | 18:851f783ec516 | 254 | stop(); |
yuron | 18:851f783ec516 | 255 | counter.start(); |
yuron | 18:851f783ec516 | 256 | if(counter.read() > 1.0f) { |
yuron | 18:851f783ec516 | 257 | phase = 11; |
yuron | 18:851f783ec516 | 258 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 259 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 260 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 261 | wheel_y2.reset(); |
yuron | 18:851f783ec516 | 262 | } |
yuron | 18:851f783ec516 | 263 | break; |
yuron | 18:851f783ec516 | 264 | case 11: |
yuron | 18:851f783ec516 | 265 | counter.reset(); |
yuron | 18:851f783ec516 | 266 | front(21500); |
yuron | 18:851f783ec516 | 267 | if((y_pulse1 > 21500) || (y_pulse2 > 21500)) { |
yuron | 18:851f783ec516 | 268 | phase = 12; |
yuron | 18:851f783ec516 | 269 | } |
yuron | 18:851f783ec516 | 270 | break; |
yuron | 18:851f783ec516 | 271 | case 12: |
yuron | 18:851f783ec516 | 272 | stop(); |
yuron | 18:851f783ec516 | 273 | counter.start(); |
yuron | 18:851f783ec516 | 274 | if(counter.read() > 1.0f) { |
yuron | 18:851f783ec516 | 275 | phase = 13; |
yuron | 18:851f783ec516 | 276 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 277 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 278 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 279 | wheel_y2.reset(); |
yuron | 18:851f783ec516 | 280 | } |
yuron | 18:851f783ec516 | 281 | break; |
yuron | 18:851f783ec516 | 282 | case 13: |
yuron | 18:851f783ec516 | 283 | left(8000); |
yuron | 18:851f783ec516 | 284 | if((x_pulse1 > 8000) || (x_pulse2 > 8000)) { |
yuron | 18:851f783ec516 | 285 | phase = 14; |
yuron | 18:851f783ec516 | 286 | } |
yuron | 18:851f783ec516 | 287 | break; |
yuron | 18:851f783ec516 | 288 | case 14: |
yuron | 18:851f783ec516 | 289 | stop(); |
yuron | 18:851f783ec516 | 290 | default: |
yuron | 18:851f783ec516 | 291 | break; |
yuron | 18:851f783ec516 | 292 | } |
yuron | 16:05b26003da50 | 293 | } |
yuron | 18:851f783ec516 | 294 | |
yuron | 18:851f783ec516 | 295 | /* |
yuron | 17:de3bc1999ae7 | 296 | if(counter.read() < 5.00f) { |
yuron | 17:de3bc1999ae7 | 297 | counter.start(); |
yuron | 18:851f783ec516 | 298 | front(3000); |
yuron | 16:05b26003da50 | 299 | } |
yuron | 17:de3bc1999ae7 | 300 | else if(counter.read() >= 5.00f && counter.read() < 10.00f) { |
yuron | 18:851f783ec516 | 301 | right(-3000); |
yuron | 16:05b26003da50 | 302 | } |
yuron | 17:de3bc1999ae7 | 303 | else if(counter.read() >= 10.00f && counter.read() < 15.00f) { |
yuron | 18:851f783ec516 | 304 | back(-3000); |
yuron | 17:de3bc1999ae7 | 305 | } |
yuron | 17:de3bc1999ae7 | 306 | else if(counter.read() >= 15.00f && counter.read() < 20.00f) { |
yuron | 18:851f783ec516 | 307 | left(3000); |
yuron | 16:05b26003da50 | 308 | } |
yuron | 17:de3bc1999ae7 | 309 | else if(counter.read() >= 20.00f && counter.read() < 25.00f) { |
yuron | 18:851f783ec516 | 310 | turn_right(535); |
yuron | 17:de3bc1999ae7 | 311 | } |
yuron | 17:de3bc1999ae7 | 312 | else if(counter.read() >= 25.00f && counter.read() < 30.00f) { |
yuron | 18:851f783ec516 | 313 | turn_left(674); |
yuron | 17:de3bc1999ae7 | 314 | } |
yuron | 17:de3bc1999ae7 | 315 | else if(counter.read() >= 30.00f) { |
yuron | 17:de3bc1999ae7 | 316 | counter.reset(); |
yuron | 17:de3bc1999ae7 | 317 | } |
yuron | 18:851f783ec516 | 318 | */ |
yuron | 16:05b26003da50 | 319 | } |
yuron | 16:05b26003da50 | 320 | } |
yuron | 17:de3bc1999ae7 | 321 | |
yuron | 14:ab89b6cd9719 | 322 | void init(void) { |
yuron | 14:ab89b6cd9719 | 323 | |
yuron | 14:ab89b6cd9719 | 324 | //緊急停止用信号ピンをLow |
yuron | 17:de3bc1999ae7 | 325 | //emergency = 0; |
yuron | 10:b672aa81b226 | 326 | |
yuron | 14:ab89b6cd9719 | 327 | //通信ボーレートの設定 |
yuron | 16:05b26003da50 | 328 | pc.baud(460800); |
yuron | 16:05b26003da50 | 329 | //pc.baud(9600); |
yuron | 16:05b26003da50 | 330 | |
yuron | 18:851f783ec516 | 331 | limit_serial.baud(115200); |
yuron | 18:851f783ec516 | 332 | |
yuron | 16:05b26003da50 | 333 | start_switch.mode(PullUp); |
yuron | 14:ab89b6cd9719 | 334 | |
yuron | 17:de3bc1999ae7 | 335 | //非常停止関連 |
yuron | 17:de3bc1999ae7 | 336 | pic.baud(19200); |
yuron | 17:de3bc1999ae7 | 337 | pic.format(8, Serial::None, 1); |
yuron | 17:de3bc1999ae7 | 338 | pic.attach(get, Serial::RxIrq); |
yuron | 17:de3bc1999ae7 | 339 | |
yuron | 14:ab89b6cd9719 | 340 | x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; |
yuron | 14:ab89b6cd9719 | 341 | migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 342 | true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 343 | } |
yuron | 5:167327a82430 | 344 | |
yuron | 14:ab89b6cd9719 | 345 | void init_send(void) { |
yuron | 14:ab89b6cd9719 | 346 | |
yuron | 14:ab89b6cd9719 | 347 | init_send_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 348 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 349 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 350 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 351 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 352 | wait(0.1); |
yuron | 14:ab89b6cd9719 | 353 | } |
yuron | 0:f73c1b076ae4 | 354 | |
yuron | 17:de3bc1999ae7 | 355 | void get(void) { |
yuron | 17:de3bc1999ae7 | 356 | |
yuron | 17:de3bc1999ae7 | 357 | baff = pic.getc(); |
yuron | 17:de3bc1999ae7 | 358 | |
yuron | 17:de3bc1999ae7 | 359 | for(; flug; flug--) |
yuron | 17:de3bc1999ae7 | 360 | RDATA = baff; |
yuron | 17:de3bc1999ae7 | 361 | |
yuron | 17:de3bc1999ae7 | 362 | if(baff == ':') |
yuron | 17:de3bc1999ae7 | 363 | flug = 1; |
yuron | 17:de3bc1999ae7 | 364 | } |
yuron | 17:de3bc1999ae7 | 365 | |
yuron | 14:ab89b6cd9719 | 366 | void get_pulses(void) { |
yuron | 14:ab89b6cd9719 | 367 | |
yuron | 14:ab89b6cd9719 | 368 | x_pulse1 = wheel_x1.getPulses(); |
yuron | 14:ab89b6cd9719 | 369 | x_pulse2 = wheel_x2.getPulses(); |
yuron | 14:ab89b6cd9719 | 370 | y_pulse1 = wheel_y1.getPulses(); |
yuron | 14:ab89b6cd9719 | 371 | y_pulse2 = wheel_y2.getPulses(); |
yuron | 14:ab89b6cd9719 | 372 | } |
yuron | 0:f73c1b076ae4 | 373 | |
yuron | 14:ab89b6cd9719 | 374 | void print_pulses(void) { |
yuron | 17:de3bc1999ae7 | 375 | |
yuron | 18:851f783ec516 | 376 | //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase); |
yuron | 16:05b26003da50 | 377 | //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]); |
yuron | 14:ab89b6cd9719 | 378 | } |
yuron | 4:df334779a69e | 379 | |
yuron | 17:de3bc1999ae7 | 380 | void get_emergency(void) { |
yuron | 14:ab89b6cd9719 | 381 | |
yuron | 17:de3bc1999ae7 | 382 | if(RDATA == '1') { |
yuron | 17:de3bc1999ae7 | 383 | myled = 1; |
yuron | 17:de3bc1999ae7 | 384 | emergency = 1; |
yuron | 17:de3bc1999ae7 | 385 | } |
yuron | 17:de3bc1999ae7 | 386 | else if(RDATA == '9'){ |
yuron | 17:de3bc1999ae7 | 387 | myled = 0.2; |
yuron | 17:de3bc1999ae7 | 388 | emergency = 0; |
yuron | 17:de3bc1999ae7 | 389 | } |
yuron | 17:de3bc1999ae7 | 390 | } |
yuron | 17:de3bc1999ae7 | 391 | |
yuron | 18:851f783ec516 | 392 | void read_limit(void) { |
yuron | 18:851f783ec516 | 393 | |
yuron | 18:851f783ec516 | 394 | limit_data = limit_serial.getc(); |
yuron | 18:851f783ec516 | 395 | |
yuron | 18:851f783ec516 | 396 | if((limit_data & 0b00000001) == 0x01) { |
yuron | 18:851f783ec516 | 397 | USR_LED1 = 1; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 0; |
yuron | 18:851f783ec516 | 398 | } |
yuron | 18:851f783ec516 | 399 | if((limit_data & 0b00000010) == 0x02) { |
yuron | 18:851f783ec516 | 400 | USR_LED1 = 0; USR_LED2 = 1; USR_LED3 = 0; USR_LED4 = 0; |
yuron | 18:851f783ec516 | 401 | } |
yuron | 18:851f783ec516 | 402 | if((limit_data & 0b00000011) == 0x03) { |
yuron | 18:851f783ec516 | 403 | USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 0; USR_LED4 = 0; |
yuron | 18:851f783ec516 | 404 | } |
yuron | 18:851f783ec516 | 405 | if((limit_data & 0b00000100) == 0x04) { |
yuron | 18:851f783ec516 | 406 | USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 1; USR_LED4 = 0; |
yuron | 18:851f783ec516 | 407 | } |
yuron | 18:851f783ec516 | 408 | if((limit_data & 0b00001000) == 0x08) { |
yuron | 18:851f783ec516 | 409 | USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 1; |
yuron | 18:851f783ec516 | 410 | } |
yuron | 18:851f783ec516 | 411 | if((limit_data & 0b00001100) == 0x0C) { |
yuron | 18:851f783ec516 | 412 | USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 1; USR_LED4 = 1; |
yuron | 18:851f783ec516 | 413 | } |
yuron | 18:851f783ec516 | 414 | if(limit_data == 0x00) { |
yuron | 18:851f783ec516 | 415 | USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 0; |
yuron | 18:851f783ec516 | 416 | } |
yuron | 18:851f783ec516 | 417 | } |
yuron | 18:851f783ec516 | 418 | |
yuron | 17:de3bc1999ae7 | 419 | void front(int target) { |
yuron | 17:de3bc1999ae7 | 420 | |
yuron | 14:ab89b6cd9719 | 421 | front_PID(target); |
yuron | 14:ab89b6cd9719 | 422 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 423 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 424 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 425 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 426 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 427 | } |
yuron | 4:df334779a69e | 428 | |
yuron | 17:de3bc1999ae7 | 429 | void back(int target) { |
yuron | 14:ab89b6cd9719 | 430 | |
yuron | 14:ab89b6cd9719 | 431 | back_PID(target); |
yuron | 14:ab89b6cd9719 | 432 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 433 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 434 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 435 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 436 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 437 | } |
yuron | 5:167327a82430 | 438 | |
yuron | 17:de3bc1999ae7 | 439 | void right(int target) { |
yuron | 14:ab89b6cd9719 | 440 | |
yuron | 14:ab89b6cd9719 | 441 | right_PID(target); |
yuron | 14:ab89b6cd9719 | 442 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 443 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 444 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 445 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 446 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 447 | } |
yuron | 5:167327a82430 | 448 | |
yuron | 17:de3bc1999ae7 | 449 | void left(int target) { |
yuron | 14:ab89b6cd9719 | 450 | |
yuron | 14:ab89b6cd9719 | 451 | left_PID(target); |
yuron | 14:ab89b6cd9719 | 452 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 453 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 454 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 455 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 456 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 457 | } |
yuron | 4:df334779a69e | 458 | |
yuron | 17:de3bc1999ae7 | 459 | void turn_right(int target) { |
yuron | 14:ab89b6cd9719 | 460 | |
yuron | 14:ab89b6cd9719 | 461 | turn_right_PID(target); |
yuron | 14:ab89b6cd9719 | 462 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 463 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 464 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 465 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 466 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 467 | } |
yuron | 4:df334779a69e | 468 | |
yuron | 17:de3bc1999ae7 | 469 | void turn_left(int target) { |
yuron | 14:ab89b6cd9719 | 470 | |
yuron | 14:ab89b6cd9719 | 471 | turn_left_PID(target); |
yuron | 14:ab89b6cd9719 | 472 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 473 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 474 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 475 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 476 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 477 | } |
yuron | 5:167327a82430 | 478 | |
yuron | 18:851f783ec516 | 479 | void stop(void) { |
yuron | 18:851f783ec516 | 480 | |
yuron | 18:851f783ec516 | 481 | true_migimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 482 | true_migiusiro_data[0] = 0x80; |
yuron | 18:851f783ec516 | 483 | true_hidarimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 484 | true_hidariusiro_data[0] = 0x80; |
yuron | 18:851f783ec516 | 485 | |
yuron | 18:851f783ec516 | 486 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 18:851f783ec516 | 487 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 18:851f783ec516 | 488 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 18:851f783ec516 | 489 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 18:851f783ec516 | 490 | wait_us(20); |
yuron | 18:851f783ec516 | 491 | } |
yuron | 18:851f783ec516 | 492 | |
yuron | 17:de3bc1999ae7 | 493 | void front_PID(int target) { |
yuron | 5:167327a82430 | 494 | |
yuron | 14:ab89b6cd9719 | 495 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 16:05b26003da50 | 496 | front_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 497 | front_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 498 | front_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 499 | front_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 5:167327a82430 | 500 | |
yuron | 14:ab89b6cd9719 | 501 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 502 | //正転(目標に達してない) |
yuron | 18:851f783ec516 | 503 | if((y_pulse1 < target) || (y_pulse2 < target)) { |
yuron | 16:05b26003da50 | 504 | front_migimae.setOutputLimits(0x84, 0xF7); |
yuron | 16:05b26003da50 | 505 | front_migiusiro.setOutputLimits(0x84, 0xF7); |
yuron | 18:851f783ec516 | 506 | //front_migimae.setOutputLimits(0x84, 0xFF); |
yuron | 18:851f783ec516 | 507 | //front_migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 16:05b26003da50 | 508 | front_hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 16:05b26003da50 | 509 | front_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 510 | } |
yuron | 18:851f783ec516 | 511 | /* |
yuron | 17:de3bc1999ae7 | 512 | //左側が前に出ちゃった♥(右側だけ回して左側は停止) |
yuron | 17:de3bc1999ae7 | 513 | else if((y_pulse1 < target) && (y_pulse2 > target)) { |
yuron | 18:851f783ec516 | 514 | front_migimae.setOutputLimits(0x84, 0xFF); |
yuron | 18:851f783ec516 | 515 | front_migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 516 | front_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 517 | front_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 518 | } |
yuron | 17:de3bc1999ae7 | 519 | //右側が前に出ちゃった♥(左側だけ回して右側は停止) |
yuron | 17:de3bc1999ae7 | 520 | else if((y_pulse1 > target) && (y_pulse2 < target)) { |
yuron | 16:05b26003da50 | 521 | front_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 16:05b26003da50 | 522 | front_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 523 | front_hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 524 | front_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 525 | } |
yuron | 18:851f783ec516 | 526 | */ |
yuron | 18:851f783ec516 | 527 | //停止(目標より行き過ぎ) |
yuron | 18:851f783ec516 | 528 | else if((y_pulse1 > target) || (y_pulse2 > target)) { |
yuron | 18:851f783ec516 | 529 | front_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 530 | front_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 531 | front_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 532 | front_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 533 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 534 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 535 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 536 | wheel_y2.reset(); |
yuron | 14:ab89b6cd9719 | 537 | } |
yuron | 5:167327a82430 | 538 | |
yuron | 14:ab89b6cd9719 | 539 | //よくわからんやつ |
yuron | 16:05b26003da50 | 540 | front_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 541 | front_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 542 | front_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 543 | front_hidariusiro.setMode(AUTO_MODE); |
yuron | 0:f73c1b076ae4 | 544 | |
yuron | 14:ab89b6cd9719 | 545 | //目標値 |
yuron | 16:05b26003da50 | 546 | front_migimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 547 | front_migiusiro.setSetPoint(target); |
yuron | 16:05b26003da50 | 548 | front_hidarimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 549 | front_hidariusiro.setSetPoint(target); |
yuron | 5:167327a82430 | 550 | |
yuron | 14:ab89b6cd9719 | 551 | //センサ出力 |
yuron | 16:05b26003da50 | 552 | front_migimae.setProcessValue(y_pulse1); |
yuron | 16:05b26003da50 | 553 | front_migiusiro.setProcessValue(y_pulse1); |
yuron | 16:05b26003da50 | 554 | front_hidarimae.setProcessValue(y_pulse2); |
yuron | 16:05b26003da50 | 555 | front_hidariusiro.setProcessValue(y_pulse2); |
yuron | 5:167327a82430 | 556 | |
yuron | 14:ab89b6cd9719 | 557 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 558 | migimae_data[0] = front_migimae.compute(); |
yuron | 16:05b26003da50 | 559 | migiusiro_data[0] = front_migiusiro.compute(); |
yuron | 16:05b26003da50 | 560 | hidarimae_data[0] = front_hidarimae.compute(); |
yuron | 16:05b26003da50 | 561 | hidariusiro_data[0] = front_hidariusiro.compute(); |
yuron | 4:df334779a69e | 562 | |
yuron | 14:ab89b6cd9719 | 563 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 564 | //正転(目標に達してない) |
yuron | 18:851f783ec516 | 565 | if((y_pulse1 < target) || (y_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 566 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 567 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 568 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 569 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 570 | } |
yuron | 18:851f783ec516 | 571 | /* |
yuron | 17:de3bc1999ae7 | 572 | //左側が前に出ちゃった♥(右側だけ回して左側は停止) |
yuron | 17:de3bc1999ae7 | 573 | else if((y_pulse1 < target) && (y_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 574 | true_migimae_data[0] = migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 575 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 576 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 577 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 578 | } |
yuron | 17:de3bc1999ae7 | 579 | //右側が前に出ちゃった♥(左側だけ回して右側は停止) |
yuron | 17:de3bc1999ae7 | 580 | else if((y_pulse1 > target) && (y_pulse2 < target)) { |
yuron | 17:de3bc1999ae7 | 581 | true_migimae_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 582 | true_migiusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 583 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 584 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 585 | } |
yuron | 18:851f783ec516 | 586 | */ |
yuron | 18:851f783ec516 | 587 | //停止(目標より行き過ぎ) |
yuron | 18:851f783ec516 | 588 | else if((y_pulse1 > target) || (y_pulse2 > target)) { |
yuron | 18:851f783ec516 | 589 | true_migimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 590 | true_migiusiro_data[0] = 0x80; |
yuron | 18:851f783ec516 | 591 | true_hidarimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 592 | true_hidariusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 593 | } |
yuron | 5:167327a82430 | 594 | } |
yuron | 5:167327a82430 | 595 | |
yuron | 17:de3bc1999ae7 | 596 | void back_PID(int target) { |
yuron | 14:ab89b6cd9719 | 597 | |
yuron | 14:ab89b6cd9719 | 598 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 16:05b26003da50 | 599 | back_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 600 | back_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 601 | back_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 602 | back_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 5:167327a82430 | 603 | |
yuron | 14:ab89b6cd9719 | 604 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 605 | //逆転(目標に達してない) |
yuron | 18:851f783ec516 | 606 | if((y_pulse1*-1 < target*-1) || (y_pulse2*-1 < target*-1)) { |
yuron | 16:05b26003da50 | 607 | back_migimae.setOutputLimits(0x00, 0x7B); |
yuron | 16:05b26003da50 | 608 | back_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 18:851f783ec516 | 609 | //back_hidarimae.setOutputLimits(0x00, 0x73); |
yuron | 18:851f783ec516 | 610 | //back_hidariusiro.setOutputLimits(0x00, 0x73); |
yuron | 18:851f783ec516 | 611 | back_hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 18:851f783ec516 | 612 | back_hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 613 | } |
yuron | 18:851f783ec516 | 614 | /* |
yuron | 17:de3bc1999ae7 | 615 | //左側が後に出ちゃった♥(右側だけ回して左側は停止) |
yuron | 17:de3bc1999ae7 | 616 | else if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 617 | back_migimae.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 618 | back_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 619 | back_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 620 | back_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 621 | } |
yuron | 17:de3bc1999ae7 | 622 | //右側が後に出ちゃった♥(左側だけ回して右側は停止) |
yuron | 17:de3bc1999ae7 | 623 | else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 < target*-1)) { |
yuron | 16:05b26003da50 | 624 | back_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 16:05b26003da50 | 625 | back_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 626 | back_hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 18:851f783ec516 | 627 | back_hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 628 | } |
yuron | 18:851f783ec516 | 629 | */ |
yuron | 18:851f783ec516 | 630 | //停止(目標より行き過ぎ) |
yuron | 18:851f783ec516 | 631 | else if((y_pulse1*-1 > target*-1) || (y_pulse2*-1 > target*-1)) { |
yuron | 18:851f783ec516 | 632 | back_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 633 | back_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 634 | back_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 635 | back_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 636 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 637 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 638 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 639 | wheel_y2.reset(); |
yuron | 4:df334779a69e | 640 | } |
yuron | 5:167327a82430 | 641 | |
yuron | 14:ab89b6cd9719 | 642 | //よくわからんやつ |
yuron | 16:05b26003da50 | 643 | back_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 644 | back_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 645 | back_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 646 | back_hidariusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 647 | |
yuron | 14:ab89b6cd9719 | 648 | //目標値 |
yuron | 17:de3bc1999ae7 | 649 | back_migimae.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 650 | back_migiusiro.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 651 | back_hidarimae.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 652 | back_hidariusiro.setSetPoint(target*-1); |
yuron | 14:ab89b6cd9719 | 653 | |
yuron | 14:ab89b6cd9719 | 654 | //センサ出力 |
yuron | 17:de3bc1999ae7 | 655 | back_migimae.setProcessValue(y_pulse1*-1); |
yuron | 17:de3bc1999ae7 | 656 | back_migiusiro.setProcessValue(y_pulse1*-1); |
yuron | 17:de3bc1999ae7 | 657 | back_hidarimae.setProcessValue(y_pulse2*-1); |
yuron | 17:de3bc1999ae7 | 658 | back_hidariusiro.setProcessValue(y_pulse2*-1); |
yuron | 14:ab89b6cd9719 | 659 | |
yuron | 14:ab89b6cd9719 | 660 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 661 | migimae_data[0] = back_migimae.compute(); |
yuron | 16:05b26003da50 | 662 | migiusiro_data[0] = back_migiusiro.compute(); |
yuron | 16:05b26003da50 | 663 | hidarimae_data[0] = back_hidarimae.compute(); |
yuron | 16:05b26003da50 | 664 | hidariusiro_data[0] = back_hidariusiro.compute(); |
yuron | 14:ab89b6cd9719 | 665 | |
yuron | 14:ab89b6cd9719 | 666 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 667 | //逆転(目標に達してない) |
yuron | 18:851f783ec516 | 668 | if((y_pulse1*-1 < target*-1) || (y_pulse2*-1 < target*-1)) { |
yuron | 14:ab89b6cd9719 | 669 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 670 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 671 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 672 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 5:167327a82430 | 673 | } |
yuron | 18:851f783ec516 | 674 | /* |
yuron | 17:de3bc1999ae7 | 675 | //左側が後に出ちゃった♥(右側だけ回して左側は停止) |
yuron | 17:de3bc1999ae7 | 676 | else if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 677 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 678 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 679 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 680 | true_hidariusiro_data[0] = 0x80; |
yuron | 5:167327a82430 | 681 | } |
yuron | 17:de3bc1999ae7 | 682 | //右側が後に出ちゃった♥(左側だけ回して右側は停止) |
yuron | 17:de3bc1999ae7 | 683 | else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 < target*-1)) { |
yuron | 17:de3bc1999ae7 | 684 | true_migimae_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 685 | true_migiusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 686 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 687 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 688 | } |
yuron | 18:851f783ec516 | 689 | */ |
yuron | 18:851f783ec516 | 690 | //停止(目標より行き過ぎ) |
yuron | 18:851f783ec516 | 691 | else if((y_pulse1*-1 > target*-1) || (y_pulse2*-1 > target*-1)) { |
yuron | 18:851f783ec516 | 692 | true_migimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 693 | true_migiusiro_data[0] = 0x80; |
yuron | 18:851f783ec516 | 694 | true_hidarimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 695 | true_hidariusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 696 | } |
yuron | 14:ab89b6cd9719 | 697 | } |
yuron | 14:ab89b6cd9719 | 698 | |
yuron | 17:de3bc1999ae7 | 699 | void right_PID(int target) { |
yuron | 14:ab89b6cd9719 | 700 | |
yuron | 14:ab89b6cd9719 | 701 | //センサ出力値の最小、最大 |
yuron | 16:05b26003da50 | 702 | right_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 703 | right_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 704 | right_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 705 | right_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 706 | |
yuron | 14:ab89b6cd9719 | 707 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 708 | //右進(目標まで達していない) |
yuron | 18:851f783ec516 | 709 | if((x_pulse1*-1 < target*-1) || (x_pulse2*-1 < target*-1)) { |
yuron | 18:851f783ec516 | 710 | // right_migimae.setOutputLimits(0x00, 0x6C); |
yuron | 18:851f783ec516 | 711 | right_migimae.setOutputLimits(0x00, 0x7B); |
yuron | 16:05b26003da50 | 712 | right_migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 18:851f783ec516 | 713 | //right_hidarimae.setOutputLimits(0x84, 0xF0); |
yuron | 18:851f783ec516 | 714 | right_hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 16:05b26003da50 | 715 | right_hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 716 | |
yuron | 17:de3bc1999ae7 | 717 | } |
yuron | 18:851f783ec516 | 718 | /* |
yuron | 17:de3bc1999ae7 | 719 | //前側が右に出ちゃった♥(後側だけ回して前側は停止) |
yuron | 17:de3bc1999ae7 | 720 | else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 < target*-1)) { |
yuron | 17:de3bc1999ae7 | 721 | right_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 722 | right_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 723 | right_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 724 | right_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 725 | } |
yuron | 17:de3bc1999ae7 | 726 | //後側が右に出ちゃった♥(前側だけ回して後側は停止) |
yuron | 17:de3bc1999ae7 | 727 | else if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 > target*-1)) { |
yuron | 18:851f783ec516 | 728 | right_migimae.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 729 | right_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 730 | right_hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 731 | right_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 732 | } |
yuron | 18:851f783ec516 | 733 | */ |
yuron | 18:851f783ec516 | 734 | //停止(目標より行き過ぎ) |
yuron | 18:851f783ec516 | 735 | else if((x_pulse1*-1 > target*-1) || (x_pulse2*-1 > target*-1)) { |
yuron | 18:851f783ec516 | 736 | right_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 737 | right_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 738 | right_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 739 | right_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 740 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 741 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 742 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 743 | wheel_y2.reset(); |
yuron | 5:167327a82430 | 744 | } |
yuron | 5:167327a82430 | 745 | |
yuron | 14:ab89b6cd9719 | 746 | //よくわからんやつ |
yuron | 16:05b26003da50 | 747 | right_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 748 | right_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 749 | right_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 750 | right_hidariusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 751 | |
yuron | 14:ab89b6cd9719 | 752 | //目標値 |
yuron | 17:de3bc1999ae7 | 753 | right_migimae.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 754 | right_migiusiro.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 755 | right_hidarimae.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 756 | right_hidariusiro.setSetPoint(target*-1); |
yuron | 8:3df97287c825 | 757 | |
yuron | 14:ab89b6cd9719 | 758 | //センサ出力 |
yuron | 17:de3bc1999ae7 | 759 | right_migimae.setProcessValue(x_pulse1*-1); |
yuron | 17:de3bc1999ae7 | 760 | right_migiusiro.setProcessValue(x_pulse2*-1); |
yuron | 17:de3bc1999ae7 | 761 | right_hidarimae.setProcessValue(x_pulse1*-1); |
yuron | 17:de3bc1999ae7 | 762 | right_hidariusiro.setProcessValue(x_pulse2*-1); |
yuron | 14:ab89b6cd9719 | 763 | |
yuron | 14:ab89b6cd9719 | 764 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 765 | migimae_data[0] = right_migimae.compute(); |
yuron | 16:05b26003da50 | 766 | migiusiro_data[0] = right_migiusiro.compute(); |
yuron | 16:05b26003da50 | 767 | hidarimae_data[0] = right_hidarimae.compute(); |
yuron | 16:05b26003da50 | 768 | hidariusiro_data[0] = right_hidariusiro.compute(); |
yuron | 8:3df97287c825 | 769 | |
yuron | 14:ab89b6cd9719 | 770 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 771 | //右進(目標まで達していない) |
yuron | 18:851f783ec516 | 772 | if((x_pulse1*-1 < target*-1) || (x_pulse2*-1 < target*-1)) { |
yuron | 14:ab89b6cd9719 | 773 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 774 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 775 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 776 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 9:1359f0c813b1 | 777 | } |
yuron | 18:851f783ec516 | 778 | /* |
yuron | 17:de3bc1999ae7 | 779 | //前側が右に出ちゃった♥(後側だけ回して前側は停止) |
yuron | 17:de3bc1999ae7 | 780 | else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 < target*-1)) { |
yuron | 16:05b26003da50 | 781 | true_migimae_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 782 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 783 | true_hidarimae_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 784 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 785 | } |
yuron | 17:de3bc1999ae7 | 786 | //後側が右に出ちゃった♥(前側だけ回して後側は停止) |
yuron | 17:de3bc1999ae7 | 787 | else if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 788 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 16:05b26003da50 | 789 | true_migiusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 790 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 16:05b26003da50 | 791 | true_hidariusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 792 | } |
yuron | 18:851f783ec516 | 793 | */ |
yuron | 17:de3bc1999ae7 | 794 | //左進(目標より行き過ぎ) |
yuron | 18:851f783ec516 | 795 | else if((x_pulse1*-1 > target*-1) || (x_pulse2*-1 > target*-1)) { |
yuron | 18:851f783ec516 | 796 | true_migimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 797 | true_migiusiro_data[0] = 0x80; |
yuron | 18:851f783ec516 | 798 | true_hidarimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 799 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 800 | } |
yuron | 14:ab89b6cd9719 | 801 | } |
yuron | 9:1359f0c813b1 | 802 | |
yuron | 17:de3bc1999ae7 | 803 | void left_PID(int target) { |
yuron | 14:ab89b6cd9719 | 804 | |
yuron | 14:ab89b6cd9719 | 805 | //センサ出力値の最小、最大 |
yuron | 16:05b26003da50 | 806 | left_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 807 | left_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 808 | left_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 809 | left_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 810 | |
yuron | 14:ab89b6cd9719 | 811 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 812 | //左進(目標まで達していない) |
yuron | 18:851f783ec516 | 813 | if((x_pulse1 < target) || (x_pulse2 < target)) { |
yuron | 16:05b26003da50 | 814 | left_migimae.setOutputLimits(0x84, 0xED); |
yuron | 16:05b26003da50 | 815 | left_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 18:851f783ec516 | 816 | left_hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 16:05b26003da50 | 817 | left_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 818 | } |
yuron | 18:851f783ec516 | 819 | /* |
yuron | 17:de3bc1999ae7 | 820 | //前側が左に出ちゃった♥(後側だけ回して前側は停止) |
yuron | 17:de3bc1999ae7 | 821 | else if((x_pulse1 > target) && (x_pulse2 < target)) { |
yuron | 17:de3bc1999ae7 | 822 | left_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 823 | left_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 824 | left_hidarimae.setOutputLimits(0x10, 0x7B); |
yuron | 18:851f783ec516 | 825 | left_hidariusiro.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 826 | } |
yuron | 17:de3bc1999ae7 | 827 | //後側が左に出ちゃった♥(前側だけ回して後側は停止) |
yuron | 17:de3bc1999ae7 | 828 | else if((x_pulse1 < target) && (x_pulse2 > target)) { |
yuron | 18:851f783ec516 | 829 | left_migimae.setOutputLimits(0x94, 0xFF); |
yuron | 18:851f783ec516 | 830 | left_migiusiro.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 831 | left_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 832 | left_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 833 | } |
yuron | 18:851f783ec516 | 834 | */ |
yuron | 18:851f783ec516 | 835 | //停止(目標より行き過ぎ) |
yuron | 18:851f783ec516 | 836 | else if((x_pulse1 > target) || (x_pulse2 > target)) { |
yuron | 18:851f783ec516 | 837 | left_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 838 | left_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 839 | left_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 840 | left_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 841 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 842 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 843 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 844 | wheel_y2.reset(); |
yuron | 8:3df97287c825 | 845 | } |
yuron | 16:05b26003da50 | 846 | |
yuron | 14:ab89b6cd9719 | 847 | //よくわからんやつ |
yuron | 16:05b26003da50 | 848 | left_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 849 | left_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 850 | left_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 851 | left_hidariusiro.setMode(AUTO_MODE); |
yuron | 10:b672aa81b226 | 852 | |
yuron | 14:ab89b6cd9719 | 853 | //目標値 |
yuron | 16:05b26003da50 | 854 | left_migimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 855 | left_migiusiro.setSetPoint(target); |
yuron | 16:05b26003da50 | 856 | left_hidarimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 857 | left_hidariusiro.setSetPoint(target); |
yuron | 9:1359f0c813b1 | 858 | |
yuron | 14:ab89b6cd9719 | 859 | //センサ出力 |
yuron | 16:05b26003da50 | 860 | left_migimae.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 861 | left_migiusiro.setProcessValue(x_pulse2); |
yuron | 16:05b26003da50 | 862 | left_hidarimae.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 863 | left_hidariusiro.setProcessValue(x_pulse2); |
yuron | 8:3df97287c825 | 864 | |
yuron | 14:ab89b6cd9719 | 865 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 866 | migimae_data[0] = left_migimae.compute(); |
yuron | 16:05b26003da50 | 867 | migiusiro_data[0] = left_migiusiro.compute(); |
yuron | 16:05b26003da50 | 868 | hidarimae_data[0] = left_hidarimae.compute(); |
yuron | 16:05b26003da50 | 869 | hidariusiro_data[0] = left_hidariusiro.compute(); |
yuron | 8:3df97287c825 | 870 | |
yuron | 14:ab89b6cd9719 | 871 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 872 | //左進(目標まで達していない) |
yuron | 18:851f783ec516 | 873 | if((x_pulse1 < target) || (x_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 874 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 875 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 876 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 877 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 878 | } |
yuron | 18:851f783ec516 | 879 | /* |
yuron | 17:de3bc1999ae7 | 880 | //前側が左に出ちゃった♥(後側だけ回して前側は停止) |
yuron | 17:de3bc1999ae7 | 881 | else if((x_pulse1 > target) && (x_pulse2 < target)) { |
yuron | 16:05b26003da50 | 882 | true_migimae_data[0] = 0x80; |
yuron | 16:05b26003da50 | 883 | true_migiusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 884 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 885 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 886 | } |
yuron | 17:de3bc1999ae7 | 887 | //後側が左に出ちゃった♥(前側だけ回して後側は停止) |
yuron | 17:de3bc1999ae7 | 888 | else if((x_pulse1 < target) && (x_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 889 | true_migimae_data[0] = migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 890 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 891 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 892 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 893 | } |
yuron | 18:851f783ec516 | 894 | */ |
yuron | 18:851f783ec516 | 895 | //停止(目標より行き過ぎ) |
yuron | 18:851f783ec516 | 896 | else if((x_pulse1 > target) || (x_pulse2 > target)) { |
yuron | 18:851f783ec516 | 897 | true_migimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 898 | true_migiusiro_data[0] = 0x80; |
yuron | 18:851f783ec516 | 899 | true_hidarimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 900 | true_hidariusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 901 | } |
yuron | 14:ab89b6cd9719 | 902 | } |
yuron | 12:1a22b9797004 | 903 | |
yuron | 17:de3bc1999ae7 | 904 | void turn_right_PID(int target) { |
yuron | 14:ab89b6cd9719 | 905 | |
yuron | 14:ab89b6cd9719 | 906 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 16:05b26003da50 | 907 | turn_right_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 908 | turn_right_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 909 | turn_right_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 910 | turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 911 | |
yuron | 14:ab89b6cd9719 | 912 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 913 | //右旋回(目標に達してない) |
yuron | 17:de3bc1999ae7 | 914 | if(x_pulse2 < target) { |
yuron | 17:de3bc1999ae7 | 915 | turn_right_migimae.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 916 | turn_right_migiusiro.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 917 | turn_right_hidarimae.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 918 | turn_right_hidariusiro.setOutputLimits(0x94, 0xFF); |
yuron | 8:3df97287c825 | 919 | } |
yuron | 18:851f783ec516 | 920 | //停止(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 921 | else if(x_pulse2 > target) { |
yuron | 18:851f783ec516 | 922 | turn_right_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 923 | turn_right_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 924 | turn_right_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 925 | turn_right_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 926 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 927 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 928 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 929 | wheel_y2.reset(); |
yuron | 4:df334779a69e | 930 | } |
yuron | 8:3df97287c825 | 931 | |
yuron | 14:ab89b6cd9719 | 932 | //よくわからんやつ |
yuron | 16:05b26003da50 | 933 | turn_right_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 934 | turn_right_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 935 | turn_right_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 936 | turn_right_hidariusiro.setMode(AUTO_MODE); |
yuron | 8:3df97287c825 | 937 | |
yuron | 14:ab89b6cd9719 | 938 | //目標値 |
yuron | 16:05b26003da50 | 939 | turn_right_migimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 940 | turn_right_migiusiro.setSetPoint(target); |
yuron | 16:05b26003da50 | 941 | turn_right_hidarimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 942 | turn_right_hidariusiro.setSetPoint(target); |
yuron | 5:167327a82430 | 943 | |
yuron | 14:ab89b6cd9719 | 944 | //センサ出力 |
yuron | 17:de3bc1999ae7 | 945 | turn_right_migimae.setProcessValue(x_pulse2); |
yuron | 17:de3bc1999ae7 | 946 | turn_right_migiusiro.setProcessValue(x_pulse2); |
yuron | 17:de3bc1999ae7 | 947 | turn_right_hidarimae.setProcessValue(x_pulse2); |
yuron | 17:de3bc1999ae7 | 948 | turn_right_hidariusiro.setProcessValue(x_pulse2); |
yuron | 5:167327a82430 | 949 | |
yuron | 14:ab89b6cd9719 | 950 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 951 | migimae_data[0] = turn_right_migimae.compute(); |
yuron | 16:05b26003da50 | 952 | migiusiro_data[0] = turn_right_migiusiro.compute(); |
yuron | 16:05b26003da50 | 953 | hidarimae_data[0] = turn_right_hidarimae.compute(); |
yuron | 16:05b26003da50 | 954 | hidariusiro_data[0] = turn_right_hidariusiro.compute(); |
yuron | 8:3df97287c825 | 955 | |
yuron | 14:ab89b6cd9719 | 956 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 957 | //右旋回(目標に達してない) |
yuron | 17:de3bc1999ae7 | 958 | if(x_pulse2 < target) { |
yuron | 14:ab89b6cd9719 | 959 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 960 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 961 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 962 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 963 | } |
yuron | 18:851f783ec516 | 964 | //停止(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 965 | else if(x_pulse2 > target) { |
yuron | 18:851f783ec516 | 966 | true_migimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 967 | true_migiusiro_data[0] = 0x80; |
yuron | 18:851f783ec516 | 968 | true_hidarimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 969 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 970 | } |
yuron | 14:ab89b6cd9719 | 971 | } |
yuron | 8:3df97287c825 | 972 | |
yuron | 17:de3bc1999ae7 | 973 | void turn_left_PID(int target) { |
yuron | 14:ab89b6cd9719 | 974 | |
yuron | 14:ab89b6cd9719 | 975 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 16:05b26003da50 | 976 | turn_left_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 977 | turn_left_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 978 | turn_left_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 979 | turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 980 | |
yuron | 14:ab89b6cd9719 | 981 | //制御量の最小、最大 |
yuron | 18:851f783ec516 | 982 | //左旋回(目標に達してない) |
yuron | 16:05b26003da50 | 983 | if(x_pulse1 < target) { |
yuron | 17:de3bc1999ae7 | 984 | turn_left_migimae.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 985 | turn_left_migiusiro.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 986 | turn_left_hidarimae.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 987 | turn_left_hidariusiro.setOutputLimits(0x10, 0x7B); |
yuron | 14:ab89b6cd9719 | 988 | } |
yuron | 18:851f783ec516 | 989 | //停止(目標より行き過ぎ) |
yuron | 16:05b26003da50 | 990 | else if(x_pulse1 > target) { |
yuron | 18:851f783ec516 | 991 | turn_left_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 992 | turn_left_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 993 | turn_left_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 994 | turn_left_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 18:851f783ec516 | 995 | wheel_x1.reset(); |
yuron | 18:851f783ec516 | 996 | wheel_x2.reset(); |
yuron | 18:851f783ec516 | 997 | wheel_y1.reset(); |
yuron | 18:851f783ec516 | 998 | wheel_y2.reset(); |
yuron | 7:7f16fb8b0192 | 999 | } |
yuron | 8:3df97287c825 | 1000 | |
yuron | 14:ab89b6cd9719 | 1001 | //よくわからんやつ |
yuron | 16:05b26003da50 | 1002 | turn_left_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 1003 | turn_left_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 1004 | turn_left_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 1005 | turn_left_hidariusiro.setMode(AUTO_MODE); |
yuron | 5:167327a82430 | 1006 | |
yuron | 14:ab89b6cd9719 | 1007 | //目標値 |
yuron | 16:05b26003da50 | 1008 | turn_left_migimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 1009 | turn_left_migiusiro.setSetPoint(target); |
yuron | 16:05b26003da50 | 1010 | turn_left_hidarimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 1011 | turn_left_hidariusiro.setSetPoint(target); |
yuron | 8:3df97287c825 | 1012 | |
yuron | 14:ab89b6cd9719 | 1013 | //センサ出力 |
yuron | 16:05b26003da50 | 1014 | turn_left_migimae.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 1015 | turn_left_migiusiro.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 1016 | turn_left_hidarimae.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 1017 | turn_left_hidariusiro.setProcessValue(x_pulse1); |
yuron | 5:167327a82430 | 1018 | |
yuron | 14:ab89b6cd9719 | 1019 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 1020 | migimae_data[0] = turn_left_migimae.compute(); |
yuron | 16:05b26003da50 | 1021 | migiusiro_data[0] = turn_left_migiusiro.compute(); |
yuron | 16:05b26003da50 | 1022 | hidarimae_data[0] = turn_left_hidarimae.compute(); |
yuron | 16:05b26003da50 | 1023 | hidariusiro_data[0] = turn_left_hidariusiro.compute(); |
yuron | 5:167327a82430 | 1024 | |
yuron | 14:ab89b6cd9719 | 1025 | //制御量をPWM値に変換 |
yuron | 18:851f783ec516 | 1026 | //左旋回(目標に達してない) |
yuron | 16:05b26003da50 | 1027 | if(x_pulse1 < target) { |
yuron | 14:ab89b6cd9719 | 1028 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 1029 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 1030 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 1031 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 1032 | } |
yuron | 14:ab89b6cd9719 | 1033 | //左旋回(目標より行き過ぎ) |
yuron | 16:05b26003da50 | 1034 | else if(x_pulse1 > target) { |
yuron | 18:851f783ec516 | 1035 | true_migimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 1036 | true_migiusiro_data[0] = 0x80; |
yuron | 18:851f783ec516 | 1037 | true_hidarimae_data[0] = 0x80; |
yuron | 18:851f783ec516 | 1038 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 1039 | } |
yuron | 14:ab89b6cd9719 | 1040 | } |
yuron | 5:167327a82430 | 1041 | |
yuron | 14:ab89b6cd9719 | 1042 | void dondonkasoku(void) { |
yuron | 4:df334779a69e | 1043 | |
yuron | 14:ab89b6cd9719 | 1044 | //どんどん加速(正転) |
yuron | 14:ab89b6cd9719 | 1045 | for(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++){ |
yuron | 14:ab89b6cd9719 | 1046 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1047 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1048 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1049 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1050 | wait(0.05); |
yuron | 14:ab89b6cd9719 | 1051 | } |
yuron | 14:ab89b6cd9719 | 1052 | //どんどん減速(正転) |
yuron | 14:ab89b6cd9719 | 1053 | for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--){ |
yuron | 14:ab89b6cd9719 | 1054 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1055 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1056 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1057 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1058 | wait(0.05); |
yuron | 14:ab89b6cd9719 | 1059 | } |
yuron | 14:ab89b6cd9719 | 1060 | //だんだん加速(逆転) |
yuron | 16:05b26003da50 | 1061 | for(init_send_data[0] = 0x7B; init_send_data[0] > 0x00; init_send_data[0]--){ |
yuron | 14:ab89b6cd9719 | 1062 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1063 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1064 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1065 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1066 | wait(0.05); |
yuron | 0:f73c1b076ae4 | 1067 | } |
yuron | 2:c32991ba628f | 1068 | //だんだん減速(逆転) |
yuron | 14:ab89b6cd9719 | 1069 | for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++){ |
yuron | 14:ab89b6cd9719 | 1070 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1071 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1072 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1073 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 1074 | wait(0.05); |
yuron | 0:f73c1b076ae4 | 1075 | } |
yuron | 0:f73c1b076ae4 | 1076 | } |