Yuhi Takaku / Mbed 2 deprecated 2019_A_ver8

ウオッチドッグタイマを追加したよん

Dependencies:   mbed QEI PID

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?

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 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 }