2019NHK_teamA / Mbed 2 deprecated 2019_A_ver6-2_fixed_phase1

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

Dependencies:   mbed QEI PID

main.cpp@18:851f783ec516, 2019-09-07 (annotated)

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