1日体験入学のやつ
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* ------------------------------------------------------------------- */ 00002 /* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */ 00003 /* Nucleo Type: F446RE */ 00004 /* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */ 00005 /* Sensor: encorder*4 */ 00006 /* ------------------------------------------------------------------- */ 00007 /* blue zone is ok, added back phase */ 00008 /* ------------------------------------------------------------------- */ 00009 #include "mbed.h" 00010 #include "math.h" 00011 #include "QEI.h" 00012 #include "PID.h" 00013 00014 //直進補正の為の前後・左右の回転差の許容値 00015 #define wheel_difference 100 00016 00017 #define RED 0 00018 #define BLUE 1 00019 00020 //PID Gain of wheels(Kp, Ti, Td, control cycle) 00021 //前進 00022 PID front_migimae(4500000.0, 0.0, 0.0, 0.001); 00023 PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001); 00024 PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001); 00025 PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001); 00026 00027 //後進 00028 PID back_migimae(4500000.0, 0.0, 0.0, 0.001); 00029 PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001); 00030 PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001); 00031 PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001); 00032 00033 //右進 00034 PID right_migimae(6000000.0, 0.0, 0.0, 0.001); 00035 PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001); 00036 PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001); 00037 PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001); 00038 00039 //左進 00040 PID left_migimae(6000000.0, 0.0, 0.0, 0.001); 00041 PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001); 00042 PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001); 00043 PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001); 00044 00045 //右旋回 00046 PID turn_right_migimae(30000000.0, 0.0, 0.0, 0.001); 00047 PID turn_right_migiusiro(30000000.0, 0.0, 0.0, 0.001); 00048 PID turn_right_hidarimae(30000000.0, 0.0, 0.0, 0.001); 00049 PID turn_right_hidariusiro(30000000.0, 0.0, 0.0, 0.001); 00050 00051 //左旋回 00052 PID turn_left_migimae(30000000.0, 0.0, 0.0, 0.001); 00053 PID turn_left_migiusiro(30000000.0, 0.0, 0.0, 0.001); 00054 PID turn_left_hidarimae(30000000.0, 0.0, 0.0, 0.001); 00055 PID turn_left_hidariusiro(30000000.0, 0.0, 0.0, 0.001); 00056 00057 //MDとの通信ポート 00058 I2C i2c(PB_9, PB_8); //SDA, SCL 00059 00060 //PCとの通信ポート 00061 Serial pc(USBTX, USBRX); //TX, RX 00062 00063 //特小モジュールとの通信ポート 00064 Serial pic(A0, A1); 00065 00066 //リミットスイッチ基板との通信ポート 00067 Serial limit_serial(PC_12, PD_2); 00068 00069 //12V停止信号ピン 00070 DigitalOut emergency(D11); 00071 00072 DigitalOut USR_LED1(PB_7); 00073 //DigitalOut USR_LED2(PC_13); 00074 DigitalOut USR_LED3(PC_2); 00075 DigitalOut USR_LED4(PC_3); 00076 DigitalOut GREEN_LED(D8); 00077 DigitalOut RED_LED(D10); 00078 00079 //遠隔非常停止ユニットLED 00080 AnalogOut myled(A2); 00081 00082 DigitalIn start_switch(PB_12); 00083 DigitalIn USR_SWITCH(PC_13); 00084 DigitalIn zone_switch(PC_10); 00085 00086 QEI wheel_x1(PA_8 , PA_6 , NC, 624); 00087 QEI wheel_x2(PB_14, PB_13, NC, 624); 00088 QEI wheel_y1(PB_1 , PB_15, NC, 624); 00089 QEI wheel_y2(PA_12, PA_11, NC, 624); 00090 QEI arm_enc(PB_5, PB_4 , NC, 624); 00091 00092 //移動後n秒停止タイマー 00093 Timer counter; 00094 00095 //エンコーダ値格納変数 00096 int x_pulse1, x_pulse2, y_pulse1, y_pulse2; 00097 00098 //操作の段階変数 00099 unsigned int phase = 0; 00100 int kaisyu_phase = 0; 00101 int tyokudo_phase = 0; 00102 unsigned int start_zone = 1; 00103 bool zone = RED; 00104 00105 //i2c送信データ変数 00106 char init_send_data[1]; 00107 char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1]; 00108 char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1]; 00109 char arm_motor[1], drop_motor[1]; 00110 char fan_data[1]; 00111 char servo_data[1]; 00112 char right_arm_data[1], left_arm_data[1]; 00113 00114 //非常停止関連変数 00115 char RDATA; 00116 char baff; 00117 int flug = 0; 00118 00119 //リミット基板からの受信データ 00120 int limit_data = 0; 00121 int upper_limit_data = 0; 00122 int lower_limit_data = 0; 00123 00124 //各辺のスイッチが押されたかのフラグ 00125 //前部が壁に当たっているか 00126 int front_limit = 0; 00127 //右部が壁にあたあっているか 00128 int right_limit = 0; 00129 //後部が壁に当たっているか 00130 int back_limit = 0; 00131 //回収機構の下限(引っ込めてるほう) 00132 bool kaisyu_mae_limit = 0; 00133 00134 bool kaisyu_usiro_limit = 0; 00135 00136 //右腕の下限 00137 bool right_arm_lower_limit = 0; 00138 //右腕の上限 00139 bool right_arm_upper_limit = 0; 00140 //左腕の下限 00141 bool left_arm_lower_limit = 0; 00142 //左腕の上限 00143 bool left_arm_upper_limit = 0; 00144 //吐き出し機構の上限 00145 bool tyokudo_mae_limit = 0; 00146 //吐き出し機構の下限 00147 bool tyokudo_usiro_limit = 0; 00148 00149 int masked_lower_front_limit_data = 0xFF; 00150 int masked_lower_back_limit_data = 0xFF; 00151 int masked_lower_right_limit_data = 0xFF; 00152 int masked_kaisyu_mae_limit_data = 0xFF; 00153 int masked_kaisyu_usiro_limit_data = 0xFF; 00154 int masked_right_arm_lower_limit_data = 0xFF; 00155 int masked_right_arm_upper_limit_data = 0xFF; 00156 int masked_left_arm_lower_limit_data = 0xFF; 00157 int masked_left_arm_upper_limit_data = 0xFF; 00158 int masked_tyokudo_mae_limit_data = 0xFF; 00159 int masked_tyokudo_usiro_limit_data = 0xFF; 00160 00161 //関数のプロトタイプ宣言 00162 void init(void); 00163 void init_send(void); 00164 void get(void); 00165 void get_pulses(void); 00166 void print_pulses(void); 00167 void get_emergency(void); 00168 void read_limit(void); 00169 void wheel_reset(void); 00170 void kaisyu(int pulse, int next_phase); 00171 void tyokudo(int pulse, int next_phase); 00172 void arm_up(int next_phase); 00173 void front(int target); 00174 void back(int target); 00175 void right(int target); 00176 void left(int target); 00177 void turn_right(int target); 00178 void turn_left(int target); 00179 void stop(void); 00180 void front_PID(int target); 00181 void back_PID(int target); 00182 void right_PID(int target); 00183 void left_PID(int target); 00184 void turn_right_PID(int target); 00185 void turn_left_PID(int target); 00186 00187 int main(void) { 00188 00189 init(); 00190 init_send(); 00191 00192 //とりあえず(後で消してね) 00193 //zone = BLUE; 00194 //phase = 16; 00195 //phase = 23; 00196 phase = 30; 00197 00198 //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止) 00199 while(1) { 00200 if(zone_switch == 0) { 00201 zone = BLUE; 00202 } else { 00203 zone = RED; 00204 } 00205 break; 00206 } 00207 00208 while(1) { 00209 00210 get_pulses(); 00211 print_pulses(); 00212 get_emergency(); 00213 read_limit(); 00214 00215 //move_servo_with_using_onboard-switch 00216 if(USR_SWITCH == 0) { 00217 servo_data[0] = 0x03; 00218 i2c.write(0x30, servo_data, 1); 00219 } else { 00220 servo_data[0] = 0x04; 00221 i2c.write(0x30, servo_data, 1); 00222 } 00223 00224 if(start_switch == 1) { 00225 phase = 23; 00226 } 00227 00228 //青ゾーン 00229 if(zone == BLUE) { 00230 GREEN_LED = 1; 00231 RED_LED = 0; 00232 00233 switch(phase) { 00234 00235 //スタート位置へセット 00236 case 0: 00237 //リミットが洗濯物台に触れているか 00238 if(right_limit == 3) { 00239 USR_LED1 = 1; 00240 //スタートスイッチが押されたか 00241 if(start_switch == 1) { 00242 wheel_reset(); 00243 phase = 1; 00244 } 00245 } else { 00246 USR_LED1 = 0; 00247 } 00248 break; 00249 00250 //回収 00251 case 1: 00252 kaisyu(arm_enc.getPulses(), 2); 00253 servo_data[0] = 0x03; 00254 i2c.write(0x30, servo_data, 1); 00255 break; 00256 00257 //1秒停止 00258 case 2: 00259 stop(); 00260 servo_data[0] = 0x04; 00261 i2c.write(0x30, servo_data, 1); 00262 counter.start(); 00263 if(counter.read() > 1.0f) { 00264 phase = 3; 00265 wheel_reset(); 00266 } 00267 break; 00268 00269 //左移動 00270 case 3: 00271 counter.reset(); 00272 left(10000); 00273 if((x_pulse1 > 10000) && (x_pulse2 > 10000)) { 00274 phase = 4; 00275 } 00276 break; 00277 00278 //1秒停止 00279 case 4: 00280 stop(); 00281 counter.start(); 00282 if(counter.read() > 1.0f) { 00283 phase = 5; 00284 wheel_reset(); 00285 } 00286 break; 00287 00288 //右旋回(180°) 00289 case 5: 00290 counter.reset(); 00291 turn_right(518); 00292 if(x_pulse2 > 518) { 00293 phase = 6; 00294 } 00295 break; 00296 00297 //1秒停止 00298 case 6: 00299 stop(); 00300 counter.start(); 00301 if(counter.read() > 1.0f) { 00302 phase = 7; 00303 wheel_reset(); 00304 } 00305 break; 00306 00307 //ちょっくら右移動 00308 case 7: 00309 counter.reset(); 00310 right(-2000); 00311 00312 if(right_limit == 3) { 00313 phase = 8; 00314 } 00315 break; 00316 00317 //1秒停止 00318 case 8: 00319 stop(); 00320 counter.start(); 00321 if(counter.read() > 1.0f) { 00322 phase = 9; 00323 wheel_reset(); 00324 } 00325 break; 00326 00327 //排出 00328 case 9: 00329 counter.reset(); 00330 tyokudo(arm_enc.getPulses(), 10); 00331 break; 00332 00333 //1秒停止 00334 case 10: 00335 stop(); 00336 counter.start(); 00337 if(counter.read() > 1.0f) { 00338 phase = 11; 00339 wheel_reset(); 00340 } 00341 break; 00342 00343 //前進 00344 case 11: 00345 counter.reset(); 00346 front(3500); 00347 if((y_pulse1 > 3500) && (y_pulse2 > 3500)) { 00348 phase = 12; 00349 } 00350 break; 00351 00352 //1秒停止 00353 case 12: 00354 stop(); 00355 counter.start(); 00356 if(counter.read() > 1.0f) { 00357 phase = 13; 00358 wheel_reset(); 00359 } 00360 break; 00361 00362 //右移動 00363 case 13: 00364 counter.reset(); 00365 right(-4000); 00366 if(right_limit == 3) { 00367 phase = 14; 00368 } 00369 break; 00370 00371 //1秒停止 00372 case 14: 00373 stop(); 00374 counter.start(); 00375 if(counter.read() > 1.0f) { 00376 phase = 16; 00377 wheel_reset(); 00378 } 00379 break; 00380 00381 /* 00382 //後進 00383 case 15: 00384 counter.reset(); 00385 back(-1000); 00386 00387 if(back_limit == 3) { 00388 phase = 16; 00389 } 00390 */ 00391 00392 //シーツ装填 00393 case 16: 00394 if(start_switch == 1) { 00395 wheel_reset(); 00396 phase = 17; 00397 } else { 00398 stop(); 00399 } 00400 break; 00401 00402 //竿のラインまで前進 00403 case 17: 00404 counter.reset(); 00405 front(22000); 00406 if((y_pulse1 > 22000) && (y_pulse2 > 22000)) { 00407 phase = 18; 00408 } 00409 break; 00410 00411 //1秒停止 00412 case 18: 00413 stop(); 00414 counter.start(); 00415 if(counter.read() > 1.0f) { 00416 phase = 19; 00417 wheel_reset(); 00418 } 00419 break; 00420 00421 //掛けるところまで左移動 00422 case 19: 00423 counter.reset(); 00424 left(10000); 00425 if((x_pulse1 > 10000) && (x_pulse2 > 10000)) { 00426 phase = 20; 00427 } 00428 break; 00429 00430 //1秒停止 00431 case 20: 00432 stop(); 00433 counter.start(); 00434 if(counter.read() > 1.0f) { 00435 phase = 21; 00436 wheel_reset(); 00437 } 00438 break; 00439 00440 //妨害防止の右旋回 00441 case 21: 00442 counter.reset(); 00443 turn_right(280); 00444 if(x_pulse2 > 280) { 00445 phase = 22; 00446 } 00447 break; 00448 00449 //1秒停止 00450 case 22: 00451 stop(); 00452 counter.start(); 00453 if(counter.read() > 1.0f) { 00454 phase = 23; 00455 wheel_reset(); 00456 } 00457 break; 00458 00459 //カウンターリセット 00460 case 23: 00461 counter.reset(); 00462 counter.start(); 00463 phase = 24; 00464 00465 //アームアップ 00466 case 24: 00467 stop(); 00468 00469 if(counter.read() < 3.0f) { 00470 right_arm_data[0] = 0xFF; 00471 left_arm_data[0] = 0xFF; 00472 i2c.write(0x22, right_arm_data, 1); 00473 i2c.write(0x24, left_arm_data, 1); 00474 wait_us(20); 00475 00476 } else { 00477 arm_up(25); 00478 } 00479 break; 00480 00481 //カウンターリセット 00482 case 25: 00483 counter.reset(); 00484 phase = 26; 00485 00486 //シーツを掛ける 00487 case 26: 00488 counter.start(); 00489 00490 //1秒間ファン送風 00491 if(counter.read() <= 1.0f) { 00492 fan_data[0] = 0xFF; 00493 i2c.write(0x26, fan_data, 1); 00494 i2c.write(0x28, fan_data, 1); 00495 servo_data[0] = 0x04; 00496 i2c.write(0x30, servo_data, 1); 00497 } 00498 //1~3秒の間でサーボを話す 00499 else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) { 00500 fan_data[0] = 0xFF; 00501 i2c.write(0x26, fan_data, 1); 00502 i2c.write(0x28, fan_data, 1); 00503 servo_data[0] = 0x03; 00504 i2c.write(0x30, servo_data, 1); 00505 } 00506 //3秒過ぎたら終わり 00507 else if(counter.read() > 3.0f) { 00508 fan_data[0] = 0x80; 00509 i2c.write(0x26, fan_data, 1); 00510 i2c.write(0x28, fan_data, 1); 00511 servo_data[0] = 0x04; 00512 i2c.write(0x30, servo_data, 1); 00513 phase = 27; 00514 } 00515 break; 00516 00517 //終了っ!(守衛さん風) 00518 case 27: 00519 //駆動系統OFF 00520 emergency = 0; 00521 break; 00522 00523 default: 00524 //駆動系統OFF 00525 emergency = 0; 00526 break; 00527 } 00528 } 00529 00530 //REDゾーン 00531 else if(zone == RED) { 00532 GREEN_LED = 0; 00533 RED_LED = 1; 00534 } 00535 } 00536 } 00537 00538 void init(void) { 00539 00540 //通信ボーレートの設定 00541 pc.baud(460800); 00542 00543 limit_serial.baud(115200); 00544 00545 start_switch.mode(PullUp); 00546 zone_switch.mode(PullDown); 00547 00548 //非常停止関連 00549 pic.baud(19200); 00550 pic.format(8, Serial::None, 1); 00551 pic.attach(get, Serial::RxIrq); 00552 00553 x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; 00554 migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80; 00555 true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80; 00556 fan_data[0] = 0x80; 00557 servo_data[0] = 0x80; 00558 arm_motor[0] = 0x80; drop_motor[0] = 0x80; 00559 right_arm_data[0] = 0x80; left_arm_data[0] = 0x80; 00560 } 00561 00562 void init_send(void) { 00563 00564 init_send_data[0] = 0x80; 00565 i2c.write(0x10, init_send_data, 1); 00566 i2c.write(0x12, init_send_data, 1); 00567 i2c.write(0x14, init_send_data, 1); 00568 i2c.write(0x16, init_send_data, 1); 00569 i2c.write(0x18, init_send_data, 1); 00570 i2c.write(0x20, init_send_data, 1); 00571 i2c.write(0x22, init_send_data, 1); 00572 i2c.write(0x24, init_send_data, 1); 00573 i2c.write(0x30, init_send_data, 1); 00574 wait(0.1); 00575 } 00576 00577 void get(void) { 00578 00579 baff = pic.getc(); 00580 00581 for(; flug; flug--) 00582 RDATA = baff; 00583 00584 if(baff == ':') 00585 flug = 1; 00586 } 00587 00588 void get_pulses(void) { 00589 00590 x_pulse1 = wheel_x1.getPulses(); 00591 x_pulse2 = wheel_x2.getPulses(); 00592 y_pulse1 = wheel_y1.getPulses(); 00593 y_pulse2 = wheel_y2.getPulses(); 00594 } 00595 00596 void print_pulses(void) { 00597 00598 pc.printf("phase: %d\n\r", phase); 00599 //pc.printf("r: %d, l: %d, %d\n\r", right_arm_upper_limit, left_arm_upper_limit, phase); 00600 //pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]); 00601 //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data); 00602 //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase); 00603 //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0], phase); 00604 //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", migimae_data[0], migiusiro_data[0], hidarimae_data[0], hidariusiro_data[0], phase); 00605 00606 } 00607 00608 void get_emergency(void) { 00609 00610 if(RDATA == '1') { 00611 myled = 1; 00612 emergency = 1; 00613 } 00614 else if(RDATA == '9'){ 00615 myled = 0.2; 00616 emergency = 0; 00617 } 00618 } 00619 00620 void read_limit(void) { 00621 00622 limit_data = limit_serial.getc(); 00623 00624 //上位1bitが1ならば下のリミットのデータだと判断 00625 if((limit_data & 0b10000000) == 0b10000000) { 00626 lower_limit_data = limit_data; 00627 00628 //上位1bitが0ならば上のリミットのデータだと判断 00629 } else { 00630 upper_limit_data = limit_data; 00631 } 00632 00633 //下リミット基板からのデータのマスク処理 00634 masked_lower_front_limit_data = lower_limit_data & 0b00000011; 00635 masked_lower_back_limit_data = lower_limit_data & 0b00001100; 00636 masked_lower_right_limit_data = lower_limit_data & 0b00110000; 00637 masked_kaisyu_mae_limit_data = lower_limit_data & 0b01000000; 00638 00639 //上リミット基板からのデータのマスク処理 00640 //masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001; 00641 masked_kaisyu_usiro_limit_data = upper_limit_data & 0b00000001; 00642 masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010; 00643 masked_left_arm_lower_limit_data = upper_limit_data & 0b00000100; 00644 masked_left_arm_upper_limit_data = upper_limit_data & 0b00001000; 00645 masked_tyokudo_mae_limit_data = upper_limit_data & 0b00010000; 00646 masked_tyokudo_usiro_limit_data = upper_limit_data & 0b00100000; 00647 00648 //前部リミット 00649 switch(masked_lower_front_limit_data) { 00650 //両方押された 00651 case 0x00: 00652 front_limit = 3; 00653 break; 00654 //右が押された 00655 case 0b00000010: 00656 front_limit = 1; 00657 break; 00658 //左が押された 00659 case 0b00000001: 00660 front_limit = 2; 00661 break; 00662 default: 00663 front_limit = 0; 00664 break; 00665 } 00666 00667 //後部リミット 00668 switch(masked_lower_back_limit_data) { 00669 //両方押された 00670 case 0x00: 00671 back_limit = 3; 00672 break; 00673 //右が押された 00674 case 0b00001000: 00675 back_limit = 1; 00676 break; 00677 //左が押された 00678 case 0b00000100: 00679 back_limit = 2; 00680 break; 00681 default: 00682 back_limit = 0; 00683 break; 00684 } 00685 00686 //右部リミット 00687 switch(masked_lower_right_limit_data) { 00688 //両方押された 00689 case 0x00: 00690 right_limit = 3; 00691 break; 00692 //右が押された 00693 case 0b00100000: 00694 right_limit = 1; 00695 break; 00696 //左が押された 00697 case 0b00010000: 00698 right_limit = 2; 00699 break; 00700 default: 00701 right_limit = 0; 00702 break; 00703 } 00704 00705 //回収機構リミット 00706 switch(masked_kaisyu_mae_limit_data) { 00707 //押された 00708 case 0b00000000: 00709 kaisyu_mae_limit = 1; 00710 break; 00711 case 0b01000000: 00712 kaisyu_mae_limit = 0; 00713 break; 00714 default: 00715 kaisyu_mae_limit = 0; 00716 break; 00717 } 00718 00719 //右腕下部リミット 00720 /* 00721 switch(masked_right_arm_lower_limit_data) { 00722 //押された 00723 case 0b00000000: 00724 right_arm_lower_limit = 1; 00725 break; 00726 case 0b00000001: 00727 right_arm_lower_limit = 0; 00728 break; 00729 default: 00730 right_arm_lower_limit = 0; 00731 break; 00732 } 00733 */ 00734 00735 //回収後リミット 00736 switch(masked_kaisyu_usiro_limit_data) { 00737 case 0b00000000: 00738 kaisyu_usiro_limit = 1; 00739 break; 00740 case 0b00000001: 00741 kaisyu_usiro_limit = 0; 00742 break; 00743 default: 00744 kaisyu_usiro_limit = 0; 00745 break; 00746 } 00747 00748 //右腕上部リミット 00749 switch(masked_right_arm_upper_limit_data) { 00750 //押された 00751 case 0b00000000: 00752 right_arm_upper_limit = 1; 00753 break; 00754 case 0b00000010: 00755 right_arm_upper_limit = 0; 00756 break; 00757 default: 00758 right_arm_upper_limit = 0; 00759 break; 00760 } 00761 00762 //左腕下部リミット 00763 switch(masked_left_arm_lower_limit_data) { 00764 //押された 00765 case 0b00000000: 00766 left_arm_lower_limit = 1; 00767 break; 00768 case 0b00000100: 00769 left_arm_lower_limit = 0; 00770 break; 00771 default: 00772 left_arm_lower_limit = 0; 00773 break; 00774 } 00775 00776 //左腕上部リミット 00777 switch(masked_left_arm_upper_limit_data) { 00778 //押された 00779 case 0b00000000: 00780 left_arm_upper_limit = 1; 00781 break; 00782 case 0b00001000: 00783 left_arm_upper_limit = 0; 00784 break; 00785 default: 00786 left_arm_upper_limit = 0; 00787 break; 00788 } 00789 00790 //直動の前 00791 switch(masked_tyokudo_mae_limit_data) { 00792 //押された 00793 case 0b00000000: 00794 tyokudo_mae_limit = 1; 00795 break; 00796 case 0b00010000: 00797 tyokudo_mae_limit = 0; 00798 break; 00799 default: 00800 tyokudo_mae_limit = 0; 00801 break; 00802 } 00803 00804 //直動の後 00805 switch(masked_tyokudo_usiro_limit_data) { 00806 //押された 00807 case 0b00000000: 00808 tyokudo_usiro_limit = 1; 00809 break; 00810 case 0b00100000: 00811 tyokudo_usiro_limit = 0; 00812 break; 00813 default: 00814 tyokudo_usiro_limit = 0; 00815 break; 00816 } 00817 } 00818 00819 void wheel_reset(void) { 00820 00821 wheel_x1.reset(); 00822 wheel_x2.reset(); 00823 wheel_y1.reset(); 00824 wheel_y2.reset(); 00825 } 00826 00827 void kaisyu(int pulse, int next_phase) { 00828 00829 switch (kaisyu_phase) { 00830 00831 case 0: 00832 //前進->減速 00833 //3000pulseまで高速前進 00834 if(pulse < 3000) { 00835 arm_motor[0] = 0xFF; 00836 //kaisyu_phase = 1; 00837 } 00838 00839 //3000pulse超えたら低速前進 00840 else if(pulse >= 3000) { 00841 arm_motor[0] = 0xB3; 00842 kaisyu_phase = 1; 00843 } 00844 break; 00845 00846 case 1: 00847 USR_LED3 = 1; 00848 //前進->停止->後進 00849 //3600pulseまで低速前進 00850 if(pulse < 3600) { 00851 arm_motor[0] = 0xB3; 00852 //kaisyu_phase = 2; 00853 } 00854 00855 //3600pulse超えたら停止 00856 else if(pulse >= 3600) { 00857 arm_motor[0] = 0x80; 00858 00859 //1秒待ってから引っ込める 00860 counter.start(); 00861 if(counter.read() > 1.0f) { 00862 kaisyu_phase = 2; 00863 } 00864 } 00865 //後ろのリミットが押されたら強制停止 00866 if(kaisyu_usiro_limit == 1) { 00867 arm_motor[0] = 0x80; 00868 } 00869 break; 00870 00871 case 2: 00872 //後進->減速 00873 //500pulseまで高速後進 00874 counter.reset(); 00875 if(pulse > 500) { 00876 arm_motor[0] = 0x00; 00877 //kaisyu_phase = 3; 00878 00879 } 00880 //500pulse以下になったら低速後進 00881 else if(pulse <= 500) { 00882 arm_motor[0] = 0x4C; 00883 kaisyu_phase = 3; 00884 } 00885 break; 00886 00887 case 3: 00888 //後進->停止 00889 //リミット押されるまで低速後進 00890 if(pulse <= 500) { 00891 arm_motor[0] = 0x4C; 00892 //kaisyu_phase = 4; 00893 } 00894 00895 //リミット押されたら停止 00896 if(kaisyu_mae_limit == 1) { 00897 arm_motor[0] = 0x80; 00898 kaisyu_phase = 4; 00899 phase = next_phase; 00900 } 00901 break; 00902 00903 default: 00904 arm_motor[0] = 0x80; 00905 break; 00906 } 00907 00908 //回収MDへ書き込み 00909 i2c.write(0x18, arm_motor, 1); 00910 } 00911 00912 void tyokudo(int pulse, int next_phase) { 00913 00914 switch(tyokudo_phase) { 00915 00916 case 0: 00917 //前進->減速 00918 00919 /* エンコーダー読まずにリミットだけ(修正必須) */ 00920 //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行 00921 if(tyokudo_mae_limit == 0) { 00922 //2000pulseまで高速前進 00923 if(pulse < 2000) { 00924 arm_motor[0] = 0xC0; 00925 drop_motor[0] = 0xE6; 00926 } 00927 //2000pulse以上で低速前進 00928 else if(pulse >= 2000) { 00929 arm_motor[0] = 0xC0; 00930 drop_motor[0] = 0xE6; 00931 } 00932 //パルスが3600を終えたらアームのみ強制停止 00933 else if(pulse > 3600) { 00934 arm_motor[0] = 0x80; 00935 drop_motor[0] = 0xE6; 00936 } 00937 00938 //後ろのリミットが押されたら強制停止 00939 if(kaisyu_usiro_limit == 1) { 00940 arm_motor[0] = 0x80; 00941 } 00942 } 00943 00944 //直動の前リミットが押されたら 00945 else if(tyokudo_mae_limit == 1) { 00946 //高速後進 00947 arm_motor[0] = 0x40; 00948 drop_motor[0] = 0x00; 00949 tyokudo_phase = 1; 00950 } 00951 break; 00952 00953 case 1: 00954 //後進->減速 00955 //リミットが押されたら強制停止 00956 if(tyokudo_usiro_limit == 1) { 00957 arm_motor[0] = 0x80; 00958 drop_motor[0] = 0x80; 00959 tyokudo_phase = 2; 00960 phase = next_phase; 00961 } 00962 break; 00963 00964 default: 00965 arm_motor[0] = 0x80; 00966 drop_motor[0] = 0x80; 00967 break; 00968 } 00969 00970 i2c.write(0x18, arm_motor, 1); 00971 i2c.write(0x20, drop_motor, 1); 00972 } 00973 00974 void arm_up(int next_phase) { 00975 00976 //両腕、上限リミットが押されてなかったら上昇 00977 if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) { 00978 right_arm_data[0] = 0xFF; left_arm_data[0] = 0xFF; 00979 } 00980 //右腕のみリミットが押されたら左腕のみ上昇 00981 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) { 00982 right_arm_data[0] = 0x80; left_arm_data[0] = 0xFF; 00983 } 00984 //左腕のみリミットが押されたら右腕のみ上昇 00985 else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) { 00986 right_arm_data[0] = 0xFF; left_arm_data[0] = 0x80; 00987 } 00988 //両腕、上限リミットが押されたら停止 00989 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) { 00990 right_arm_data[0] = 0x80; left_arm_data[0] = 0x80; 00991 phase = next_phase; 00992 } 00993 00994 i2c.write(0x22, right_arm_data, 1); 00995 i2c.write(0x24, left_arm_data, 1); 00996 wait_us(20); 00997 } 00998 00999 void front(int target) { 01000 01001 front_PID(target); 01002 i2c.write(0x10, true_migimae_data, 1, false); 01003 i2c.write(0x12, true_migiusiro_data, 1, false); 01004 i2c.write(0x14, true_hidarimae_data, 1, false); 01005 i2c.write(0x16, true_hidariusiro_data, 1, false); 01006 wait_us(20); 01007 } 01008 01009 void back(int target) { 01010 01011 back_PID(target); 01012 i2c.write(0x10, true_migimae_data, 1, false); 01013 i2c.write(0x12, true_migiusiro_data, 1, false); 01014 i2c.write(0x14, true_hidarimae_data, 1, false); 01015 i2c.write(0x16, true_hidariusiro_data, 1, false); 01016 wait_us(20); 01017 } 01018 01019 void right(int target) { 01020 01021 right_PID(target); 01022 i2c.write(0x10, true_migimae_data, 1, false); 01023 i2c.write(0x12, true_migiusiro_data, 1, false); 01024 i2c.write(0x14, true_hidarimae_data, 1, false); 01025 i2c.write(0x16, true_hidariusiro_data, 1, false); 01026 wait_us(20); 01027 } 01028 01029 void left(int target) { 01030 01031 left_PID(target); 01032 i2c.write(0x10, true_migimae_data, 1, false); 01033 i2c.write(0x12, true_migiusiro_data, 1, false); 01034 i2c.write(0x14, true_hidarimae_data, 1, false); 01035 i2c.write(0x16, true_hidariusiro_data, 1, false); 01036 wait_us(20); 01037 } 01038 01039 void turn_right(int target) { 01040 01041 turn_right_PID(target); 01042 i2c.write(0x10, true_migimae_data, 1, false); 01043 i2c.write(0x12, true_migiusiro_data, 1, false); 01044 i2c.write(0x14, true_hidarimae_data, 1, false); 01045 i2c.write(0x16, true_hidariusiro_data, 1, false); 01046 wait_us(20); 01047 } 01048 01049 void turn_left(int target) { 01050 01051 turn_left_PID(target); 01052 i2c.write(0x10, true_migimae_data, 1, false); 01053 i2c.write(0x12, true_migiusiro_data, 1, false); 01054 i2c.write(0x14, true_hidarimae_data, 1, false); 01055 i2c.write(0x16, true_hidariusiro_data, 1, false); 01056 wait_us(20); 01057 } 01058 01059 void stop(void) { 01060 01061 true_migimae_data[0] = 0x80; 01062 true_migiusiro_data[0] = 0x80; 01063 true_hidarimae_data[0] = 0x80; 01064 true_hidariusiro_data[0] = 0x80; 01065 01066 i2c.write(0x10, true_migimae_data, 1, false); 01067 i2c.write(0x12, true_migiusiro_data, 1, false); 01068 i2c.write(0x14, true_hidarimae_data, 1, false); 01069 i2c.write(0x16, true_hidariusiro_data, 1, false); 01070 wait_us(20); 01071 } 01072 01073 void front_PID(int target) { 01074 01075 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01076 front_migimae.setInputLimits(-2147483648, 2147483647); 01077 front_migiusiro.setInputLimits(-2147483648, 2147483647); 01078 front_hidarimae.setInputLimits(-2147483648, 2147483647); 01079 front_hidariusiro.setInputLimits(-2147483648, 2147483647); 01080 01081 //制御量の最小、最大 01082 //正転(目標に達してない) 01083 if((y_pulse1 < target) && (y_pulse2 < target)) { 01084 front_migimae.setOutputLimits(0x84, 0xF7); 01085 front_migiusiro.setOutputLimits(0x84, 0xF7); 01086 //front_migimae.setOutputLimits(0x84, 0xFF); 01087 //front_migiusiro.setOutputLimits(0x84, 0xFF); 01088 front_hidarimae.setOutputLimits(0x84, 0xFF); 01089 front_hidariusiro.setOutputLimits(0x84, 0xFF); 01090 } 01091 /* 01092 //左側が前に出ちゃった♥(右側だけ回して左側は停止) 01093 else if((y_pulse1 + wheel_difference) < y_pulse2) { 01094 front_migimae.setOutputLimits(0x84, 0xFF); 01095 front_migiusiro.setOutputLimits(0x84, 0xFF); 01096 front_hidarimae.setOutputLimits(0x7C, 0x83); 01097 front_hidariusiro.setOutputLimits(0x7C, 0x83); 01098 } 01099 //右側が前に出ちゃった♥(左側だけ回して右側は停止) 01100 else if(y_pulse1 > (y_pulse2 + wheel_difference)) { 01101 front_migimae.setOutputLimits(0x7C, 0x83); 01102 front_migiusiro.setOutputLimits(0x7C, 0x83); 01103 front_hidarimae.setOutputLimits(0x84, 0xFF); 01104 front_hidariusiro.setOutputLimits(0x84, 0xFF); 01105 } 01106 */ 01107 //停止(目標より行き過ぎ) 01108 else if((y_pulse1 > target) && (y_pulse2 > target)) { 01109 front_migimae.setOutputLimits(0x7C, 0x83); 01110 front_migiusiro.setOutputLimits(0x7C, 0x83); 01111 front_hidarimae.setOutputLimits(0x7C, 0x83); 01112 front_hidariusiro.setOutputLimits(0x7C, 0x83); 01113 } 01114 01115 //よくわからんやつ 01116 front_migimae.setMode(AUTO_MODE); 01117 front_migiusiro.setMode(AUTO_MODE); 01118 front_hidarimae.setMode(AUTO_MODE); 01119 front_hidariusiro.setMode(AUTO_MODE); 01120 01121 //目標値 01122 front_migimae.setSetPoint(target); 01123 front_migiusiro.setSetPoint(target); 01124 front_hidarimae.setSetPoint(target); 01125 front_hidariusiro.setSetPoint(target); 01126 01127 //センサ出力 01128 front_migimae.setProcessValue(y_pulse1); 01129 front_migiusiro.setProcessValue(y_pulse1); 01130 front_hidarimae.setProcessValue(y_pulse2); 01131 front_hidariusiro.setProcessValue(y_pulse2); 01132 01133 //制御量(計算結果) 01134 migimae_data[0] = front_migimae.compute(); 01135 migiusiro_data[0] = front_migiusiro.compute(); 01136 hidarimae_data[0] = front_hidarimae.compute(); 01137 hidariusiro_data[0] = front_hidariusiro.compute(); 01138 01139 //制御量をPWM値に変換 01140 //正転(目標に達してない) 01141 if((y_pulse1 < target) && (y_pulse2 < target)) { 01142 true_migimae_data[0] = migimae_data[0]; 01143 true_migiusiro_data[0] = migiusiro_data[0]; 01144 true_hidarimae_data[0] = hidarimae_data[0]; 01145 true_hidariusiro_data[0] = hidariusiro_data[0]; 01146 } 01147 /* 01148 //左側が前に出ちゃった♥(右側だけ回して左側は停止) 01149 else if((y_pulse1 + wheel_difference) < y_pulse2) { 01150 true_migimae_data[0] = migimae_data[0]; 01151 true_migiusiro_data[0] = migiusiro_data[0]; 01152 true_hidarimae_data[0] = 0x80; 01153 true_hidariusiro_data[0] = 0x80; 01154 } 01155 //右側が前に出ちゃった♥(左側だけ回して右側は停止) 01156 else if(y_pulse1 > (y_pulse2 + wheel_difference)) { 01157 true_migimae_data[0] = 0x80; 01158 true_migiusiro_data[0] = 0x80; 01159 true_hidarimae_data[0] = hidarimae_data[0]; 01160 true_hidariusiro_data[0] = hidariusiro_data[0]; 01161 } 01162 */ 01163 //停止(目標より行き過ぎ) 01164 else if((y_pulse1 > target) && (y_pulse2 > target)) { 01165 true_migimae_data[0] = 0x80; 01166 true_migiusiro_data[0] = 0x80; 01167 true_hidarimae_data[0] = 0x80; 01168 true_hidariusiro_data[0] = 0x80; 01169 } 01170 } 01171 01172 void back_PID(int target) { 01173 01174 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01175 back_migimae.setInputLimits(-2147483648, 2147483647); 01176 back_migiusiro.setInputLimits(-2147483648, 2147483647); 01177 back_hidarimae.setInputLimits(-2147483648, 2147483647); 01178 back_hidariusiro.setInputLimits(-2147483648, 2147483647); 01179 01180 //制御量の最小、最大 01181 //逆転(目標に達してない) 01182 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { 01183 back_migimae.setOutputLimits(0x00, 0x7B); 01184 back_migiusiro.setOutputLimits(0x00, 0x7B); 01185 //back_hidarimae.setOutputLimits(0x00, 0x73); 01186 //back_hidariusiro.setOutputLimits(0x00, 0x73); 01187 back_hidarimae.setOutputLimits(0x00, 0x7B); 01188 back_hidariusiro.setOutputLimits(0x00, 0x7B); 01189 } 01190 /* 01191 //左側が後に出ちゃった♥(右側だけ回して左側は停止) 01192 else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){ 01193 back_migimae.setOutputLimits(0x00, 0x7B); 01194 back_migiusiro.setOutputLimits(0x00, 0x7B); 01195 back_hidarimae.setOutputLimits(0x7C, 0x83); 01196 back_hidariusiro.setOutputLimits(0x7C, 0x83); 01197 } 01198 //右側が後に出ちゃった♥(左側だけ回して右側は停止) 01199 else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){ 01200 back_migimae.setOutputLimits(0x7C, 0x83); 01201 back_migiusiro.setOutputLimits(0x7C, 0x83); 01202 back_hidarimae.setOutputLimits(0x00, 0x7B); 01203 back_hidariusiro.setOutputLimits(0x00, 0x7B); 01204 } 01205 */ 01206 //停止(目標より行き過ぎ) 01207 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { 01208 back_migimae.setOutputLimits(0x7C, 0x83); 01209 back_migiusiro.setOutputLimits(0x7C, 0x83); 01210 back_hidarimae.setOutputLimits(0x7C, 0x83); 01211 back_hidariusiro.setOutputLimits(0x7C, 0x83); 01212 } 01213 01214 //よくわからんやつ 01215 back_migimae.setMode(AUTO_MODE); 01216 back_migiusiro.setMode(AUTO_MODE); 01217 back_hidarimae.setMode(AUTO_MODE); 01218 back_hidariusiro.setMode(AUTO_MODE); 01219 01220 //目標値 01221 back_migimae.setSetPoint(target*-1); 01222 back_migiusiro.setSetPoint(target*-1); 01223 back_hidarimae.setSetPoint(target*-1); 01224 back_hidariusiro.setSetPoint(target*-1); 01225 01226 //センサ出力 01227 back_migimae.setProcessValue(y_pulse1*-1); 01228 back_migiusiro.setProcessValue(y_pulse1*-1); 01229 back_hidarimae.setProcessValue(y_pulse2*-1); 01230 back_hidariusiro.setProcessValue(y_pulse2*-1); 01231 01232 //制御量(計算結果) 01233 migimae_data[0] = back_migimae.compute(); 01234 migiusiro_data[0] = back_migiusiro.compute(); 01235 hidarimae_data[0] = back_hidarimae.compute(); 01236 hidariusiro_data[0] = back_hidariusiro.compute(); 01237 01238 //制御量をPWM値に変換 01239 //逆転(目標に達してない) 01240 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { 01241 true_migimae_data[0] = 0x7B - migimae_data[0]; 01242 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01243 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01244 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01245 } 01246 /* 01247 //左側が後に出ちゃった♥(右側だけ回して左側は停止) 01248 else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){ 01249 true_migimae_data[0] = 0x7B - migimae_data[0]; 01250 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01251 true_hidarimae_data[0] = 0x80; 01252 true_hidariusiro_data[0] = 0x80; 01253 } 01254 //右側が後に出ちゃった♥(左側だけ回して右側は停止) 01255 else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){ 01256 true_migimae_data[0] = 0x80; 01257 true_migiusiro_data[0] = 0x80; 01258 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01259 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01260 } 01261 */ 01262 //停止(目標より行き過ぎ) 01263 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { 01264 true_migimae_data[0] = 0x80; 01265 true_migiusiro_data[0] = 0x80; 01266 true_hidarimae_data[0] = 0x80; 01267 true_hidariusiro_data[0] = 0x80; 01268 } 01269 } 01270 01271 void right_PID(int target) { 01272 01273 //センサ出力値の最小、最大 01274 right_migimae.setInputLimits(-2147483648, 2147483647); 01275 right_migiusiro.setInputLimits(-2147483648, 2147483647); 01276 right_hidarimae.setInputLimits(-2147483648, 2147483647); 01277 right_hidariusiro.setInputLimits(-2147483648, 2147483647); 01278 01279 //制御量の最小、最大 01280 //右進(目標まで達していない) 01281 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { 01282 // right_migimae.setOutputLimits(0x00, 0x6C); 01283 right_migimae.setOutputLimits(0x7A, 0x7B); 01284 right_migiusiro.setOutputLimits(0xFE, 0xFF); 01285 //right_hidarimae.setOutputLimits(0x84, 0xF0); 01286 right_hidarimae.setOutputLimits(0xFE, 0xFF); 01287 right_hidariusiro.setOutputLimits(0x7A, 0x7B); 01288 01289 } 01290 /* 01291 //前側が右に出ちゃった♥(後側だけ回して前側は停止) 01292 else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){ 01293 right_migimae.setOutputLimits(0x7C, 0x83); 01294 right_migiusiro.setOutputLimits(0x00, 0x7B); 01295 right_hidarimae.setOutputLimits(0x7C, 0x83); 01296 right_hidariusiro.setOutputLimits(0x84, 0xFF); 01297 } 01298 //後側が右に出ちゃった♥(前側だけ回して後側は停止) 01299 else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){ 01300 right_migimae.setOutputLimits(0x84, 0xFF); 01301 right_migiusiro.setOutputLimits(0x7C, 0x83); 01302 right_hidarimae.setOutputLimits(0x00, 0x7B); 01303 right_hidariusiro.setOutputLimits(0x7C, 0x83); 01304 } 01305 */ 01306 //停止(目標より行き過ぎ) 01307 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { 01308 right_migimae.setOutputLimits(0x7C, 0x83); 01309 right_migiusiro.setOutputLimits(0x7C, 0x83); 01310 right_hidarimae.setOutputLimits(0x7C, 0x83); 01311 right_hidariusiro.setOutputLimits(0x7C, 0x83); 01312 } 01313 01314 //よくわからんやつ 01315 right_migimae.setMode(AUTO_MODE); 01316 right_migiusiro.setMode(AUTO_MODE); 01317 right_hidarimae.setMode(AUTO_MODE); 01318 right_hidariusiro.setMode(AUTO_MODE); 01319 01320 //目標値 01321 right_migimae.setSetPoint(target*-1); 01322 right_migiusiro.setSetPoint(target*-1); 01323 right_hidarimae.setSetPoint(target*-1); 01324 right_hidariusiro.setSetPoint(target*-1); 01325 01326 //センサ出力 01327 right_migimae.setProcessValue(x_pulse1*-1); 01328 right_migiusiro.setProcessValue(x_pulse2*-1); 01329 right_hidarimae.setProcessValue(x_pulse1*-1); 01330 right_hidariusiro.setProcessValue(x_pulse2*-1); 01331 01332 //制御量(計算結果) 01333 migimae_data[0] = right_migimae.compute(); 01334 migiusiro_data[0] = right_migiusiro.compute(); 01335 hidarimae_data[0] = right_hidarimae.compute(); 01336 hidariusiro_data[0] = right_hidariusiro.compute(); 01337 01338 //制御量をPWM値に変換 01339 //右進(目標まで達していない) 01340 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { 01341 true_migimae_data[0] = 0x7B - migimae_data[0]; 01342 true_migiusiro_data[0] = migiusiro_data[0]; 01343 true_hidarimae_data[0] = hidarimae_data[0]; 01344 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01345 } 01346 /* 01347 //前側が右に出ちゃった♥(後側だけ回して前側は停止) 01348 else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){ 01349 true_migimae_data[0] = 0x80; 01350 true_migiusiro_data[0] = migiusiro_data[0]; 01351 true_hidarimae_data[0] = 0x80; 01352 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01353 } 01354 //後側が右に出ちゃった♥(前側だけ回して後側は停止) 01355 else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){ 01356 true_migimae_data[0] = 0x7B - migimae_data[0]; 01357 true_migiusiro_data[0] = 0x80; 01358 true_hidarimae_data[0] = hidarimae_data[0]; 01359 true_hidariusiro_data[0] = 0x80; 01360 } 01361 */ 01362 //左進(目標より行き過ぎ) 01363 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { 01364 true_migimae_data[0] = 0x80; 01365 true_migiusiro_data[0] = 0x80; 01366 true_hidarimae_data[0] = 0x80; 01367 true_hidariusiro_data[0] = 0x80; 01368 } 01369 } 01370 01371 void left_PID(int target) { 01372 01373 //センサ出力値の最小、最大 01374 left_migimae.setInputLimits(-2147483648, 2147483647); 01375 left_migiusiro.setInputLimits(-2147483648, 2147483647); 01376 left_hidarimae.setInputLimits(-2147483648, 2147483647); 01377 left_hidariusiro.setInputLimits(-2147483648, 2147483647); 01378 01379 //制御量の最小、最大 01380 //左進(目標まで達していない) 01381 if((x_pulse1 < target) || (x_pulse2 < target)) { 01382 left_migimae.setOutputLimits(0xEC, 0xED); 01383 //left_migimae.setOutputLimits(0x84, 0xFF); 01384 left_migiusiro.setOutputLimits(0x7A, 0x7B); 01385 left_hidarimae.setOutputLimits(0x7A, 0x7B); 01386 left_hidariusiro.setOutputLimits(0xFE, 0xFF); 01387 } 01388 /* 01389 //前側が左に出ちゃった♥(後側だけ回して前側は停止) 01390 else if(x_pulse1 > (x_pulse2 + wheel_difference)){ 01391 left_migimae.setOutputLimits(0x7C, 0x83); 01392 left_migiusiro.setOutputLimits(0x7C, 0x83); 01393 left_hidarimae.setOutputLimits(0x10, 0x7B); 01394 left_hidariusiro.setOutputLimits(0x94, 0xFF); 01395 } 01396 //後側が左に出ちゃった♥(前側だけ回して後側は停止) 01397 else if((x_pulse1 + wheel_difference) < x_pulse2){ 01398 left_migimae.setOutputLimits(0x94, 0xFF); 01399 left_migiusiro.setOutputLimits(0x10, 0x7B); 01400 left_hidarimae.setOutputLimits(0x7C, 0x83); 01401 left_hidariusiro.setOutputLimits(0x7C, 0x83); 01402 } 01403 */ 01404 //停止(目標より行き過ぎ) 01405 else if((x_pulse1 > target) || (x_pulse2 > target)) { 01406 left_migimae.setOutputLimits(0x7C, 0x83); 01407 left_migiusiro.setOutputLimits(0x7C, 0x83); 01408 left_hidarimae.setOutputLimits(0x7C, 0x83); 01409 left_hidariusiro.setOutputLimits(0x7C, 0x83); 01410 } 01411 01412 //よくわからんやつ 01413 left_migimae.setMode(AUTO_MODE); 01414 left_migiusiro.setMode(AUTO_MODE); 01415 left_hidarimae.setMode(AUTO_MODE); 01416 left_hidariusiro.setMode(AUTO_MODE); 01417 01418 //目標値 01419 left_migimae.setSetPoint(target); 01420 left_migiusiro.setSetPoint(target); 01421 left_hidarimae.setSetPoint(target); 01422 left_hidariusiro.setSetPoint(target); 01423 01424 //センサ出力 01425 left_migimae.setProcessValue(x_pulse1); 01426 left_migiusiro.setProcessValue(x_pulse2); 01427 left_hidarimae.setProcessValue(x_pulse1); 01428 left_hidariusiro.setProcessValue(x_pulse2); 01429 01430 //制御量(計算結果) 01431 migimae_data[0] = left_migimae.compute(); 01432 migiusiro_data[0] = left_migiusiro.compute(); 01433 hidarimae_data[0] = left_hidarimae.compute(); 01434 hidariusiro_data[0] = left_hidariusiro.compute(); 01435 01436 //制御量をPWM値に変換 01437 //左進(目標まで達していない) 01438 if((x_pulse1 < target) || (x_pulse2 < target)) { 01439 true_migimae_data[0] = migimae_data[0]; 01440 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01441 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01442 true_hidariusiro_data[0] = hidariusiro_data[0]; 01443 } 01444 /* 01445 //前側が左に出ちゃった♥(後側だけ回して前側は停止) 01446 else if(x_pulse1 > (x_pulse2 + wheel_difference)){ 01447 true_migimae_data[0] = 0x80; 01448 true_migiusiro_data[0] = 0x80; 01449 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01450 true_hidariusiro_data[0] = hidariusiro_data[0]; 01451 } 01452 //後側が左に出ちゃった♥(前側だけ回して後側は停止) 01453 else if((x_pulse1 + wheel_difference) < x_pulse2){ 01454 true_migimae_data[0] = migimae_data[0]; 01455 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01456 true_hidarimae_data[0] = 0x80; 01457 true_hidariusiro_data[0] = 0x80; 01458 } 01459 */ 01460 //停止(目標より行き過ぎ) 01461 else if((x_pulse1 > target) || (x_pulse2 > target)) { 01462 true_migimae_data[0] = 0x80; 01463 true_migiusiro_data[0] = 0x80; 01464 true_hidarimae_data[0] = 0x80; 01465 true_hidariusiro_data[0] = 0x80; 01466 } 01467 } 01468 01469 void turn_right_PID(int target) { 01470 01471 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01472 turn_right_migimae.setInputLimits(-2147483648, 2147483647); 01473 turn_right_migiusiro.setInputLimits(-2147483648, 2147483647); 01474 turn_right_hidarimae.setInputLimits(-2147483648, 2147483647); 01475 turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647); 01476 01477 //制御量の最小、最大 01478 //右旋回(目標に達してない) 01479 if(x_pulse2 < target) { 01480 turn_right_migimae.setOutputLimits(0x10, 0x7B); 01481 turn_right_migiusiro.setOutputLimits(0x10, 0x7B); 01482 turn_right_hidarimae.setOutputLimits(0x94, 0xFF); 01483 turn_right_hidariusiro.setOutputLimits(0x94, 0xFF); 01484 } 01485 //停止(目標より行き過ぎ) 01486 else if(x_pulse2 > target) { 01487 turn_right_migimae.setOutputLimits(0x7C, 0x83); 01488 turn_right_migiusiro.setOutputLimits(0x7C, 0x83); 01489 turn_right_hidarimae.setOutputLimits(0x7C, 0x83); 01490 turn_right_hidariusiro.setOutputLimits(0x7C, 0x83); 01491 } 01492 01493 //よくわからんやつ 01494 turn_right_migimae.setMode(AUTO_MODE); 01495 turn_right_migiusiro.setMode(AUTO_MODE); 01496 turn_right_hidarimae.setMode(AUTO_MODE); 01497 turn_right_hidariusiro.setMode(AUTO_MODE); 01498 01499 //目標値 01500 turn_right_migimae.setSetPoint(target); 01501 turn_right_migiusiro.setSetPoint(target); 01502 turn_right_hidarimae.setSetPoint(target); 01503 turn_right_hidariusiro.setSetPoint(target); 01504 01505 //センサ出力 01506 turn_right_migimae.setProcessValue(x_pulse2); 01507 turn_right_migiusiro.setProcessValue(x_pulse2); 01508 turn_right_hidarimae.setProcessValue(x_pulse2); 01509 turn_right_hidariusiro.setProcessValue(x_pulse2); 01510 01511 //制御量(計算結果) 01512 migimae_data[0] = turn_right_migimae.compute(); 01513 migiusiro_data[0] = turn_right_migiusiro.compute(); 01514 hidarimae_data[0] = turn_right_hidarimae.compute(); 01515 hidariusiro_data[0] = turn_right_hidariusiro.compute(); 01516 01517 //制御量をPWM値に変換 01518 //右旋回(目標に達してない) 01519 if(x_pulse2 < target) { 01520 true_migimae_data[0] = 0x7B - migimae_data[0]; 01521 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01522 true_hidarimae_data[0] = hidarimae_data[0]; 01523 true_hidariusiro_data[0] = hidariusiro_data[0]; 01524 } 01525 //停止(目標より行き過ぎ) 01526 else if(x_pulse2 > target) { 01527 true_migimae_data[0] = 0x80; 01528 true_migiusiro_data[0] = 0x80; 01529 true_hidarimae_data[0] = 0x80; 01530 true_hidariusiro_data[0] = 0x80; 01531 } 01532 } 01533 01534 void turn_left_PID(int target) { 01535 01536 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01537 turn_left_migimae.setInputLimits(-2147483648, 2147483647); 01538 turn_left_migiusiro.setInputLimits(-2147483648, 2147483647); 01539 turn_left_hidarimae.setInputLimits(-2147483648, 2147483647); 01540 turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647); 01541 01542 //制御量の最小、最大 01543 //左旋回(目標に達してない) 01544 if(x_pulse1 < target) { 01545 turn_left_migimae.setOutputLimits(0x94, 0xFF); 01546 turn_left_migiusiro.setOutputLimits(0x94, 0xFF); 01547 turn_left_hidarimae.setOutputLimits(0x10, 0x7B); 01548 turn_left_hidariusiro.setOutputLimits(0x10, 0x7B); 01549 } 01550 //停止(目標より行き過ぎ) 01551 else if(x_pulse1 > target) { 01552 turn_left_migimae.setOutputLimits(0x7C, 0x83); 01553 turn_left_migiusiro.setOutputLimits(0x7C, 0x83); 01554 turn_left_hidarimae.setOutputLimits(0x7C, 0x83); 01555 turn_left_hidariusiro.setOutputLimits(0x7C, 0x83); 01556 } 01557 01558 //よくわからんやつ 01559 turn_left_migimae.setMode(AUTO_MODE); 01560 turn_left_migiusiro.setMode(AUTO_MODE); 01561 turn_left_hidarimae.setMode(AUTO_MODE); 01562 turn_left_hidariusiro.setMode(AUTO_MODE); 01563 01564 //目標値 01565 turn_left_migimae.setSetPoint(target); 01566 turn_left_migiusiro.setSetPoint(target); 01567 turn_left_hidarimae.setSetPoint(target); 01568 turn_left_hidariusiro.setSetPoint(target); 01569 01570 //センサ出力 01571 turn_left_migimae.setProcessValue(x_pulse1); 01572 turn_left_migiusiro.setProcessValue(x_pulse1); 01573 turn_left_hidarimae.setProcessValue(x_pulse1); 01574 turn_left_hidariusiro.setProcessValue(x_pulse1); 01575 01576 //制御量(計算結果) 01577 migimae_data[0] = turn_left_migimae.compute(); 01578 migiusiro_data[0] = turn_left_migiusiro.compute(); 01579 hidarimae_data[0] = turn_left_hidarimae.compute(); 01580 hidariusiro_data[0] = turn_left_hidariusiro.compute(); 01581 01582 //制御量をPWM値に変換 01583 //左旋回(目標に達してない) 01584 if(x_pulse1 < target) { 01585 true_migimae_data[0] = migimae_data[0]; 01586 true_migiusiro_data[0] = migiusiro_data[0]; 01587 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01588 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01589 } 01590 //左旋回(目標より行き過ぎ) 01591 else if(x_pulse1 > target) { 01592 true_migimae_data[0] = 0x80; 01593 true_migiusiro_data[0] = 0x80; 01594 true_hidarimae_data[0] = 0x80; 01595 true_hidariusiro_data[0] = 0x80; 01596 } 01597 }
Generated on Thu Sep 15 2022 04:10:31 by
1.7.2