Yuhi Takaku / Mbed 2 deprecated 2019_A_ver8

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

Dependencies:   mbed QEI PID

main.cpp@16:05b26003da50, 2019-09-01 (annotated)

Committer:
yuron
Date:
Sun Sep 01 12:12:55 2019 +0000
Revision:
16:05b26003da50
Parent:
15:d022288aec51
Child:
17:de3bc1999ae7
aaa

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