2019NHK_teamA
/
2019_A_ver6-2_fixed_phase1
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
main.cpp@14:ab89b6cd9719, 2019-08-26 (annotated)
- Committer:
- yuron
- Date:
- Mon Aug 26 01:39:35 2019 +0000
- Revision:
- 14:ab89b6cd9719
- Parent:
- 13:93321c73df60
- Child:
- 15:d022288aec51
aaa
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 | 14:ab89b6cd9719 | 7 | /* PID関数を各方向毎に追加した */ |
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 | 14:ab89b6cd9719 | 20 | PID migimae(Kp, Ti, Td, 0.001); |
yuron | 14:ab89b6cd9719 | 21 | PID migiusiro(Kp, Ti, Td, 0.001); |
yuron | 14:ab89b6cd9719 | 22 | PID hidarimae(Kp, Ti, Td, 0.001); |
yuron | 14:ab89b6cd9719 | 23 | PID hidariusiro(Kp, Ti, Td, 0.001); |
yuron | 7:7f16fb8b0192 | 24 | |
yuron | 5:167327a82430 | 25 | //前進 |
yuron | 14:ab89b6cd9719 | 26 | PID front_migimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 27 | PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 28 | PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 29 | PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 30 | |
yuron | 5:167327a82430 | 31 | //後進 |
yuron | 14:ab89b6cd9719 | 32 | PID back_migimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 33 | PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 34 | PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 35 | PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 8:3df97287c825 | 36 | |
yuron | 14:ab89b6cd9719 | 37 | //右進 |
yuron | 14:ab89b6cd9719 | 38 | PID right_migimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 39 | PID right_migiusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 40 | PID right_hidarimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 41 | PID right_hidariusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 9:1359f0c813b1 | 42 | |
yuron | 14:ab89b6cd9719 | 43 | //左進 |
yuron | 14:ab89b6cd9719 | 44 | PID left_migimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 45 | PID left_migiusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 46 | PID left_hidarimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 47 | PID left_hidariusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 10:b672aa81b226 | 48 | |
yuron | 14:ab89b6cd9719 | 49 | //右旋回 |
yuron | 14:ab89b6cd9719 | 50 | PID turn_right_migimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 51 | PID turn_right_migiusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 52 | PID turn_right_hidarimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 53 | PID turn_right_hidariusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 4:df334779a69e | 54 | |
yuron | 14:ab89b6cd9719 | 55 | //左旋回 |
yuron | 14:ab89b6cd9719 | 56 | PID turn_left_migimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 57 | PID turn_left_migiusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 58 | PID turn_left_hidarimae(4500000.0, 0.0, 0.0, 0.001); |
yuron | 14:ab89b6cd9719 | 59 | PID turn_left_hidariusiro(4500000.0, 0.0, 0.0, 0.001); |
yuron | 0:f73c1b076ae4 | 60 | |
yuron | 4:df334779a69e | 61 | //MDとの通信ポート |
yuron | 4:df334779a69e | 62 | I2C i2c(PB_9, PB_8); //SDA, SCL |
yuron | 14:ab89b6cd9719 | 63 | |
yuron | 4:df334779a69e | 64 | //PCとの通信ポート |
yuron | 4:df334779a69e | 65 | Serial pc(USBTX, USBRX); //TX, RX |
yuron | 4:df334779a69e | 66 | |
yuron | 4:df334779a69e | 67 | //12V停止信号ピン |
yuron | 14:ab89b6cd9719 | 68 | DigitalOut emergency(D11); |
yuron | 4:df334779a69e | 69 | |
yuron | 14:ab89b6cd9719 | 70 | DigitalOut USR_LED1(PC_9); |
yuron | 14:ab89b6cd9719 | 71 | DigitalOut USR_LED2(PC_8); |
yuron | 14:ab89b6cd9719 | 72 | DigitalOut USR_LED3(PC_6); |
yuron | 14:ab89b6cd9719 | 73 | DigitalOut USR_LED4(PC_5); |
yuron | 8:3df97287c825 | 74 | |
yuron | 14:ab89b6cd9719 | 75 | QEI wheel_x1(PA_8 , PA_6 , NC, 624); |
yuron | 14:ab89b6cd9719 | 76 | QEI wheel_x2(PB_14, PB_13, NC, 624); |
yuron | 14:ab89b6cd9719 | 77 | QEI wheel_y1(PB_1 , PB_15, NC, 624); |
yuron | 14:ab89b6cd9719 | 78 | QEI wheel_y2(PA_12, PA_11, NC, 624); |
yuron | 14:ab89b6cd9719 | 79 | //QEI wheel1(D2, D3, NC, 624); |
yuron | 14:ab89b6cd9719 | 80 | //QEI wheel2(D5, D4, NC, 624); |
yuron | 14:ab89b6cd9719 | 81 | |
yuron | 14:ab89b6cd9719 | 82 | //エンコーダ値格納変数 |
yuron | 14:ab89b6cd9719 | 83 | int x_pulse1, x_pulse2, y_pulse1, y_pulse2; |
yuron | 14:ab89b6cd9719 | 84 | |
yuron | 14:ab89b6cd9719 | 85 | //操作の段階変数 |
yuron | 14:ab89b6cd9719 | 86 | unsigned int phase = 0; |
yuron | 0:f73c1b076ae4 | 87 | |
yuron | 14:ab89b6cd9719 | 88 | //MD送信データ変数 |
yuron | 14:ab89b6cd9719 | 89 | char init_send_data[1]; |
yuron | 14:ab89b6cd9719 | 90 | char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1]; |
yuron | 14:ab89b6cd9719 | 91 | char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1]; |
yuron | 0:f73c1b076ae4 | 92 | |
yuron | 14:ab89b6cd9719 | 93 | //関数のプロトタイプ宣言 |
yuron | 14:ab89b6cd9719 | 94 | void init(void); |
yuron | 14:ab89b6cd9719 | 95 | void init_send(void); |
yuron | 14:ab89b6cd9719 | 96 | void get_pulses(void); |
yuron | 14:ab89b6cd9719 | 97 | void print_pulses(void); |
yuron | 14:ab89b6cd9719 | 98 | void front(int target); |
yuron | 14:ab89b6cd9719 | 99 | void back(int target); |
yuron | 14:ab89b6cd9719 | 100 | void right(int target); |
yuron | 14:ab89b6cd9719 | 101 | void left(int target); |
yuron | 14:ab89b6cd9719 | 102 | void turn_right(int target); |
yuron | 14:ab89b6cd9719 | 103 | void turn_left(int target); |
yuron | 14:ab89b6cd9719 | 104 | void dondonkasoku(void); |
yuron | 14:ab89b6cd9719 | 105 | void front_PID(int target); |
yuron | 14:ab89b6cd9719 | 106 | void back_PID(int target); |
yuron | 14:ab89b6cd9719 | 107 | void right_PID(int target); |
yuron | 14:ab89b6cd9719 | 108 | void left_PID(int target); |
yuron | 14:ab89b6cd9719 | 109 | void turn_right_PID(int target); |
yuron | 14:ab89b6cd9719 | 110 | void turn_left_PID(int target); |
yuron | 8:3df97287c825 | 111 | |
yuron | 14:ab89b6cd9719 | 112 | int main(void) { |
yuron | 14:ab89b6cd9719 | 113 | |
yuron | 14:ab89b6cd9719 | 114 | init(); |
yuron | 14:ab89b6cd9719 | 115 | init_send(); |
yuron | 8:3df97287c825 | 116 | |
yuron | 14:ab89b6cd9719 | 117 | while(1) { |
yuron | 14:ab89b6cd9719 | 118 | |
yuron | 14:ab89b6cd9719 | 119 | get_pulses(); |
yuron | 14:ab89b6cd9719 | 120 | print_pulses(); |
yuron | 14:ab89b6cd9719 | 121 | front(500); |
yuron | 14:ab89b6cd9719 | 122 | //back(500); |
yuron | 14:ab89b6cd9719 | 123 | //right(500); |
yuron | 14:ab89b6cd9719 | 124 | //left(500); |
yuron | 14:ab89b6cd9719 | 125 | //turn_right(500); |
yuron | 14:ab89b6cd9719 | 126 | //turn_left(500); |
yuron | 14:ab89b6cd9719 | 127 | } |
yuron | 14:ab89b6cd9719 | 128 | } |
yuron | 0:f73c1b076ae4 | 129 | |
yuron | 14:ab89b6cd9719 | 130 | void init(void) { |
yuron | 14:ab89b6cd9719 | 131 | |
yuron | 14:ab89b6cd9719 | 132 | //緊急停止用信号ピンをLow |
yuron | 14:ab89b6cd9719 | 133 | emergency = 0; |
yuron | 14:ab89b6cd9719 | 134 | //USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1; |
yuron | 10:b672aa81b226 | 135 | |
yuron | 14:ab89b6cd9719 | 136 | //通信ボーレートの設定 |
yuron | 14:ab89b6cd9719 | 137 | //pc.baud(460800); |
yuron | 14:ab89b6cd9719 | 138 | pc.baud(9600); |
yuron | 14:ab89b6cd9719 | 139 | |
yuron | 14:ab89b6cd9719 | 140 | x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; |
yuron | 14:ab89b6cd9719 | 141 | migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 142 | true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 143 | } |
yuron | 5:167327a82430 | 144 | |
yuron | 14:ab89b6cd9719 | 145 | void init_send(void) { |
yuron | 14:ab89b6cd9719 | 146 | |
yuron | 14:ab89b6cd9719 | 147 | init_send_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 148 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 149 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 150 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 151 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 152 | wait(0.1); |
yuron | 14:ab89b6cd9719 | 153 | } |
yuron | 0:f73c1b076ae4 | 154 | |
yuron | 14:ab89b6cd9719 | 155 | void get_pulses(void) { |
yuron | 14:ab89b6cd9719 | 156 | |
yuron | 14:ab89b6cd9719 | 157 | x_pulse1 = wheel_x1.getPulses(); |
yuron | 14:ab89b6cd9719 | 158 | x_pulse2 = wheel_x2.getPulses(); |
yuron | 14:ab89b6cd9719 | 159 | y_pulse1 = wheel_y1.getPulses(); |
yuron | 14:ab89b6cd9719 | 160 | y_pulse2 = wheel_y2.getPulses(); |
yuron | 14:ab89b6cd9719 | 161 | } |
yuron | 0:f73c1b076ae4 | 162 | |
yuron | 14:ab89b6cd9719 | 163 | void print_pulses(void) { |
yuron | 14:ab89b6cd9719 | 164 | |
yuron | 14:ab89b6cd9719 | 165 | pc.printf("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2); |
yuron | 14:ab89b6cd9719 | 166 | } |
yuron | 4:df334779a69e | 167 | |
yuron | 14:ab89b6cd9719 | 168 | void front(int target) { |
yuron | 14:ab89b6cd9719 | 169 | |
yuron | 14:ab89b6cd9719 | 170 | front_PID(target); |
yuron | 14:ab89b6cd9719 | 171 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 172 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 173 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 174 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 175 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 176 | } |
yuron | 4:df334779a69e | 177 | |
yuron | 14:ab89b6cd9719 | 178 | void back(int target) { |
yuron | 14:ab89b6cd9719 | 179 | |
yuron | 14:ab89b6cd9719 | 180 | back_PID(target); |
yuron | 14:ab89b6cd9719 | 181 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 182 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 183 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 184 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 185 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 186 | } |
yuron | 5:167327a82430 | 187 | |
yuron | 14:ab89b6cd9719 | 188 | void right(int target) { |
yuron | 14:ab89b6cd9719 | 189 | |
yuron | 14:ab89b6cd9719 | 190 | right_PID(target); |
yuron | 14:ab89b6cd9719 | 191 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 192 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 193 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 194 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 195 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 196 | } |
yuron | 5:167327a82430 | 197 | |
yuron | 14:ab89b6cd9719 | 198 | void left(int target) { |
yuron | 14:ab89b6cd9719 | 199 | |
yuron | 14:ab89b6cd9719 | 200 | left_PID(target); |
yuron | 14:ab89b6cd9719 | 201 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 202 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 203 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 204 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 205 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 206 | } |
yuron | 4:df334779a69e | 207 | |
yuron | 14:ab89b6cd9719 | 208 | void turn_right(int target) { |
yuron | 14:ab89b6cd9719 | 209 | |
yuron | 14:ab89b6cd9719 | 210 | turn_right_PID(target); |
yuron | 14:ab89b6cd9719 | 211 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 212 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 213 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 214 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 215 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 216 | } |
yuron | 4:df334779a69e | 217 | |
yuron | 14:ab89b6cd9719 | 218 | void turn_left(int target) { |
yuron | 14:ab89b6cd9719 | 219 | |
yuron | 14:ab89b6cd9719 | 220 | turn_left_PID(target); |
yuron | 14:ab89b6cd9719 | 221 | i2c.write(0x10, true_migimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 222 | i2c.write(0x12, true_migiusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 223 | i2c.write(0x14, true_hidarimae_data, 1, false); |
yuron | 14:ab89b6cd9719 | 224 | i2c.write(0x16, true_hidariusiro_data, 1, false); |
yuron | 14:ab89b6cd9719 | 225 | wait_us(20); |
yuron | 14:ab89b6cd9719 | 226 | } |
yuron | 5:167327a82430 | 227 | |
yuron | 14:ab89b6cd9719 | 228 | void front_PID(int target) { |
yuron | 5:167327a82430 | 229 | |
yuron | 14:ab89b6cd9719 | 230 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 14:ab89b6cd9719 | 231 | migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 232 | migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 233 | hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 234 | hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 5:167327a82430 | 235 | |
yuron | 14:ab89b6cd9719 | 236 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 237 | //正転(目標に達してない) |
yuron | 14:ab89b6cd9719 | 238 | if((y_pulse1 < target) && (y_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 239 | migimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 240 | migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 241 | hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 242 | hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 243 | } |
yuron | 14:ab89b6cd9719 | 244 | //逆転(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 245 | else if((y_pulse1 > target) && (y_pulse2 > target)) { |
yuron | 14:ab89b6cd9719 | 246 | migimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 247 | migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 248 | hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 249 | hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 250 | } else { |
yuron | 14:ab89b6cd9719 | 251 | migimae.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 252 | migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 253 | hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 254 | hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 255 | } |
yuron | 5:167327a82430 | 256 | |
yuron | 14:ab89b6cd9719 | 257 | //よくわからんやつ |
yuron | 4:df334779a69e | 258 | migimae.setMode(AUTO_MODE); |
yuron | 4:df334779a69e | 259 | migiusiro.setMode(AUTO_MODE); |
yuron | 4:df334779a69e | 260 | hidarimae.setMode(AUTO_MODE); |
yuron | 4:df334779a69e | 261 | hidariusiro.setMode(AUTO_MODE); |
yuron | 0:f73c1b076ae4 | 262 | |
yuron | 14:ab89b6cd9719 | 263 | //目標値 |
yuron | 14:ab89b6cd9719 | 264 | migimae.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 265 | migiusiro.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 266 | hidarimae.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 267 | hidariusiro.setSetPoint(target); |
yuron | 5:167327a82430 | 268 | |
yuron | 14:ab89b6cd9719 | 269 | //センサ出力 |
yuron | 14:ab89b6cd9719 | 270 | migimae.setProcessValue(y_pulse1); |
yuron | 14:ab89b6cd9719 | 271 | migiusiro.setProcessValue(y_pulse1); |
yuron | 14:ab89b6cd9719 | 272 | hidarimae.setProcessValue(y_pulse2); |
yuron | 14:ab89b6cd9719 | 273 | hidariusiro.setProcessValue(y_pulse2); |
yuron | 5:167327a82430 | 274 | |
yuron | 14:ab89b6cd9719 | 275 | //制御量(計算結果) |
yuron | 4:df334779a69e | 276 | migimae_data[0] = migimae.compute(); |
yuron | 4:df334779a69e | 277 | migiusiro_data[0] = migiusiro.compute(); |
yuron | 4:df334779a69e | 278 | hidarimae_data[0] = hidarimae.compute(); |
yuron | 4:df334779a69e | 279 | hidariusiro_data[0] = hidariusiro.compute(); |
yuron | 4:df334779a69e | 280 | |
yuron | 14:ab89b6cd9719 | 281 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 282 | //正転(目標に達してない) |
yuron | 14:ab89b6cd9719 | 283 | if((y_pulse1 < target) && (y_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 284 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 285 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 286 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 287 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 288 | } |
yuron | 14:ab89b6cd9719 | 289 | //逆転(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 290 | else if((y_pulse1 > target) && (y_pulse2 > target)) { |
yuron | 14:ab89b6cd9719 | 291 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 292 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 293 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 294 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 295 | } else { |
yuron | 14:ab89b6cd9719 | 296 | true_migimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 297 | true_migiusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 298 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 299 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 300 | } |
yuron | 5:167327a82430 | 301 | } |
yuron | 5:167327a82430 | 302 | |
yuron | 14:ab89b6cd9719 | 303 | void back_PID(int target) { |
yuron | 14:ab89b6cd9719 | 304 | |
yuron | 14:ab89b6cd9719 | 305 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 14:ab89b6cd9719 | 306 | migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 307 | migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 308 | hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 309 | hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 5:167327a82430 | 310 | |
yuron | 14:ab89b6cd9719 | 311 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 312 | //逆転(目標に達してない) |
yuron | 14:ab89b6cd9719 | 313 | if((abs(y_pulse1) < abs(target)) || (abs(y_pulse2) < abs(target))) { |
yuron | 14:ab89b6cd9719 | 314 | migimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 315 | migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 316 | hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 317 | hidariusiro.setOutputLimits(0x0, 0x7B); |
yuron | 14:ab89b6cd9719 | 318 | } |
yuron | 14:ab89b6cd9719 | 319 | //正転(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 320 | else if((abs(y_pulse1) > abs(target)) || (abs(y_pulse2) > abs(target))) { |
yuron | 14:ab89b6cd9719 | 321 | migimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 322 | migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 323 | hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 324 | hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 4:df334779a69e | 325 | } else { |
yuron | 14:ab89b6cd9719 | 326 | migimae.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 327 | migiusiro.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 328 | hidarimae.setOutputLimits(0x7C, 0x83); |
yuron | 14:ab89b6cd9719 | 329 | hidariusiro.setOutputLimits(0x7C, 0x83); |
yuron | 4:df334779a69e | 330 | } |
yuron | 5:167327a82430 | 331 | |
yuron | 14:ab89b6cd9719 | 332 | //よくわからんやつ |
yuron | 14:ab89b6cd9719 | 333 | migimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 334 | migiusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 335 | hidarimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 336 | hidariusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 337 | |
yuron | 14:ab89b6cd9719 | 338 | //目標値 |
yuron | 14:ab89b6cd9719 | 339 | migimae.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 340 | migiusiro.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 341 | hidarimae.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 342 | hidariusiro.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 343 | |
yuron | 14:ab89b6cd9719 | 344 | //センサ出力 |
yuron | 14:ab89b6cd9719 | 345 | migimae.setProcessValue(abs(y_pulse1)); |
yuron | 14:ab89b6cd9719 | 346 | migiusiro.setProcessValue(abs(y_pulse1)); |
yuron | 14:ab89b6cd9719 | 347 | hidarimae.setProcessValue(abs(y_pulse2)); |
yuron | 14:ab89b6cd9719 | 348 | hidariusiro.setProcessValue(abs(y_pulse2)); |
yuron | 14:ab89b6cd9719 | 349 | |
yuron | 14:ab89b6cd9719 | 350 | //制御量(計算結果) |
yuron | 14:ab89b6cd9719 | 351 | migimae_data[0] = migimae.compute(); |
yuron | 14:ab89b6cd9719 | 352 | migiusiro_data[0] = migiusiro.compute(); |
yuron | 14:ab89b6cd9719 | 353 | hidarimae_data[0] = hidarimae.compute(); |
yuron | 14:ab89b6cd9719 | 354 | hidariusiro_data[0] = hidariusiro.compute(); |
yuron | 14:ab89b6cd9719 | 355 | |
yuron | 14:ab89b6cd9719 | 356 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 357 | //逆転(目標に達してない) |
yuron | 14:ab89b6cd9719 | 358 | if((abs(y_pulse1) < abs(target)) || (abs(y_pulse2) < abs(target))) { |
yuron | 14:ab89b6cd9719 | 359 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 360 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 361 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 362 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 5:167327a82430 | 363 | } |
yuron | 14:ab89b6cd9719 | 364 | //正転(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 365 | else if((abs(y_pulse1) > abs(target)) || (abs(y_pulse2) > abs(target))) { |
yuron | 14:ab89b6cd9719 | 366 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 367 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 368 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 369 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 370 | } else { |
yuron | 14:ab89b6cd9719 | 371 | true_migimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 372 | true_migiusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 373 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 374 | true_hidariusiro_data[0] = 0x80; |
yuron | 5:167327a82430 | 375 | } |
yuron | 14:ab89b6cd9719 | 376 | } |
yuron | 14:ab89b6cd9719 | 377 | |
yuron | 14:ab89b6cd9719 | 378 | void right_PID(int target) { |
yuron | 14:ab89b6cd9719 | 379 | |
yuron | 14:ab89b6cd9719 | 380 | //センサ出力値の最小、最大 |
yuron | 14:ab89b6cd9719 | 381 | migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 382 | migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 383 | hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 384 | hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 385 | |
yuron | 14:ab89b6cd9719 | 386 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 387 | //右進(目標まで達していない) |
yuron | 14:ab89b6cd9719 | 388 | if((x_pulse1 < target) && (x_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 389 | migimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 390 | migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 391 | hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 392 | hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 393 | } |
yuron | 14:ab89b6cd9719 | 394 | //左進(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 395 | else if((x_pulse1 > target) && (x_pulse2 > target)) { |
yuron | 14:ab89b6cd9719 | 396 | migimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 397 | migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 398 | hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 399 | hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 5:167327a82430 | 400 | } |
yuron | 5:167327a82430 | 401 | |
yuron | 14:ab89b6cd9719 | 402 | //よくわからんやつ |
yuron | 14:ab89b6cd9719 | 403 | migimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 404 | migiusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 405 | hidarimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 406 | hidariusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 407 | |
yuron | 14:ab89b6cd9719 | 408 | //目標値 |
yuron | 14:ab89b6cd9719 | 409 | migimae.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 410 | migiusiro.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 411 | hidarimae.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 412 | hidariusiro.setSetPoint(target); |
yuron | 8:3df97287c825 | 413 | |
yuron | 14:ab89b6cd9719 | 414 | //センサ出力 |
yuron | 14:ab89b6cd9719 | 415 | migimae.setProcessValue(x_pulse1); |
yuron | 14:ab89b6cd9719 | 416 | migiusiro.setProcessValue(x_pulse1); |
yuron | 14:ab89b6cd9719 | 417 | hidarimae.setProcessValue(x_pulse2); |
yuron | 14:ab89b6cd9719 | 418 | hidariusiro.setProcessValue(x_pulse2); |
yuron | 14:ab89b6cd9719 | 419 | |
yuron | 14:ab89b6cd9719 | 420 | //制御量(計算結果) |
yuron | 14:ab89b6cd9719 | 421 | migimae_data[0] = migimae.compute(); |
yuron | 14:ab89b6cd9719 | 422 | migiusiro_data[0] = migiusiro.compute(); |
yuron | 14:ab89b6cd9719 | 423 | hidarimae_data[0] = hidarimae.compute(); |
yuron | 14:ab89b6cd9719 | 424 | hidariusiro_data[0] = hidariusiro.compute(); |
yuron | 8:3df97287c825 | 425 | |
yuron | 14:ab89b6cd9719 | 426 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 427 | //右進(目標まで達していない) |
yuron | 14:ab89b6cd9719 | 428 | if((x_pulse1 < target) && (x_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 429 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 430 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 431 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 432 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 9:1359f0c813b1 | 433 | } |
yuron | 14:ab89b6cd9719 | 434 | //左進(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 435 | else if((x_pulse1 > target) && (x_pulse2 > target)) { |
yuron | 14:ab89b6cd9719 | 436 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 437 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 438 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 439 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 440 | } else { |
yuron | 14:ab89b6cd9719 | 441 | true_migimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 442 | true_migiusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 443 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 444 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 445 | } |
yuron | 14:ab89b6cd9719 | 446 | } |
yuron | 9:1359f0c813b1 | 447 | |
yuron | 14:ab89b6cd9719 | 448 | void left_PID(int target) { |
yuron | 14:ab89b6cd9719 | 449 | |
yuron | 14:ab89b6cd9719 | 450 | //センサ出力値の最小、最大 |
yuron | 14:ab89b6cd9719 | 451 | migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 452 | migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 453 | hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 454 | hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 455 | |
yuron | 14:ab89b6cd9719 | 456 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 457 | //左進(目標まで達していない) |
yuron | 14:ab89b6cd9719 | 458 | if((abs(x_pulse1) < abs(target)) && (abs(x_pulse2) < abs(target))) { |
yuron | 14:ab89b6cd9719 | 459 | migimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 460 | migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 461 | hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 462 | hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 463 | } |
yuron | 14:ab89b6cd9719 | 464 | //右進(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 465 | else if((abs(x_pulse1) > abs(target)) && (abs(x_pulse2) > abs(target))) { |
yuron | 14:ab89b6cd9719 | 466 | migimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 467 | migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 468 | hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 469 | hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 8:3df97287c825 | 470 | } |
yuron | 8:3df97287c825 | 471 | |
yuron | 14:ab89b6cd9719 | 472 | //よくわからんやつ |
yuron | 14:ab89b6cd9719 | 473 | migimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 474 | migiusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 475 | hidarimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 476 | hidariusiro.setMode(AUTO_MODE); |
yuron | 10:b672aa81b226 | 477 | |
yuron | 14:ab89b6cd9719 | 478 | //目標値 |
yuron | 14:ab89b6cd9719 | 479 | migimae.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 480 | migiusiro.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 481 | hidarimae.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 482 | hidariusiro.setSetPoint(abs(target)); |
yuron | 9:1359f0c813b1 | 483 | |
yuron | 14:ab89b6cd9719 | 484 | //センサ出力 |
yuron | 14:ab89b6cd9719 | 485 | migimae.setProcessValue(abs(x_pulse1)); |
yuron | 14:ab89b6cd9719 | 486 | migiusiro.setProcessValue(abs(x_pulse1)); |
yuron | 14:ab89b6cd9719 | 487 | hidarimae.setProcessValue(abs(x_pulse2)); |
yuron | 14:ab89b6cd9719 | 488 | hidariusiro.setProcessValue(abs(x_pulse2)); |
yuron | 8:3df97287c825 | 489 | |
yuron | 14:ab89b6cd9719 | 490 | //制御量(計算結果) |
yuron | 14:ab89b6cd9719 | 491 | migimae_data[0] = migimae.compute(); |
yuron | 14:ab89b6cd9719 | 492 | migiusiro_data[0] = migiusiro.compute(); |
yuron | 14:ab89b6cd9719 | 493 | hidarimae_data[0] = hidarimae.compute(); |
yuron | 14:ab89b6cd9719 | 494 | hidariusiro_data[0] = hidariusiro.compute(); |
yuron | 8:3df97287c825 | 495 | |
yuron | 14:ab89b6cd9719 | 496 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 497 | //左進(目標まで達していない) |
yuron | 14:ab89b6cd9719 | 498 | if((abs(x_pulse1) < abs(target)) && (abs(x_pulse2) < abs(target))) { |
yuron | 14:ab89b6cd9719 | 499 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 500 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 501 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 502 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 503 | } |
yuron | 14:ab89b6cd9719 | 504 | //右進(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 505 | else if((abs(x_pulse1) > abs(target)) && (abs(x_pulse2) > abs(target))) { |
yuron | 14:ab89b6cd9719 | 506 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 507 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 508 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 509 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 510 | } else { |
yuron | 14:ab89b6cd9719 | 511 | true_migimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 512 | true_migiusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 513 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 514 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 515 | } |
yuron | 14:ab89b6cd9719 | 516 | } |
yuron | 12:1a22b9797004 | 517 | |
yuron | 14:ab89b6cd9719 | 518 | void turn_right_PID(int target) { |
yuron | 14:ab89b6cd9719 | 519 | |
yuron | 14:ab89b6cd9719 | 520 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 14:ab89b6cd9719 | 521 | migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 522 | migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 523 | hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 524 | hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 525 | |
yuron | 14:ab89b6cd9719 | 526 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 527 | //右旋回(目標に達してない) |
yuron | 14:ab89b6cd9719 | 528 | if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse2 < target)) { |
yuron | 14:ab89b6cd9719 | 529 | migimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 530 | migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 531 | hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 532 | hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 8:3df97287c825 | 533 | } |
yuron | 14:ab89b6cd9719 | 534 | //左旋回(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 535 | else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) { |
yuron | 14:ab89b6cd9719 | 536 | migimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 537 | migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 538 | hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 539 | hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 4:df334779a69e | 540 | } |
yuron | 8:3df97287c825 | 541 | |
yuron | 14:ab89b6cd9719 | 542 | //よくわからんやつ |
yuron | 14:ab89b6cd9719 | 543 | migimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 544 | migiusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 545 | hidarimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 546 | hidariusiro.setMode(AUTO_MODE); |
yuron | 8:3df97287c825 | 547 | |
yuron | 14:ab89b6cd9719 | 548 | //目標値 |
yuron | 14:ab89b6cd9719 | 549 | migimae.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 550 | migiusiro.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 551 | hidarimae.setSetPoint(target); |
yuron | 14:ab89b6cd9719 | 552 | hidariusiro.setSetPoint(target); |
yuron | 5:167327a82430 | 553 | |
yuron | 14:ab89b6cd9719 | 554 | //センサ出力 |
yuron | 14:ab89b6cd9719 | 555 | migimae.setProcessValue(x_pulse1); |
yuron | 14:ab89b6cd9719 | 556 | migiusiro.setProcessValue(x_pulse2); |
yuron | 14:ab89b6cd9719 | 557 | hidarimae.setProcessValue(y_pulse1); |
yuron | 14:ab89b6cd9719 | 558 | hidariusiro.setProcessValue(y_pulse2); |
yuron | 5:167327a82430 | 559 | |
yuron | 14:ab89b6cd9719 | 560 | //制御量(計算結果) |
yuron | 14:ab89b6cd9719 | 561 | migimae_data[0] = migimae.compute(); |
yuron | 14:ab89b6cd9719 | 562 | migiusiro_data[0] = migiusiro.compute(); |
yuron | 14:ab89b6cd9719 | 563 | hidarimae_data[0] = hidarimae.compute(); |
yuron | 14:ab89b6cd9719 | 564 | hidariusiro_data[0] = hidariusiro.compute(); |
yuron | 8:3df97287c825 | 565 | |
yuron | 14:ab89b6cd9719 | 566 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 567 | //右旋回(目標に達してない) |
yuron | 14:ab89b6cd9719 | 568 | if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse1 < target)) { |
yuron | 14:ab89b6cd9719 | 569 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 570 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 571 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 572 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 573 | } |
yuron | 14:ab89b6cd9719 | 574 | //左旋回(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 575 | else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) { |
yuron | 14:ab89b6cd9719 | 576 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 577 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 578 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 579 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 580 | } else { |
yuron | 14:ab89b6cd9719 | 581 | true_migimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 582 | true_migiusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 583 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 584 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 585 | } |
yuron | 14:ab89b6cd9719 | 586 | } |
yuron | 8:3df97287c825 | 587 | |
yuron | 14:ab89b6cd9719 | 588 | void turn_left_PID(int target) { |
yuron | 14:ab89b6cd9719 | 589 | |
yuron | 14:ab89b6cd9719 | 590 | //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) |
yuron | 14:ab89b6cd9719 | 591 | migimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 592 | migiusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 593 | hidarimae.setInputLimits(-2147483648, 2147483647); |
yuron | 14:ab89b6cd9719 | 594 | hidariusiro.setInputLimits(-2147483648, 2147483647); |
yuron | 9:1359f0c813b1 | 595 | |
yuron | 14:ab89b6cd9719 | 596 | //制御量の最小、最大 |
yuron | 14:ab89b6cd9719 | 597 | //右旋回(目標に達してない) |
yuron | 14:ab89b6cd9719 | 598 | if((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < target)) { |
yuron | 14:ab89b6cd9719 | 599 | migimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 600 | migiusiro.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 601 | hidarimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 602 | hidariusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 603 | } |
yuron | 14:ab89b6cd9719 | 604 | //左旋回(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 605 | else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) { |
yuron | 14:ab89b6cd9719 | 606 | migimae.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 607 | migiusiro.setOutputLimits(0x00, 0x7B); |
yuron | 14:ab89b6cd9719 | 608 | hidarimae.setOutputLimits(0x84, 0xFF); |
yuron | 14:ab89b6cd9719 | 609 | hidariusiro.setOutputLimits(0x84, 0xFF); |
yuron | 7:7f16fb8b0192 | 610 | } |
yuron | 8:3df97287c825 | 611 | |
yuron | 14:ab89b6cd9719 | 612 | //よくわからんやつ |
yuron | 14:ab89b6cd9719 | 613 | migimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 614 | migiusiro.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 615 | hidarimae.setMode(AUTO_MODE); |
yuron | 14:ab89b6cd9719 | 616 | hidariusiro.setMode(AUTO_MODE); |
yuron | 5:167327a82430 | 617 | |
yuron | 14:ab89b6cd9719 | 618 | //目標値 |
yuron | 14:ab89b6cd9719 | 619 | migimae.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 620 | migiusiro.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 621 | hidarimae.setSetPoint(abs(target)); |
yuron | 14:ab89b6cd9719 | 622 | hidariusiro.setSetPoint(abs(target)); |
yuron | 8:3df97287c825 | 623 | |
yuron | 14:ab89b6cd9719 | 624 | //センサ出力 |
yuron | 14:ab89b6cd9719 | 625 | migimae.setProcessValue(abs(x_pulse1)); |
yuron | 14:ab89b6cd9719 | 626 | migiusiro.setProcessValue(abs(y_pulse1)); |
yuron | 14:ab89b6cd9719 | 627 | hidarimae.setProcessValue(abs(y_pulse2)); |
yuron | 14:ab89b6cd9719 | 628 | hidariusiro.setProcessValue(abs(x_pulse2)); |
yuron | 5:167327a82430 | 629 | |
yuron | 14:ab89b6cd9719 | 630 | //制御量(計算結果) |
yuron | 14:ab89b6cd9719 | 631 | migimae_data[0] = migimae.compute(); |
yuron | 14:ab89b6cd9719 | 632 | migiusiro_data[0] = migiusiro.compute(); |
yuron | 14:ab89b6cd9719 | 633 | hidarimae_data[0] = hidarimae.compute(); |
yuron | 14:ab89b6cd9719 | 634 | hidariusiro_data[0] = hidariusiro.compute(); |
yuron | 5:167327a82430 | 635 | |
yuron | 14:ab89b6cd9719 | 636 | //制御量をPWM値に変換 |
yuron | 14:ab89b6cd9719 | 637 | //右旋回(目標に達してない) |
yuron | 14:ab89b6cd9719 | 638 | if((abs(x_pulse1) < abs(target)) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < abs(target))) { |
yuron | 14:ab89b6cd9719 | 639 | true_migimae_data[0] = migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 640 | true_migiusiro_data[0] = migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 641 | true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 642 | true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 643 | } |
yuron | 14:ab89b6cd9719 | 644 | //左旋回(目標より行き過ぎ) |
yuron | 14:ab89b6cd9719 | 645 | else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) { |
yuron | 14:ab89b6cd9719 | 646 | true_migimae_data[0] = 0x7B - migimae_data[0]; |
yuron | 14:ab89b6cd9719 | 647 | true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 648 | true_hidarimae_data[0] = hidarimae_data[0]; |
yuron | 14:ab89b6cd9719 | 649 | true_hidariusiro_data[0] = hidariusiro_data[0]; |
yuron | 14:ab89b6cd9719 | 650 | } else { |
yuron | 14:ab89b6cd9719 | 651 | true_migimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 652 | true_migiusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 653 | true_hidarimae_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 654 | true_hidariusiro_data[0] = 0x80; |
yuron | 14:ab89b6cd9719 | 655 | } |
yuron | 14:ab89b6cd9719 | 656 | } |
yuron | 5:167327a82430 | 657 | |
yuron | 14:ab89b6cd9719 | 658 | void dondonkasoku(void) { |
yuron | 4:df334779a69e | 659 | |
yuron | 14:ab89b6cd9719 | 660 | //どんどん加速(正転) |
yuron | 14:ab89b6cd9719 | 661 | for(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++){ |
yuron | 14:ab89b6cd9719 | 662 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 663 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 664 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 665 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 666 | wait(0.05); |
yuron | 14:ab89b6cd9719 | 667 | } |
yuron | 14:ab89b6cd9719 | 668 | //どんどん減速(正転) |
yuron | 14:ab89b6cd9719 | 669 | for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--){ |
yuron | 14:ab89b6cd9719 | 670 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 671 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 672 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 673 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 674 | wait(0.05); |
yuron | 14:ab89b6cd9719 | 675 | } |
yuron | 14:ab89b6cd9719 | 676 | //だんだん加速(逆転) |
yuron | 14:ab89b6cd9719 | 677 | for(init_send_data[0] = 0x7B; init_send_data[0] <= 0x00; init_send_data[0]--){ |
yuron | 14:ab89b6cd9719 | 678 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 679 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 680 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 681 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 682 | wait(0.05); |
yuron | 0:f73c1b076ae4 | 683 | } |
yuron | 2:c32991ba628f | 684 | //だんだん減速(逆転) |
yuron | 14:ab89b6cd9719 | 685 | for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++){ |
yuron | 14:ab89b6cd9719 | 686 | i2c.write(0x10, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 687 | i2c.write(0x12, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 688 | i2c.write(0x14, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 689 | i2c.write(0x16, init_send_data, 1); |
yuron | 14:ab89b6cd9719 | 690 | wait(0.05); |
yuron | 0:f73c1b076ae4 | 691 | } |
yuron | 0:f73c1b076ae4 | 692 | } |