2019NHK_teamA / Mbed 2 deprecated 2019_A_ver6-2_fixed_phase1

ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数

Dependencies:   mbed QEI PID

main.cpp@15:d022288aec51, 2019-08-26 (annotated)

Committer:
yuron
Date:
Mon Aug 26 02:23:20 2019 +0000
Revision:
15:d022288aec51
Parent:
14:ab89b6cd9719
Child:
16:05b26003da50
h

Who changed what in which revision?

UserRevisionLine numberNew 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 15:d022288aec51 98 void front(unsigned int target);
yuron 15:d022288aec51 99 void back(unsigned int target);
yuron 15:d022288aec51 100 void right(unsigned int target);
yuron 15:d022288aec51 101 void left(unsigned int target);
yuron 15:d022288aec51 102 void turn_right(unsigned int target);
yuron 15:d022288aec51 103 void turn_left(unsigned int target);
yuron 15:d022288aec51 104 void front_PID(unsigned int target);
yuron 15:d022288aec51 105 void back_PID(unsigned int target);
yuron 15:d022288aec51 106 void right_PID(unsigned int target);
yuron 15:d022288aec51 107 void left_PID(unsigned int target);
yuron 15:d022288aec51 108 void turn_right_PID(unsigned int target);
yuron 15:d022288aec51 109 void turn_left_PID(unsigned int target);
yuron 14:ab89b6cd9719 110 void dondonkasoku(void);
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 15:d022288aec51 168 void front(unsigned 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 15:d022288aec51 178 void back(unsigned 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 15:d022288aec51 188 void right(unsigned 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 15:d022288aec51 198 void left(unsigned 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 15:d022288aec51 208 void turn_right(unsigned 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 15:d022288aec51 218 void turn_left(unsigned 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 15:d022288aec51 228 void front_PID(unsigned 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 15:d022288aec51 303 void back_PID(unsigned 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 15:d022288aec51 313 if((abs(y_pulse1) < target) && (abs(y_pulse2) < 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 15:d022288aec51 320 else if((abs(y_pulse1) > target) && (abs(y_pulse2) > 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 15:d022288aec51 339 migimae.setSetPoint(target);
yuron 15:d022288aec51 340 migiusiro.setSetPoint(target);
yuron 15:d022288aec51 341 hidarimae.setSetPoint(target);
yuron 15:d022288aec51 342 hidariusiro.setSetPoint(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 15:d022288aec51 358 if((abs(y_pulse1) < target) && (abs(y_pulse2) < 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 15:d022288aec51 365 else if((abs(y_pulse1) > target) && (abs(y_pulse2) > 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 15:d022288aec51 378 void right_PID(unsigned 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 15:d022288aec51 448 void left_PID(unsigned 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 15:d022288aec51 458 if((abs(x_pulse1) < target) && (abs(x_pulse2) < 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 15:d022288aec51 465 else if((abs(x_pulse1) > target) && (abs(x_pulse2) > 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 15:d022288aec51 479 migimae.setSetPoint(target);
yuron 15:d022288aec51 480 migiusiro.setSetPoint(target);
yuron 15:d022288aec51 481 hidarimae.setSetPoint(target);
yuron 15:d022288aec51 482 hidariusiro.setSetPoint(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 15:d022288aec51 498 if((abs(x_pulse1) < target) && (abs(x_pulse2) < 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 15:d022288aec51 505 else if((abs(x_pulse1) > target) && (abs(x_pulse2) > 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 15:d022288aec51 518 void turn_right_PID(unsigned 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 15:d022288aec51 588 void turn_left_PID(unsigned 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 15:d022288aec51 619 migimae.setSetPoint(target);
yuron 15:d022288aec51 620 migiusiro.setSetPoint(target);
yuron 15:d022288aec51 621 hidarimae.setSetPoint(target);
yuron 15:d022288aec51 622 hidariusiro.setSetPoint(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 15:d022288aec51 638 if((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < 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 }