2019NHK_teamA
/
2019_A_ver6-2_fixed_phase1
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
main.cpp@17:de3bc1999ae7, 2019-09-03 (annotated)
- Committer:
- yuron
- Date:
- Tue Sep 03 05:47:40 2019 +0000
- Revision:
- 17:de3bc1999ae7
- Parent:
- 16:05b26003da50
- Child:
- 18:851f783ec516
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 | 17:de3bc1999ae7 | 53 | PID turn_right_migimae(18000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 54 | PID turn_right_migiusiro(18000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 55 | PID turn_right_hidarimae(18000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 56 | PID turn_right_hidariusiro(18000000.0, 0.0, 0.0, 0.001); |
yuron | 4:df334779a69e | 57 | |
yuron | 14:ab89b6cd9719 | 58 | //左旋回 |
yuron | 17:de3bc1999ae7 | 59 | PID turn_left_migimae(18000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 60 | PID turn_left_migiusiro(18000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 61 | PID turn_left_hidarimae(18000000.0, 0.0, 0.0, 0.001); |
yuron | 17:de3bc1999ae7 | 62 | PID turn_left_hidariusiro(18000000.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 | 4:df334779a69e | 73 | //12V停止信号ピン |
yuron | 14:ab89b6cd9719 | 74 | DigitalOut emergency(D11); |
yuron | 4:df334779a69e | 75 | |
yuron | 16:05b26003da50 | 76 | DigitalOut USR_LED1(PB_7); |
yuron | 16:05b26003da50 | 77 | DigitalOut USR_LED2(PC_13); |
yuron | 16:05b26003da50 | 78 | DigitalOut USR_LED3(PC_2); |
yuron | 16:05b26003da50 | 79 | DigitalOut USR_LED4(PC_3); |
yuron | 16:05b26003da50 | 80 | |
yuron | 17:de3bc1999ae7 | 81 | //遠隔非常停止ユニットLED |
yuron | 17:de3bc1999ae7 | 82 | AnalogOut myled(A2); |
yuron | 17:de3bc1999ae7 | 83 | |
yuron | 16:05b26003da50 | 84 | DigitalIn start_switch(PB_12); |
yuron | 8:3df97287c825 | 85 | |
yuron | 14:ab89b6cd9719 | 86 | QEI wheel_x1(PA_8 , PA_6 , NC, 624); |
yuron | 14:ab89b6cd9719 | 87 | QEI wheel_x2(PB_14, PB_13, NC, 624); |
yuron | 14:ab89b6cd9719 | 88 | QEI wheel_y1(PB_1 , PB_15, NC, 624); |
yuron | 14:ab89b6cd9719 | 89 | QEI wheel_y2(PA_12, PA_11, NC, 624); |
yuron | 14:ab89b6cd9719 | 90 | //QEI wheel1(D2, D3, NC, 624); |
yuron | 14:ab89b6cd9719 | 91 | //QEI wheel2(D5, D4, NC, 624); |
yuron | 14:ab89b6cd9719 | 92 | |
yuron | 17:de3bc1999ae7 | 93 | Ticker look_switch; |
yuron | 17:de3bc1999ae7 | 94 | |
yuron | 17:de3bc1999ae7 | 95 | Timer counter; |
yuron | 16:05b26003da50 | 96 | |
yuron | 14:ab89b6cd9719 | 97 | //エンコーダ値格納変数 |
yuron | 14:ab89b6cd9719 | 98 | int x_pulse1, x_pulse2, y_pulse1, y_pulse2; |
yuron | 14:ab89b6cd9719 | 99 | |
yuron | 14:ab89b6cd9719 | 100 | //操作の段階変数 |
yuron | 14:ab89b6cd9719 | 101 | unsigned int phase = 0; |
yuron | 16:05b26003da50 | 102 | unsigned int start_zone = 1; |
yuron | 16:05b26003da50 | 103 | bool zone = RED; |
yuron | 0:f73c1b076ae4 | 104 | |
yuron | 14:ab89b6cd9719 | 105 | //MD送信データ変数 |
yuron | 14:ab89b6cd9719 | 106 | char init_send_data[1]; |
yuron | 14:ab89b6cd9719 | 107 | char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1]; |
yuron | 14:ab89b6cd9719 | 108 | char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1]; |
yuron | 0:f73c1b076ae4 | 109 | |
yuron | 17:de3bc1999ae7 | 110 | //非常停止関連変数 |
yuron | 17:de3bc1999ae7 | 111 | char RDATA; |
yuron | 17:de3bc1999ae7 | 112 | char baff; |
yuron | 17:de3bc1999ae7 | 113 | int flug = 0; |
yuron | 17:de3bc1999ae7 | 114 | |
yuron | 14:ab89b6cd9719 | 115 | //関数のプロトタイプ宣言 |
yuron | 16:05b26003da50 | 116 | void incriment(void); |
yuron | 14:ab89b6cd9719 | 117 | void init(void); |
yuron | 14:ab89b6cd9719 | 118 | void init_send(void); |
yuron | 17:de3bc1999ae7 | 119 | void get(void); |
yuron | 14:ab89b6cd9719 | 120 | void get_pulses(void); |
yuron | 14:ab89b6cd9719 | 121 | void print_pulses(void); |
yuron | 17:de3bc1999ae7 | 122 | void get_emergency(void); |
yuron | 17:de3bc1999ae7 | 123 | void front(int target); |
yuron | 17:de3bc1999ae7 | 124 | void back(int target); |
yuron | 17:de3bc1999ae7 | 125 | void right(int target); |
yuron | 17:de3bc1999ae7 | 126 | void left(int target); |
yuron | 17:de3bc1999ae7 | 127 | void turn_right(int target); |
yuron | 17:de3bc1999ae7 | 128 | void turn_left(int target); |
yuron | 17:de3bc1999ae7 | 129 | void front_PID(int target); |
yuron | 17:de3bc1999ae7 | 130 | void back_PID(int target); |
yuron | 17:de3bc1999ae7 | 131 | void right_PID(int target); |
yuron | 17:de3bc1999ae7 | 132 | void left_PID(int target); |
yuron | 17:de3bc1999ae7 | 133 | void turn_right_PID(int target); |
yuron | 17:de3bc1999ae7 | 134 | void turn_left_PID(int target); |
yuron | 14:ab89b6cd9719 | 135 | void dondonkasoku(void); |
yuron | 8:3df97287c825 | 136 | |
yuron | 14:ab89b6cd9719 | 137 | int main(void) { |
yuron | 14:ab89b6cd9719 | 138 | |
yuron | 14:ab89b6cd9719 | 139 | init(); |
yuron | 14:ab89b6cd9719 | 140 | init_send(); |
yuron | 16:05b26003da50 | 141 | |
yuron | 16:05b26003da50 | 142 | //look_switch.attach_us(&incriment, 100000.0); |
yuron | 16:05b26003da50 | 143 | |
yuron | 17:de3bc1999ae7 | 144 | /* |
yuron | 16:05b26003da50 | 145 | while(1) { |
yuron | 16:05b26003da50 | 146 | if(!start_switch) |
yuron | 16:05b26003da50 | 147 | break; |
yuron | 16:05b26003da50 | 148 | } |
yuron | 17:de3bc1999ae7 | 149 | */ |
yuron | 16:05b26003da50 | 150 | |
yuron | 14:ab89b6cd9719 | 151 | while(1) { |
yuron | 14:ab89b6cd9719 | 152 | |
yuron | 14:ab89b6cd9719 | 153 | get_pulses(); |
yuron | 14:ab89b6cd9719 | 154 | print_pulses(); |
yuron | 17:de3bc1999ae7 | 155 | get_emergency(); |
yuron | 16:05b26003da50 | 156 | |
yuron | 16:05b26003da50 | 157 | if(start_switch == 0) { |
yuron | 16:05b26003da50 | 158 | USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1; |
yuron | 16:05b26003da50 | 159 | } else { |
yuron | 16:05b26003da50 | 160 | USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 0; |
yuron | 16:05b26003da50 | 161 | } |
yuron | 17:de3bc1999ae7 | 162 | |
yuron | 17:de3bc1999ae7 | 163 | //front(5000); |
yuron | 17:de3bc1999ae7 | 164 | //back(-5000); |
yuron | 17:de3bc1999ae7 | 165 | //right(-5000); |
yuron | 17:de3bc1999ae7 | 166 | //left(5000); |
yuron | 17:de3bc1999ae7 | 167 | //turn_right(550); |
yuron | 17:de3bc1999ae7 | 168 | //turn_left(600); |
yuron | 17:de3bc1999ae7 | 169 | |
yuron | 17:de3bc1999ae7 | 170 | if(counter.read() < 5.00f) { |
yuron | 17:de3bc1999ae7 | 171 | counter.start(); |
yuron | 17:de3bc1999ae7 | 172 | front(1000); |
yuron | 16:05b26003da50 | 173 | } |
yuron | 17:de3bc1999ae7 | 174 | else if(counter.read() >= 5.00f && counter.read() < 10.00f) { |
yuron | 17:de3bc1999ae7 | 175 | right(-1000); |
yuron | 16:05b26003da50 | 176 | } |
yuron | 17:de3bc1999ae7 | 177 | else if(counter.read() >= 10.00f && counter.read() < 15.00f) { |
yuron | 17:de3bc1999ae7 | 178 | back(-1000); |
yuron | 17:de3bc1999ae7 | 179 | } |
yuron | 17:de3bc1999ae7 | 180 | else if(counter.read() >= 15.00f && counter.read() < 20.00f) { |
yuron | 17:de3bc1999ae7 | 181 | left(1000); |
yuron | 16:05b26003da50 | 182 | } |
yuron | 17:de3bc1999ae7 | 183 | else if(counter.read() >= 20.00f && counter.read() < 25.00f) { |
yuron | 17:de3bc1999ae7 | 184 | turn_right(550); |
yuron | 17:de3bc1999ae7 | 185 | } |
yuron | 17:de3bc1999ae7 | 186 | else if(counter.read() >= 25.00f && counter.read() < 30.00f) { |
yuron | 17:de3bc1999ae7 | 187 | turn_left(600); |
yuron | 17:de3bc1999ae7 | 188 | } |
yuron | 17:de3bc1999ae7 | 189 | else if(counter.read() >= 30.00f) { |
yuron | 17:de3bc1999ae7 | 190 | counter.reset(); |
yuron | 17:de3bc1999ae7 | 191 | } |
yuron | 14:ab89b6cd9719 | 192 | } |
yuron | 14:ab89b6cd9719 | 193 | } |
yuron | 0:f73c1b076ae4 | 194 | |
yuron | 16:05b26003da50 | 195 | void incriment(void) { |
yuron | 17:de3bc1999ae7 | 196 | |
yuron | 16:05b26003da50 | 197 | if(start_switch == 0) { |
yuron | 16:05b26003da50 | 198 | phase++; |
yuron | 16:05b26003da50 | 199 | } |
yuron | 16:05b26003da50 | 200 | } |
yuron | 17:de3bc1999ae7 | 201 | |
yuron | 14:ab89b6cd9719 | 202 | void init(void) { |
yuron | 14:ab89b6cd9719 | 203 | |
yuron | 14:ab89b6cd9719 | 204 | //緊急停止用信号ピンをLow |
yuron | 17:de3bc1999ae7 | 205 | //emergency = 0; |
yuron | 10:b672aa81b226 | 206 | |
yuron | 14:ab89b6cd9719 | 207 | //通信ボーレートの設定 |
yuron | 16:05b26003da50 | 208 | pc.baud(460800); |
yuron | 16:05b26003da50 | 209 | //pc.baud(9600); |
yuron | 16:05b26003da50 | 210 | |
yuron | 16:05b26003da50 | 211 | start_switch.mode(PullUp); |
yuron | 14:ab89b6cd9719 | 212 | |
yuron | 17:de3bc1999ae7 | 213 | //非常停止関連 |
yuron | 17:de3bc1999ae7 | 214 | pic.baud(19200); |
yuron | 17:de3bc1999ae7 | 215 | pic.format(8, Serial::None, 1); |
yuron | 17:de3bc1999ae7 | 216 | pic.attach(get, Serial::RxIrq); |
yuron | 17:de3bc1999ae7 | 217 | |
yuron | 14:ab89b6cd9719 | 218 | x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; |
yuron | 14:ab89b6cd9719 | 219 | migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 220 | true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 221 | } |
yuron | 5:167327a82430 | 222 | |
yuron | 14:ab89b6cd9719 | 223 | void init_send(void) { |
yuron | 14:ab89b6cd9719 | 224 | |
yuron | 14:ab89b6cd9719 | 225 | init_send_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 226 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 227 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 228 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 229 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 230 | wait(0.1); |
yuron | 14:ab89b6cd9719 | 231 | } |
yuron | 0:f73c1b076ae4 | 232 | |
yuron | 17:de3bc1999ae7 | 233 | void get(void) { |
yuron | 17:de3bc1999ae7 | 234 | |
yuron | 17:de3bc1999ae7 | 235 | baff = pic.getc(); |
yuron | 17:de3bc1999ae7 | 236 | |
yuron | 17:de3bc1999ae7 | 237 | for(; flug; flug--) |
yuron | 17:de3bc1999ae7 | 238 | RDATA = baff; |
yuron | 17:de3bc1999ae7 | 239 | |
yuron | 17:de3bc1999ae7 | 240 | if(baff == ':') |
yuron | 17:de3bc1999ae7 | 241 | flug = 1; |
yuron | 17:de3bc1999ae7 | 242 | } |
yuron | 17:de3bc1999ae7 | 243 | |
yuron | 14:ab89b6cd9719 | 244 | void get_pulses(void) { |
yuron | 14:ab89b6cd9719 | 245 | |
yuron | 14:ab89b6cd9719 | 246 | x_pulse1 = wheel_x1.getPulses(); |
yuron | 14:ab89b6cd9719 | 247 | x_pulse2 = wheel_x2.getPulses(); |
yuron | 14:ab89b6cd9719 | 248 | y_pulse1 = wheel_y1.getPulses(); |
yuron | 14:ab89b6cd9719 | 249 | y_pulse2 = wheel_y2.getPulses(); |
yuron | 14:ab89b6cd9719 | 250 | } |
yuron | 0:f73c1b076ae4 | 251 | |
yuron | 14:ab89b6cd9719 | 252 | void print_pulses(void) { |
yuron | 17:de3bc1999ae7 | 253 | |
yuron | 17:de3bc1999ae7 | 254 | //pc.printf("%f %d\n\r", counter.read(), phase); |
yuron | 17:de3bc1999ae7 | 255 | //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2); |
yuron | 16:05b26003da50 | 256 | //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 | 257 | } |
yuron | 4:df334779a69e | 258 | |
yuron | 17:de3bc1999ae7 | 259 | void get_emergency(void) { |
yuron | 14:ab89b6cd9719 | 260 | |
yuron | 17:de3bc1999ae7 | 261 | if(RDATA == '1') { |
yuron | 17:de3bc1999ae7 | 262 | myled = 1; |
yuron | 17:de3bc1999ae7 | 263 | emergency = 1; |
yuron | 17:de3bc1999ae7 | 264 | } |
yuron | 17:de3bc1999ae7 | 265 | else if(RDATA == '9'){ |
yuron | 17:de3bc1999ae7 | 266 | myled = 0.2; |
yuron | 17:de3bc1999ae7 | 267 | emergency = 0; |
yuron | 17:de3bc1999ae7 | 268 | } |
yuron | 17:de3bc1999ae7 | 269 | } |
yuron | 17:de3bc1999ae7 | 270 | |
yuron | 17:de3bc1999ae7 | 271 | void front(int target) { |
yuron | 17:de3bc1999ae7 | 272 | |
yuron | 14:ab89b6cd9719 | 273 | front_PID(target); |
yuron | 14:ab89b6cd9719 | 274 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 275 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 276 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 277 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 278 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 279 | } |
yuron | 4:df334779a69e | 280 | |
yuron | 17:de3bc1999ae7 | 281 | void back(int target) { |
yuron | 14:ab89b6cd9719 | 282 | |
yuron | 14:ab89b6cd9719 | 283 | back_PID(target); |
yuron | 14:ab89b6cd9719 | 284 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 285 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 286 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 287 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 288 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 289 | } |
yuron | 5:167327a82430 | 290 | |
yuron | 17:de3bc1999ae7 | 291 | void right(int target) { |
yuron | 14:ab89b6cd9719 | 292 | |
yuron | 14:ab89b6cd9719 | 293 | right_PID(target); |
yuron | 14:ab89b6cd9719 | 294 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 295 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 296 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 297 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 298 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 299 | } |
yuron | 5:167327a82430 | 300 | |
yuron | 17:de3bc1999ae7 | 301 | void left(int target) { |
yuron | 14:ab89b6cd9719 | 302 | |
yuron | 14:ab89b6cd9719 | 303 | left_PID(target); |
yuron | 14:ab89b6cd9719 | 304 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 305 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 306 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 307 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 308 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 309 | } |
yuron | 4:df334779a69e | 310 | |
yuron | 17:de3bc1999ae7 | 311 | void turn_right(int target) { |
yuron | 14:ab89b6cd9719 | 312 | |
yuron | 14:ab89b6cd9719 | 313 | turn_right_PID(target); |
yuron | 14:ab89b6cd9719 | 314 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 315 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 316 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 317 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 318 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 319 | } |
yuron | 4:df334779a69e | 320 | |
yuron | 17:de3bc1999ae7 | 321 | void turn_left(int target) { |
yuron | 14:ab89b6cd9719 | 322 | |
yuron | 14:ab89b6cd9719 | 323 | turn_left_PID(target); |
yuron | 14:ab89b6cd9719 | 324 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 325 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 326 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 327 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 328 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 329 | } |
yuron | 5:167327a82430 | 330 | |
yuron | 17:de3bc1999ae7 | 331 | void front_PID(int target) { |
yuron | 5:167327a82430 | 332 | |
yuron | 14:ab89b6cd9719 | 333 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 16:05b26003da50 | 334 | front_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 335 | front_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 336 | front_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 337 | front_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 5:167327a82430 | 338 | |
yuron | 14:ab89b6cd9719 | 339 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 340 | //正転(目標に達してない) |
yuron | 17:de3bc1999ae7 | 341 | if((y_pulse1 < target) && (y_pulse2 < target)) { |
yuron | 16:05b26003da50 | 342 | front_migimae.setOutputLimits(0x84, 0xF7); |
yuron | 16:05b26003da50 | 343 | front_migiusiro.setOutputLimits(0x84, 0xF7); |
yuron | 16:05b26003da50 | 344 | front_hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 16:05b26003da50 | 345 | front_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 346 | } |
yuron | 17:de3bc1999ae7 | 347 | //左側が前に出ちゃった♥(右側だけ回して左側は停止) |
yuron | 17:de3bc1999ae7 | 348 | else if((y_pulse1 < target) && (y_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 349 | front_migimae.setOutputLimits(0x84, 0xF7); |
yuron | 17:de3bc1999ae7 | 350 | front_migiusiro.setOutputLimits(0x84, 0xF7); |
yuron | 17:de3bc1999ae7 | 351 | front_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 352 | front_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 353 | } |
yuron | 17:de3bc1999ae7 | 354 | //右側が前に出ちゃった♥(左側だけ回して右側は停止) |
yuron | 17:de3bc1999ae7 | 355 | else if((y_pulse1 > target) && (y_pulse2 < target)) { |
yuron | 16:05b26003da50 | 356 | front_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 16:05b26003da50 | 357 | front_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 358 | front_hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 359 | front_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 360 | } |
yuron | 17:de3bc1999ae7 | 361 | //逆転(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 362 | else if((y_pulse1 > target) && (y_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 363 | front_migimae.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 364 | front_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 365 | front_hidarimae.setOutputLimits(0x00, 0x73); |
yuron | 17:de3bc1999ae7 | 366 | front_hidariusiro.setOutputLimits(0x00, 0x73); |
yuron | 14:ab89b6cd9719 | 367 | } |
yuron | 5:167327a82430 | 368 | |
yuron | 14:ab89b6cd9719 | 369 | //よくわからんやつ |
yuron | 16:05b26003da50 | 370 | front_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 371 | front_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 372 | front_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 373 | front_hidariusiro.setMode(AUTO_MODE); |
yuron | 0:f73c1b076ae4 | 374 | |
yuron | 14:ab89b6cd9719 | 375 | //目標値 |
yuron | 16:05b26003da50 | 376 | front_migimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 377 | front_migiusiro.setSetPoint(target); |
yuron | 16:05b26003da50 | 378 | front_hidarimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 379 | front_hidariusiro.setSetPoint(target); |
yuron | 5:167327a82430 | 380 | |
yuron | 14:ab89b6cd9719 | 381 | //センサ出力 |
yuron | 16:05b26003da50 | 382 | front_migimae.setProcessValue(y_pulse1); |
yuron | 16:05b26003da50 | 383 | front_migiusiro.setProcessValue(y_pulse1); |
yuron | 16:05b26003da50 | 384 | front_hidarimae.setProcessValue(y_pulse2); |
yuron | 16:05b26003da50 | 385 | front_hidariusiro.setProcessValue(y_pulse2); |
yuron | 5:167327a82430 | 386 | |
yuron | 14:ab89b6cd9719 | 387 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 388 | migimae_data[0] = front_migimae.compute(); |
yuron | 16:05b26003da50 | 389 | migiusiro_data[0] = front_migiusiro.compute(); |
yuron | 16:05b26003da50 | 390 | hidarimae_data[0] = front_hidarimae.compute(); |
yuron | 16:05b26003da50 | 391 | hidariusiro_data[0] = front_hidariusiro.compute(); |
yuron | 4:df334779a69e | 392 | |
yuron | 14:ab89b6cd9719 | 393 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 394 | //正転(目標に達してない) |
yuron | 17:de3bc1999ae7 | 395 | if((y_pulse1 < target) && (y_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 396 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 397 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 398 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 399 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 400 | } |
yuron | 17:de3bc1999ae7 | 401 | //左側が前に出ちゃった♥(右側だけ回して左側は停止) |
yuron | 17:de3bc1999ae7 | 402 | else if((y_pulse1 < target) && (y_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 403 | true_migimae_data[0] = migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 404 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 405 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 406 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 407 | } |
yuron | 17:de3bc1999ae7 | 408 | //右側が前に出ちゃった♥(左側だけ回して右側は停止) |
yuron | 17:de3bc1999ae7 | 409 | else if((y_pulse1 > target) && (y_pulse2 < target)) { |
yuron | 17:de3bc1999ae7 | 410 | true_migimae_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 411 | true_migiusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 412 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 413 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 414 | } |
yuron | 17:de3bc1999ae7 | 415 | //逆転(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 416 | else if((y_pulse1 > target) && (y_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 417 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 418 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 419 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 420 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 421 | } |
yuron | 5:167327a82430 | 422 | } |
yuron | 5:167327a82430 | 423 | |
yuron | 17:de3bc1999ae7 | 424 | void back_PID(int target) { |
yuron | 14:ab89b6cd9719 | 425 | |
yuron | 14:ab89b6cd9719 | 426 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 16:05b26003da50 | 427 | back_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 428 | back_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 429 | back_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 430 | back_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 5:167327a82430 | 431 | |
yuron | 14:ab89b6cd9719 | 432 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 433 | //逆転(目標に達してない) |
yuron | 17:de3bc1999ae7 | 434 | if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { |
yuron | 16:05b26003da50 | 435 | back_migimae.setOutputLimits(0x00, 0x7B); |
yuron | 16:05b26003da50 | 436 | back_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 16:05b26003da50 | 437 | back_hidarimae.setOutputLimits(0x00, 0x73); |
yuron | 16:05b26003da50 | 438 | back_hidariusiro.setOutputLimits(0x00, 0x73); |
yuron | 14:ab89b6cd9719 | 439 | } |
yuron | 17:de3bc1999ae7 | 440 | //左側が後に出ちゃった♥(右側だけ回して左側は停止) |
yuron | 17:de3bc1999ae7 | 441 | else if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 442 | back_migimae.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 443 | back_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 444 | back_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 445 | back_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 446 | } |
yuron | 17:de3bc1999ae7 | 447 | //右側が後に出ちゃった♥(左側だけ回して右側は停止) |
yuron | 17:de3bc1999ae7 | 448 | else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 < target*-1)) { |
yuron | 16:05b26003da50 | 449 | back_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 16:05b26003da50 | 450 | back_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 451 | back_hidarimae.setOutputLimits(0x00, 0x73); |
yuron | 17:de3bc1999ae7 | 452 | back_hidariusiro.setOutputLimits(0x00, 0x73); |
yuron | 17:de3bc1999ae7 | 453 | } |
yuron | 17:de3bc1999ae7 | 454 | //正転(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 455 | else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 456 | back_migimae.setOutputLimits(0x84, 0xF7); |
yuron | 17:de3bc1999ae7 | 457 | back_migiusiro.setOutputLimits(0x84, 0xF7); |
yuron | 17:de3bc1999ae7 | 458 | back_hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 459 | back_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 4:df334779a69e | 460 | } |
yuron | 5:167327a82430 | 461 | |
yuron | 14:ab89b6cd9719 | 462 | //よくわからんやつ |
yuron | 16:05b26003da50 | 463 | back_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 464 | back_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 465 | back_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 466 | back_hidariusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 467 | |
yuron | 14:ab89b6cd9719 | 468 | //目標値 |
yuron | 17:de3bc1999ae7 | 469 | back_migimae.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 470 | back_migiusiro.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 471 | back_hidarimae.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 472 | back_hidariusiro.setSetPoint(target*-1); |
yuron | 14:ab89b6cd9719 | 473 | |
yuron | 14:ab89b6cd9719 | 474 | //センサ出力 |
yuron | 17:de3bc1999ae7 | 475 | back_migimae.setProcessValue(y_pulse1*-1); |
yuron | 17:de3bc1999ae7 | 476 | back_migiusiro.setProcessValue(y_pulse1*-1); |
yuron | 17:de3bc1999ae7 | 477 | back_hidarimae.setProcessValue(y_pulse2*-1); |
yuron | 17:de3bc1999ae7 | 478 | back_hidariusiro.setProcessValue(y_pulse2*-1); |
yuron | 14:ab89b6cd9719 | 479 | |
yuron | 14:ab89b6cd9719 | 480 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 481 | migimae_data[0] = back_migimae.compute(); |
yuron | 16:05b26003da50 | 482 | migiusiro_data[0] = back_migiusiro.compute(); |
yuron | 16:05b26003da50 | 483 | hidarimae_data[0] = back_hidarimae.compute(); |
yuron | 16:05b26003da50 | 484 | hidariusiro_data[0] = back_hidariusiro.compute(); |
yuron | 14:ab89b6cd9719 | 485 | |
yuron | 14:ab89b6cd9719 | 486 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 487 | //逆転(目標に達してない) |
yuron | 17:de3bc1999ae7 | 488 | if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { |
yuron | 14:ab89b6cd9719 | 489 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 490 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 491 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 492 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 5:167327a82430 | 493 | } |
yuron | 17:de3bc1999ae7 | 494 | //左側が後に出ちゃった♥(右側だけ回して左側は停止) |
yuron | 17:de3bc1999ae7 | 495 | else if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 496 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 497 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 498 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 499 | true_hidariusiro_data[0] = 0x80; |
yuron | 5:167327a82430 | 500 | } |
yuron | 17:de3bc1999ae7 | 501 | //右側が後に出ちゃった♥(左側だけ回して右側は停止) |
yuron | 17:de3bc1999ae7 | 502 | else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 < target*-1)) { |
yuron | 17:de3bc1999ae7 | 503 | true_migimae_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 504 | true_migiusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 505 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 506 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 507 | } |
yuron | 17:de3bc1999ae7 | 508 | //正転(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 509 | else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 510 | true_migimae_data[0] = migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 511 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 512 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 513 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 514 | } |
yuron | 14:ab89b6cd9719 | 515 | } |
yuron | 14:ab89b6cd9719 | 516 | |
yuron | 17:de3bc1999ae7 | 517 | void right_PID(int target) { |
yuron | 14:ab89b6cd9719 | 518 | |
yuron | 14:ab89b6cd9719 | 519 | //センサ出力値の最小、最大 |
yuron | 16:05b26003da50 | 520 | right_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 521 | right_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 522 | right_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 523 | right_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 524 | |
yuron | 14:ab89b6cd9719 | 525 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 526 | //右進(目標まで達していない) |
yuron | 17:de3bc1999ae7 | 527 | if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { |
yuron | 16:05b26003da50 | 528 | right_migimae.setOutputLimits(0x00, 0x6C); |
yuron | 16:05b26003da50 | 529 | right_migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 16:05b26003da50 | 530 | right_hidarimae.setOutputLimits(0x84, 0xF0); |
yuron | 16:05b26003da50 | 531 | right_hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 532 | |
yuron | 17:de3bc1999ae7 | 533 | } |
yuron | 17:de3bc1999ae7 | 534 | //前側が右に出ちゃった♥(後側だけ回して前側は停止) |
yuron | 17:de3bc1999ae7 | 535 | else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 < target*-1)) { |
yuron | 17:de3bc1999ae7 | 536 | right_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 537 | right_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 538 | right_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 539 | right_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 540 | } |
yuron | 17:de3bc1999ae7 | 541 | //後側が右に出ちゃった♥(前側だけ回して後側は停止) |
yuron | 17:de3bc1999ae7 | 542 | else if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 543 | right_migimae.setOutputLimits(0x84, 0xED); |
yuron | 17:de3bc1999ae7 | 544 | right_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 545 | right_hidarimae.setOutputLimits(0x00, 0x69); |
yuron | 17:de3bc1999ae7 | 546 | right_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 547 | } |
yuron | 14:ab89b6cd9719 | 548 | //左進(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 549 | else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 550 | right_migimae.setOutputLimits(0x84, 0xED); |
yuron | 17:de3bc1999ae7 | 551 | right_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 552 | right_hidarimae.setOutputLimits(0x00, 0x69); |
yuron | 17:de3bc1999ae7 | 553 | right_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 5:167327a82430 | 554 | } |
yuron | 5:167327a82430 | 555 | |
yuron | 14:ab89b6cd9719 | 556 | //よくわからんやつ |
yuron | 16:05b26003da50 | 557 | right_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 558 | right_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 559 | right_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 560 | right_hidariusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 561 | |
yuron | 14:ab89b6cd9719 | 562 | //目標値 |
yuron | 17:de3bc1999ae7 | 563 | right_migimae.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 564 | right_migiusiro.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 565 | right_hidarimae.setSetPoint(target*-1); |
yuron | 17:de3bc1999ae7 | 566 | right_hidariusiro.setSetPoint(target*-1); |
yuron | 8:3df97287c825 | 567 | |
yuron | 14:ab89b6cd9719 | 568 | //センサ出力 |
yuron | 17:de3bc1999ae7 | 569 | right_migimae.setProcessValue(x_pulse1*-1); |
yuron | 17:de3bc1999ae7 | 570 | right_migiusiro.setProcessValue(x_pulse2*-1); |
yuron | 17:de3bc1999ae7 | 571 | right_hidarimae.setProcessValue(x_pulse1*-1); |
yuron | 17:de3bc1999ae7 | 572 | right_hidariusiro.setProcessValue(x_pulse2*-1); |
yuron | 14:ab89b6cd9719 | 573 | |
yuron | 14:ab89b6cd9719 | 574 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 575 | migimae_data[0] = right_migimae.compute(); |
yuron | 16:05b26003da50 | 576 | migiusiro_data[0] = right_migiusiro.compute(); |
yuron | 16:05b26003da50 | 577 | hidarimae_data[0] = right_hidarimae.compute(); |
yuron | 16:05b26003da50 | 578 | hidariusiro_data[0] = right_hidariusiro.compute(); |
yuron | 8:3df97287c825 | 579 | |
yuron | 14:ab89b6cd9719 | 580 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 581 | //右進(目標まで達していない) |
yuron | 17:de3bc1999ae7 | 582 | if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { |
yuron | 14:ab89b6cd9719 | 583 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 584 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 585 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 586 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 9:1359f0c813b1 | 587 | } |
yuron | 17:de3bc1999ae7 | 588 | //前側が右に出ちゃった♥(後側だけ回して前側は停止) |
yuron | 17:de3bc1999ae7 | 589 | else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 < target*-1)) { |
yuron | 16:05b26003da50 | 590 | true_migimae_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 591 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 592 | true_hidarimae_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 593 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 594 | } |
yuron | 17:de3bc1999ae7 | 595 | //後側が右に出ちゃった♥(前側だけ回して後側は停止) |
yuron | 17:de3bc1999ae7 | 596 | else if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 597 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 16:05b26003da50 | 598 | true_migiusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 599 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 16:05b26003da50 | 600 | true_hidariusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 601 | } |
yuron | 17:de3bc1999ae7 | 602 | //左進(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 603 | else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { |
yuron | 17:de3bc1999ae7 | 604 | true_migimae_data[0] = migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 605 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 606 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 607 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 608 | } |
yuron | 14:ab89b6cd9719 | 609 | } |
yuron | 9:1359f0c813b1 | 610 | |
yuron | 17:de3bc1999ae7 | 611 | void left_PID(int target) { |
yuron | 14:ab89b6cd9719 | 612 | |
yuron | 14:ab89b6cd9719 | 613 | //センサ出力値の最小、最大 |
yuron | 16:05b26003da50 | 614 | left_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 615 | left_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 616 | left_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 617 | left_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 618 | |
yuron | 14:ab89b6cd9719 | 619 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 620 | //左進(目標まで達していない) |
yuron | 17:de3bc1999ae7 | 621 | if((x_pulse1 < target) && (x_pulse2 < target)) { |
yuron | 16:05b26003da50 | 622 | left_migimae.setOutputLimits(0x84, 0xED); |
yuron | 16:05b26003da50 | 623 | left_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 16:05b26003da50 | 624 | left_hidarimae.setOutputLimits(0x00, 0x69); |
yuron | 16:05b26003da50 | 625 | left_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 626 | } |
yuron | 17:de3bc1999ae7 | 627 | //前側が左に出ちゃった♥(後側だけ回して前側は停止) |
yuron | 17:de3bc1999ae7 | 628 | else if((x_pulse1 > target) && (x_pulse2 < target)) { |
yuron | 17:de3bc1999ae7 | 629 | left_migimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 630 | left_migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 631 | left_hidarimae.setOutputLimits(0x00, 0x69); |
yuron | 17:de3bc1999ae7 | 632 | left_hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 633 | } |
yuron | 17:de3bc1999ae7 | 634 | //後側が左に出ちゃった♥(前側だけ回して後側は停止) |
yuron | 17:de3bc1999ae7 | 635 | else if((x_pulse1 < target) && (x_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 636 | left_migimae.setOutputLimits(0x84, 0xED); |
yuron | 17:de3bc1999ae7 | 637 | left_migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 17:de3bc1999ae7 | 638 | left_hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 17:de3bc1999ae7 | 639 | left_hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 640 | } |
yuron | 14:ab89b6cd9719 | 641 | //右進(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 642 | else if((x_pulse1 > target) && (x_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 643 | left_migimae.setOutputLimits(0x00, 0x6C); |
yuron | 17:de3bc1999ae7 | 644 | left_migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 17:de3bc1999ae7 | 645 | left_hidarimae.setOutputLimits(0x84, 0xF0); |
yuron | 17:de3bc1999ae7 | 646 | left_hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 8:3df97287c825 | 647 | } |
yuron | 16:05b26003da50 | 648 | |
yuron | 14:ab89b6cd9719 | 649 | //よくわからんやつ |
yuron | 16:05b26003da50 | 650 | left_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 651 | left_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 652 | left_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 653 | left_hidariusiro.setMode(AUTO_MODE); |
yuron | 10:b672aa81b226 | 654 | |
yuron | 14:ab89b6cd9719 | 655 | //目標値 |
yuron | 16:05b26003da50 | 656 | left_migimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 657 | left_migiusiro.setSetPoint(target); |
yuron | 16:05b26003da50 | 658 | left_hidarimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 659 | left_hidariusiro.setSetPoint(target); |
yuron | 9:1359f0c813b1 | 660 | |
yuron | 14:ab89b6cd9719 | 661 | //センサ出力 |
yuron | 16:05b26003da50 | 662 | left_migimae.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 663 | left_migiusiro.setProcessValue(x_pulse2); |
yuron | 16:05b26003da50 | 664 | left_hidarimae.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 665 | left_hidariusiro.setProcessValue(x_pulse2); |
yuron | 8:3df97287c825 | 666 | |
yuron | 14:ab89b6cd9719 | 667 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 668 | migimae_data[0] = left_migimae.compute(); |
yuron | 16:05b26003da50 | 669 | migiusiro_data[0] = left_migiusiro.compute(); |
yuron | 16:05b26003da50 | 670 | hidarimae_data[0] = left_hidarimae.compute(); |
yuron | 16:05b26003da50 | 671 | hidariusiro_data[0] = left_hidariusiro.compute(); |
yuron | 8:3df97287c825 | 672 | |
yuron | 14:ab89b6cd9719 | 673 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 674 | //左進(目標まで達していない) |
yuron | 17:de3bc1999ae7 | 675 | if((x_pulse1 < target) && (x_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 676 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 677 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 678 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 679 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 680 | } |
yuron | 17:de3bc1999ae7 | 681 | //前側が左に出ちゃった♥(後側だけ回して前側は停止) |
yuron | 17:de3bc1999ae7 | 682 | else if((x_pulse1 > target) && (x_pulse2 < target)) { |
yuron | 16:05b26003da50 | 683 | true_migimae_data[0] = 0x80; |
yuron | 16:05b26003da50 | 684 | true_migiusiro_data[0] = 0x80; |
yuron | 17:de3bc1999ae7 | 685 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 686 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 687 | } |
yuron | 17:de3bc1999ae7 | 688 | //後側が左に出ちゃった♥(前側だけ回して後側は停止) |
yuron | 17:de3bc1999ae7 | 689 | else if((x_pulse1 < target) && (x_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 690 | true_migimae_data[0] = migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 691 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 692 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 693 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 694 | } |
yuron | 17:de3bc1999ae7 | 695 | //右進(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 696 | else if((x_pulse1 > target) && (x_pulse2 > target)) { |
yuron | 17:de3bc1999ae7 | 697 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 698 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 699 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 700 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 701 | } |
yuron | 14:ab89b6cd9719 | 702 | } |
yuron | 12:1a22b9797004 | 703 | |
yuron | 17:de3bc1999ae7 | 704 | void turn_right_PID(int target) { |
yuron | 14:ab89b6cd9719 | 705 | |
yuron | 14:ab89b6cd9719 | 706 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 16:05b26003da50 | 707 | turn_right_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 708 | turn_right_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 709 | turn_right_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 710 | turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 711 | |
yuron | 14:ab89b6cd9719 | 712 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 713 | //右旋回(目標に達してない) |
yuron | 17:de3bc1999ae7 | 714 | if(x_pulse2 < target) { |
yuron | 17:de3bc1999ae7 | 715 | turn_right_migimae.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 716 | turn_right_migiusiro.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 717 | turn_right_hidarimae.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 718 | turn_right_hidariusiro.setOutputLimits(0x94, 0xFF); |
yuron | 8:3df97287c825 | 719 | } |
yuron | 14:ab89b6cd9719 | 720 | //左旋回(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 721 | else if(x_pulse2 > target) { |
yuron | 17:de3bc1999ae7 | 722 | turn_right_migimae.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 723 | turn_right_migiusiro.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 724 | turn_right_hidarimae.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 725 | turn_right_hidariusiro.setOutputLimits(0x10, 0x7B); |
yuron | 4:df334779a69e | 726 | } |
yuron | 8:3df97287c825 | 727 | |
yuron | 14:ab89b6cd9719 | 728 | //よくわからんやつ |
yuron | 16:05b26003da50 | 729 | turn_right_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 730 | turn_right_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 731 | turn_right_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 732 | turn_right_hidariusiro.setMode(AUTO_MODE); |
yuron | 8:3df97287c825 | 733 | |
yuron | 14:ab89b6cd9719 | 734 | //目標値 |
yuron | 16:05b26003da50 | 735 | turn_right_migimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 736 | turn_right_migiusiro.setSetPoint(target); |
yuron | 16:05b26003da50 | 737 | turn_right_hidarimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 738 | turn_right_hidariusiro.setSetPoint(target); |
yuron | 5:167327a82430 | 739 | |
yuron | 14:ab89b6cd9719 | 740 | //センサ出力 |
yuron | 17:de3bc1999ae7 | 741 | turn_right_migimae.setProcessValue(x_pulse2); |
yuron | 17:de3bc1999ae7 | 742 | turn_right_migiusiro.setProcessValue(x_pulse2); |
yuron | 17:de3bc1999ae7 | 743 | turn_right_hidarimae.setProcessValue(x_pulse2); |
yuron | 17:de3bc1999ae7 | 744 | turn_right_hidariusiro.setProcessValue(x_pulse2); |
yuron | 5:167327a82430 | 745 | |
yuron | 14:ab89b6cd9719 | 746 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 747 | migimae_data[0] = turn_right_migimae.compute(); |
yuron | 16:05b26003da50 | 748 | migiusiro_data[0] = turn_right_migiusiro.compute(); |
yuron | 16:05b26003da50 | 749 | hidarimae_data[0] = turn_right_hidarimae.compute(); |
yuron | 16:05b26003da50 | 750 | hidariusiro_data[0] = turn_right_hidariusiro.compute(); |
yuron | 8:3df97287c825 | 751 | |
yuron | 14:ab89b6cd9719 | 752 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 753 | //右旋回(目標に達してない) |
yuron | 17:de3bc1999ae7 | 754 | if(x_pulse2 < target) { |
yuron | 14:ab89b6cd9719 | 755 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 756 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 757 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 758 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 759 | } |
yuron | 14:ab89b6cd9719 | 760 | //左旋回(目標より行き過ぎ) |
yuron | 17:de3bc1999ae7 | 761 | else if(x_pulse2 > target) { |
yuron | 17:de3bc1999ae7 | 762 | true_migimae_data[0] = migimae_data[0]; |
yuron | 17:de3bc1999ae7 | 763 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 17:de3bc1999ae7 | 764 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 17:de3bc1999ae7 | 765 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 766 | } |
yuron | 14:ab89b6cd9719 | 767 | } |
yuron | 8:3df97287c825 | 768 | |
yuron | 17:de3bc1999ae7 | 769 | void turn_left_PID(int target) { |
yuron | 14:ab89b6cd9719 | 770 | |
yuron | 14:ab89b6cd9719 | 771 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 16:05b26003da50 | 772 | turn_left_migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 773 | turn_left_migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 774 | turn_left_hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 16:05b26003da50 | 775 | turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 776 | |
yuron | 14:ab89b6cd9719 | 777 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 778 | //右旋回(目標に達してない) |
yuron | 16:05b26003da50 | 779 | if(x_pulse1 < target) { |
yuron | 17:de3bc1999ae7 | 780 | turn_left_migimae.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 781 | turn_left_migiusiro.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 782 | turn_left_hidarimae.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 783 | turn_left_hidariusiro.setOutputLimits(0x10, 0x7B); |
yuron | 14:ab89b6cd9719 | 784 | } |
yuron | 14:ab89b6cd9719 | 785 | //左旋回(目標より行き過ぎ) |
yuron | 16:05b26003da50 | 786 | else if(x_pulse1 > target) { |
yuron | 17:de3bc1999ae7 | 787 | turn_left_migimae.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 788 | turn_left_migiusiro.setOutputLimits(0x10, 0x7B); |
yuron | 17:de3bc1999ae7 | 789 | turn_left_hidarimae.setOutputLimits(0x94, 0xFF); |
yuron | 17:de3bc1999ae7 | 790 | turn_left_hidariusiro.setOutputLimits(0x94, 0xFF); |
yuron | 7:7f16fb8b0192 | 791 | } |
yuron | 8:3df97287c825 | 792 | |
yuron | 14:ab89b6cd9719 | 793 | //よくわからんやつ |
yuron | 16:05b26003da50 | 794 | turn_left_migimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 795 | turn_left_migiusiro.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 796 | turn_left_hidarimae.setMode(AUTO_MODE); |
yuron | 16:05b26003da50 | 797 | turn_left_hidariusiro.setMode(AUTO_MODE); |
yuron | 5:167327a82430 | 798 | |
yuron | 14:ab89b6cd9719 | 799 | //目標値 |
yuron | 16:05b26003da50 | 800 | turn_left_migimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 801 | turn_left_migiusiro.setSetPoint(target); |
yuron | 16:05b26003da50 | 802 | turn_left_hidarimae.setSetPoint(target); |
yuron | 16:05b26003da50 | 803 | turn_left_hidariusiro.setSetPoint(target); |
yuron | 8:3df97287c825 | 804 | |
yuron | 14:ab89b6cd9719 | 805 | //センサ出力 |
yuron | 16:05b26003da50 | 806 | turn_left_migimae.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 807 | turn_left_migiusiro.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 808 | turn_left_hidarimae.setProcessValue(x_pulse1); |
yuron | 16:05b26003da50 | 809 | turn_left_hidariusiro.setProcessValue(x_pulse1); |
yuron | 5:167327a82430 | 810 | |
yuron | 14:ab89b6cd9719 | 811 | //制御量(計算結果) |
yuron | 16:05b26003da50 | 812 | migimae_data[0] = turn_left_migimae.compute(); |
yuron | 16:05b26003da50 | 813 | migiusiro_data[0] = turn_left_migiusiro.compute(); |
yuron | 16:05b26003da50 | 814 | hidarimae_data[0] = turn_left_hidarimae.compute(); |
yuron | 16:05b26003da50 | 815 | hidariusiro_data[0] = turn_left_hidariusiro.compute(); |
yuron | 5:167327a82430 | 816 | |
yuron | 14:ab89b6cd9719 | 817 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 818 | //右旋回(目標に達してない) |
yuron | 16:05b26003da50 | 819 | if(x_pulse1 < target) { |
yuron | 14:ab89b6cd9719 | 820 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 821 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 822 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 823 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 824 | } |
yuron | 14:ab89b6cd9719 | 825 | //左旋回(目標より行き過ぎ) |
yuron | 16:05b26003da50 | 826 | else if(x_pulse1 > target) { |
yuron | 14:ab89b6cd9719 | 827 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 828 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 829 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 830 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 831 | } |
yuron | 14:ab89b6cd9719 | 832 | } |
yuron | 5:167327a82430 | 833 | |
yuron | 14:ab89b6cd9719 | 834 | void dondonkasoku(void) { |
yuron | 4:df334779a69e | 835 | |
yuron | 14:ab89b6cd9719 | 836 | //どんどん加速(正転) |
yuron | 14:ab89b6cd9719 | 837 | for(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++){ |
yuron | 14:ab89b6cd9719 | 838 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 839 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 840 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 841 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 842 | wait(0.05); |
yuron | 14:ab89b6cd9719 | 843 | } |
yuron | 14:ab89b6cd9719 | 844 | //どんどん減速(正転) |
yuron | 14:ab89b6cd9719 | 845 | for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--){ |
yuron | 14:ab89b6cd9719 | 846 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 847 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 848 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 849 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 850 | wait(0.05); |
yuron | 14:ab89b6cd9719 | 851 | } |
yuron | 14:ab89b6cd9719 | 852 | //だんだん加速(逆転) |
yuron | 16:05b26003da50 | 853 | for(init_send_data[0] = 0x7B; init_send_data[0] > 0x00; init_send_data[0]--){ |
yuron | 14:ab89b6cd9719 | 854 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 855 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 856 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 857 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 858 | wait(0.05); |
yuron | 0:f73c1b076ae4 | 859 | } |
yuron | 2:c32991ba628f | 860 | //だんだん減速(逆転) |
yuron | 14:ab89b6cd9719 | 861 | for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++){ |
yuron | 14:ab89b6cd9719 | 862 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 863 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 864 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 865 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 866 | wait(0.05); |
yuron | 0:f73c1b076ae4 | 867 | } |
yuron | 0:f73c1b076ae4 | 868 | } |