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