Yuhi Takaku
/
2019_A_ver8
ウオッチドッグタイマを追加したよん
Diff: main.cpp
- Revision:
- 22:5682246f9409
- Parent:
- 21:89db2a19e52e
- Child:
- 23:1e4d7540715f
--- a/main.cpp Sat Sep 21 14:38:10 2019 +0000 +++ b/main.cpp Wed Sep 25 02:07:26 2019 +0000 @@ -2,9 +2,10 @@ /* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */ /* Nucleo Type: F446RE */ /* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */ -/* Sensor: encorder*4 */ +/* Actuator: RS-555*4, RS-380*2, RZ-735*2, RS-385*2, PWM_Servo(KONDO)*2 */ +/* Sensor: encorder*4, limit_switch*14 */ /* ------------------------------------------------------------------- */ -/* blue zone is ok, added back phase */ +/* added red zone(checked) */ /* ------------------------------------------------------------------- */ #include "mbed.h" #include "math.h" @@ -43,16 +44,16 @@ PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001); //右旋回 -PID turn_right_migimae(30000000.0, 0.0, 0.0, 0.001); -PID turn_right_migiusiro(30000000.0, 0.0, 0.0, 0.001); -PID turn_right_hidarimae(30000000.0, 0.0, 0.0, 0.001); -PID turn_right_hidariusiro(30000000.0, 0.0, 0.0, 0.001); +PID turn_right_migimae(3000000.0, 0.0, 0.0, 0.001); +PID turn_right_migiusiro(3000000.0, 0.0, 0.0, 0.001); +PID turn_right_hidarimae(3000000.0, 0.0, 0.0, 0.001); +PID turn_right_hidariusiro(3000000.0, 0.0, 0.0, 0.001); //左旋回 -PID turn_left_migimae(30000000.0, 0.0, 0.0, 0.001); -PID turn_left_migiusiro(30000000.0, 0.0, 0.0, 0.001); -PID turn_left_hidarimae(30000000.0, 0.0, 0.0, 0.001); -PID turn_left_hidariusiro(30000000.0, 0.0, 0.0, 0.001); +PID turn_left_migimae(3000000.0, 0.0, 0.0, 0.001); +PID turn_left_migiusiro(3000000.0, 0.0, 0.0, 0.001); +PID turn_left_hidarimae(3000000.0, 0.0, 0.0, 0.001); +PID turn_left_hidariusiro(3000000.0, 0.0, 0.0, 0.001); //MDとの通信ポート I2C i2c(PB_9, PB_8); //SDA, SCL @@ -93,7 +94,7 @@ Timer counter; //エンコーダ値格納変数 -int x_pulse1, x_pulse2, y_pulse1, y_pulse2; +int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse; //操作の段階変数 unsigned int phase = 0; @@ -177,6 +178,7 @@ void turn_right(int target); void turn_left(int target); void stop(void); +void all_stop(void); void front_PID(int target); void back_PID(int target); void right_PID(int target); @@ -193,7 +195,7 @@ //zone = BLUE; //phase = 16; //phase = 23; - phase = 30; + phase = 50; //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止) while(1) { @@ -222,9 +224,21 @@ } if(start_switch == 1) { - phase = 23; + //phase = 31; + right_arm_data[0] = 0xFF; + left_arm_data[0] = 0xFF; + i2c.write(0x22, right_arm_data, 1); + i2c.write(0x24, left_arm_data, 1); + wait_us(20); + } else { + right_arm_data[0] = 0x80; + left_arm_data[0] = 0x80; + i2c.write(0x22, right_arm_data, 1); + i2c.write(0x24, left_arm_data, 1); + wait_us(20); } - + + /* //青ゾーン if(zone == BLUE) { GREEN_LED = 1; @@ -248,7 +262,7 @@ break; //回収 - case 1: + case 1: kaisyu(arm_enc.getPulses(), 2); servo_data[0] = 0x03; i2c.write(0x30, servo_data, 1); @@ -269,8 +283,8 @@ //左移動 case 3: counter.reset(); - left(10000); - if((x_pulse1 > 10000) && (x_pulse2 > 10000)) { + left(11500); + if((x_pulse1 > 11500) || (x_pulse2 > 11500)) { phase = 4; } break; @@ -288,8 +302,8 @@ //右旋回(180°) case 5: counter.reset(); - turn_right(518); - if(x_pulse2 > 518) { + turn_right(975); + if(sum_pulse > 975) { phase = 6; } break; @@ -304,14 +318,24 @@ } break; - //ちょっくら右移動 + //壁に当たるまで右移動 case 7: - counter.reset(); - right(-2000); - + counter.reset(); + if(right_limit == 3) { phase = 8; } + else if(right_limit != 3) { + true_migimae_data[0] = 0x40; + true_migiusiro_data[0] = 0xBF; + true_hidarimae_data[0] = 0xBF; + true_hidariusiro_data[0] = 0x40; + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + wait_us(20); + } break; //1秒停止 @@ -343,8 +367,8 @@ //前進 case 11: counter.reset(); - front(3500); - if((y_pulse1 > 3500) && (y_pulse2 > 3500)) { + front(5000); + if((y_pulse1 > 5000) || (y_pulse2 > 5000)) { phase = 12; } break; @@ -359,13 +383,24 @@ } break; - //右移動 + //壁に当たるまで右移動 case 13: counter.reset(); - right(-4000); + if(right_limit == 3) { phase = 14; } + else if(right_limit != 3) { + true_migimae_data[0] = 0x40; + true_migiusiro_data[0] = 0xBF; + true_hidarimae_data[0] = 0xBF; + true_hidariusiro_data[0] = 0x40; + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + wait_us(20); + } break; //1秒停止 @@ -373,22 +408,31 @@ stop(); counter.start(); if(counter.read() > 1.0f) { + phase = 15; + wheel_reset(); + } + break; + + //壁に当たるまで後進 + case 15: + counter.reset(); + + if(back_limit == 3) { phase = 16; - wheel_reset(); + } + else if(back_limit != 3){ + true_migimae_data[0] = 0x50; + true_migiusiro_data[0] = 0x50; + true_hidarimae_data[0] = 0x50; + true_hidariusiro_data[0] = 0x50; + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + wait_us(20); } break; - /* - //後進 - case 15: - counter.reset(); - back(-1000); - - if(back_limit == 3) { - phase = 16; - } - */ - //シーツ装填 case 16: if(start_switch == 1) { @@ -403,7 +447,7 @@ case 17: counter.reset(); front(22000); - if((y_pulse1 > 22000) && (y_pulse2 > 22000)) { + if((y_pulse1 > 22000) || (y_pulse2 > 22000)) { phase = 18; } break; @@ -417,15 +461,15 @@ wheel_reset(); } break; - - //掛けるところまで左移動 + + //ちょっと左移動 case 19: counter.reset(); - left(10000); - if((x_pulse1 > 10000) && (x_pulse2 > 10000)) { + left(500); + if((x_pulse1 > 800) || (x_pulse2 > 800)) { phase = 20; } - break; + break; //1秒停止 case 20: @@ -436,16 +480,16 @@ wheel_reset(); } break; - - //妨害防止の右旋回 + + //90°右旋回 case 21: counter.reset(); - turn_right(280); - if(x_pulse2 > 280) { + turn_right(465); + if(sum_pulse > 465) { phase = 22; } break; - + //1秒停止 case 22: stop(); @@ -455,36 +499,112 @@ wheel_reset(); } break; - + //カウンターリセット case 23: counter.reset(); counter.start(); phase = 24; + break; + + //壁に当たるまで前進 + case 24: + if(front_limit == 3) { + counter.reset(); + phase = 25; + } + else if(front_limit != 3){ + true_migimae_data[0] = 0xC0; + true_migiusiro_data[0] = 0xC0; + true_hidarimae_data[0] = 0xC0; + true_hidariusiro_data[0] = 0xC0; + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + wait_us(20); + } + break; + + //1秒停止 + case 25: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 26; + wheel_reset(); + } + break; + + //掛けるところまで後進 + case 26: + counter.reset(); + back(-10000); + if((y_pulse1*-1 > 10000) || (y_pulse2*-1 > 10000)) { + phase = 27; + counter.start(); + } + break; + + //1秒停止 + case 27: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 28; + wheel_reset(); + } + break; + + //妨害防止の右旋回 + case 28: + counter.reset(); + turn_right(30); + if(sum_pulse > 30) { + phase = 29; + } + break; + + //1秒停止 + case 29: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 30; + wheel_reset(); + } + break; + + //カウンターリセット + case 30: + counter.reset(); + counter.start(); + phase = 31; + break; //アームアップ - case 24: + case 31: stop(); - + //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止) if(counter.read() < 3.0f) { right_arm_data[0] = 0xFF; left_arm_data[0] = 0xFF; i2c.write(0x22, right_arm_data, 1); i2c.write(0x24, left_arm_data, 1); wait_us(20); - } else { - arm_up(25); + arm_up(32); } break; //カウンターリセット - case 25: + case 32: counter.reset(); - phase = 26; + phase = 33; + break; //シーツを掛ける - case 26: + case 33: counter.start(); //1秒間ファン送風 @@ -495,7 +615,7 @@ servo_data[0] = 0x04; i2c.write(0x30, servo_data, 1); } - //1~3秒の間でサーボを話す + //1~3秒の間でサーボを開放 else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) { fan_data[0] = 0xFF; i2c.write(0x26, fan_data, 1); @@ -510,19 +630,16 @@ i2c.write(0x28, fan_data, 1); servo_data[0] = 0x04; i2c.write(0x30, servo_data, 1); - phase = 27; + phase = 34; } break; //終了っ!(守衛さん風) - case 27: - //駆動系統OFF - emergency = 0; - break; - + case 34: default: //駆動系統OFF emergency = 0; + all_stop(); break; } } @@ -531,7 +648,407 @@ else if(zone == RED) { GREEN_LED = 0; RED_LED = 1; + + switch(phase) { + + //スタート位置へセット + case 0: + //リミットが洗濯物台に触れているか + if(right_limit == 3) { + USR_LED1 = 1; + //スタートスイッチが押されたか + if(start_switch == 1) { + wheel_reset(); + phase = 1; + } + } else { + USR_LED1 = 0; + } + break; + + //回収 + case 1: + kaisyu(arm_enc.getPulses(), 2); + servo_data[0] = 0x03; + i2c.write(0x30, servo_data, 1); + break; + + //1秒停止 + case 2: + stop(); + servo_data[0] = 0x04; + i2c.write(0x30, servo_data, 1); + counter.start(); + if(counter.read() > 1.0f) { + phase = 3; + wheel_reset(); + } + break; + + //左移動 + case 3: + counter.reset(); + left(11500); + if((x_pulse1 > 11500) || (x_pulse2 > 11500)) { + phase = 4; + } + break; + + //1秒停止 + case 4: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 5; + wheel_reset(); + } + break; + + //右旋回(180°) + case 5: + counter.reset(); + turn_right(975); + if(sum_pulse > 975) { + phase = 6; + } + break; + + //1秒停止 + case 6: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 7; + wheel_reset(); + } + break; + + //壁に当たるまで右移動 + case 7: + counter.reset(); + + if(right_limit == 3) { + phase = 8; + } + else if(right_limit != 3) { + true_migimae_data[0] = 0x40; + true_migiusiro_data[0] = 0xBF; + true_hidarimae_data[0] = 0xBF; + true_hidariusiro_data[0] = 0x40; + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + wait_us(20); + } + break; + + //1秒停止 + case 8: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 9; + wheel_reset(); + } + break; + + //排出 + case 9: + counter.reset(); + tyokudo(arm_enc.getPulses(), 10); + break; + + //1秒停止 + case 10: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 11; + wheel_reset(); + } + break; + + //後進 + case 11: + counter.reset(); + back(-5000); + if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) { + phase = 12; + } + break; + + //1秒停止 + case 12: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 13; + wheel_reset(); + } + break; + + //壁に当たるまで右移動 + case 13: + counter.reset(); + + if(right_limit == 3) { + phase = 14; + } + else if(right_limit != 3) { + true_migimae_data[0] = 0x40; + true_migiusiro_data[0] = 0xBF; + true_hidarimae_data[0] = 0xBF; + true_hidariusiro_data[0] = 0x40; + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + wait_us(20); + } + break; + + //1秒停止 + case 14: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 15; + wheel_reset(); + } + break; + + //壁に当たるまで前進 + case 15: + counter.reset(); + + if(front_limit == 3) { + phase = 16; + } + else if(front_limit != 3){ + true_migimae_data[0] = 0xC0; + true_migiusiro_data[0] = 0xC0; + true_hidarimae_data[0] = 0xC0; + true_hidariusiro_data[0] = 0xC0; + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + wait_us(20); + } + break; + + //シーツ装填 + case 16: + if(start_switch == 1) { + wheel_reset(); + phase = 17; + } else { + stop(); + } + break; + + //竿のラインまで後進 + case 17: + counter.reset(); + back(-22000); + if((y_pulse1*-1 > 22000) || (y_pulse2*-1 > 22000)) { + phase = 18; + } + break; + + //1秒停止 + case 18: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 19; + wheel_reset(); + } + break; + + //ちょっと左移動 + case 19: + counter.reset(); + left(500); + if((x_pulse1 > 500) || (x_pulse2 > 500)) { + phase = 20; + } + break; + + //1秒停止 + case 20: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 21; + wheel_reset(); + } + break; + + //90°左旋回 + case 21: + counter.reset(); + turn_left(465); + if(sum_pulse > 465) { + phase = 22; + } + break; + + //1秒停止 + case 22: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 23; + wheel_reset(); + } + break; + + //カウンターリセット + case 23: + counter.reset(); + counter.start(); + phase = 24; + break; + + //壁に当たるまで後進 + case 24: + if(back_limit == 3) { + counter.reset(); + phase = 25; + } + else if(back_limit != 3){ + true_migimae_data[0] = 0x50; + true_migiusiro_data[0] = 0x50; + true_hidarimae_data[0] = 0x50; + true_hidariusiro_data[0] = 0x50; + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + wait_us(20); + } + break; + + //1秒停止 + case 25: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 26; + wheel_reset(); + } + break; + + //掛けるところまで前進 + case 26: + counter.reset(); + front(10000); + if((y_pulse1 > 10000) || (y_pulse2 > 10000)) { + phase = 27; + counter.start(); + } + break; + + //1秒停止 + case 27: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 28; + wheel_reset(); + } + break; + + //妨害防止の左旋回 + case 28: + counter.reset(); + turn_left(30); + if(sum_pulse > 30) { + phase = 29; + } + break; + + //1秒停止 + case 29: + stop(); + counter.start(); + if(counter.read() > 1.0f) { + phase = 30; + wheel_reset(); + } + break; + + //カウンターリセット + case 30: + counter.reset(); + counter.start(); + phase = 31; + break; + + //アームアップ + case 31: + stop(); + //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止) + if(counter.read() < 3.0f) { + right_arm_data[0] = 0xFF; + left_arm_data[0] = 0xFF; + i2c.write(0x22, right_arm_data, 1); + i2c.write(0x24, left_arm_data, 1); + wait_us(20); + } else { + arm_up(32); + } + break; + + //カウンターリセット + case 32: + counter.reset(); + phase = 33; + break; + + //シーツを掛ける + case 33: + counter.start(); + + //1秒間ファン送風 + if(counter.read() <= 1.0f) { + fan_data[0] = 0xFF; + i2c.write(0x26, fan_data, 1); + i2c.write(0x28, fan_data, 1); + servo_data[0] = 0x04; + i2c.write(0x30, servo_data, 1); + } + //1~3秒の間でサーボを開放 + else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) { + fan_data[0] = 0xFF; + i2c.write(0x26, fan_data, 1); + i2c.write(0x28, fan_data, 1); + servo_data[0] = 0x03; + i2c.write(0x30, servo_data, 1); + } + //3秒過ぎたら終わり + else if(counter.read() > 3.0f) { + fan_data[0] = 0x80; + i2c.write(0x26, fan_data, 1); + i2c.write(0x28, fan_data, 1); + servo_data[0] = 0x04; + i2c.write(0x30, servo_data, 1); + phase = 34; + } + break; + + //終了っ!(守衛さん風) + case 34: + default: + //駆動系統OFF + emergency = 0; + all_stop(); + break; + } } + */ } } @@ -550,7 +1067,7 @@ pic.format(8, Serial::None, 1); pic.attach(get, Serial::RxIrq); - x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; + x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; sum_pulse = 0; migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80; true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80; fan_data[0] = 0x80; @@ -591,18 +1108,18 @@ x_pulse2 = wheel_x2.getPulses(); y_pulse1 = wheel_y1.getPulses(); y_pulse2 = wheel_y2.getPulses(); + sum_pulse = (abs(x_pulse1) + abs(x_pulse2) + abs(y_pulse1) + abs(y_pulse2)) / 4; } void print_pulses(void) { - pc.printf("phase: %d\n\r", phase); - //pc.printf("r: %d, l: %d, %d\n\r", right_arm_upper_limit, left_arm_upper_limit, phase); + pc.printf("X1: %d, X2: %d, Y1: %d, Y2: %d, sum: %d\n\r", abs(x_pulse1), x_pulse2, abs(y_pulse1), y_pulse2, sum_pulse); + //pc.printf("f: %d, b: %d, r: %d, phase: %d\n\r", front_limit, back_limit, right_limit, phase); //pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]); //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data); //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase); //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); //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); - } void get_emergency(void) { @@ -1070,6 +1587,33 @@ wait_us(20); } +void all_stop(void) { + + true_migimae_data[0] = 0x80; + true_migiusiro_data[0] = 0x80; + true_hidarimae_data[0] = 0x80; + true_hidariusiro_data[0] = 0x80; + arm_motor[0] = 0x80; + drop_motor[0] = 0x80; + right_arm_data[0] = 0x80; + left_arm_data[0] = 0x80; + fan_data[0] = 0x80; + servo_data[0] = 0x04; + + i2c.write(0x10, true_migimae_data, 1, false); + i2c.write(0x12, true_migiusiro_data, 1, false); + i2c.write(0x14, true_hidarimae_data, 1, false); + i2c.write(0x16, true_hidariusiro_data, 1, false); + i2c.write(0x18, arm_motor, 1); + i2c.write(0x20, drop_motor, 1); + i2c.write(0x22, right_arm_data, 1); + i2c.write(0x24, left_arm_data, 1); + i2c.write(0x26, fan_data, 1); + i2c.write(0x28, fan_data, 1); + i2c.write(0x30, servo_data, 1); + wait_us(20); +} + void front_PID(int target) { //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) @@ -1081,29 +1625,11 @@ //制御量の最小、最大 //正転(目標に達してない) if((y_pulse1 < target) && (y_pulse2 < target)) { - front_migimae.setOutputLimits(0x84, 0xF7); - front_migiusiro.setOutputLimits(0x84, 0xF7); - //front_migimae.setOutputLimits(0x84, 0xFF); - //front_migiusiro.setOutputLimits(0x84, 0xFF); + front_migimae.setOutputLimits(0x84, 0xF5); + front_migiusiro.setOutputLimits(0x84, 0xF5); front_hidarimae.setOutputLimits(0x84, 0xFF); front_hidariusiro.setOutputLimits(0x84, 0xFF); } - /* - //左側が前に出ちゃった♥(右側だけ回して左側は停止) - else if((y_pulse1 + wheel_difference) < y_pulse2) { - front_migimae.setOutputLimits(0x84, 0xFF); - front_migiusiro.setOutputLimits(0x84, 0xFF); - front_hidarimae.setOutputLimits(0x7C, 0x83); - front_hidariusiro.setOutputLimits(0x7C, 0x83); - } - //右側が前に出ちゃった♥(左側だけ回して右側は停止) - else if(y_pulse1 > (y_pulse2 + wheel_difference)) { - front_migimae.setOutputLimits(0x7C, 0x83); - front_migiusiro.setOutputLimits(0x7C, 0x83); - front_hidarimae.setOutputLimits(0x84, 0xFF); - front_hidariusiro.setOutputLimits(0x84, 0xFF); - } - */ //停止(目標より行き過ぎ) else if((y_pulse1 > target) && (y_pulse2 > target)) { front_migimae.setOutputLimits(0x7C, 0x83); @@ -1144,22 +1670,6 @@ true_hidarimae_data[0] = hidarimae_data[0]; true_hidariusiro_data[0] = hidariusiro_data[0]; } - /* - //左側が前に出ちゃった♥(右側だけ回して左側は停止) - else if((y_pulse1 + wheel_difference) < y_pulse2) { - true_migimae_data[0] = migimae_data[0]; - true_migiusiro_data[0] = migiusiro_data[0]; - true_hidarimae_data[0] = 0x80; - true_hidariusiro_data[0] = 0x80; - } - //右側が前に出ちゃった♥(左側だけ回して右側は停止) - else if(y_pulse1 > (y_pulse2 + wheel_difference)) { - true_migimae_data[0] = 0x80; - true_migiusiro_data[0] = 0x80; - true_hidarimae_data[0] = hidarimae_data[0]; - true_hidariusiro_data[0] = hidariusiro_data[0]; - } - */ //停止(目標より行き過ぎ) else if((y_pulse1 > target) && (y_pulse2 > target)) { true_migimae_data[0] = 0x80; @@ -1182,27 +1692,11 @@ if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { back_migimae.setOutputLimits(0x00, 0x7B); back_migiusiro.setOutputLimits(0x00, 0x7B); - //back_hidarimae.setOutputLimits(0x00, 0x73); - //back_hidariusiro.setOutputLimits(0x00, 0x73); - back_hidarimae.setOutputLimits(0x00, 0x7B); - back_hidariusiro.setOutputLimits(0x00, 0x7B); + back_hidarimae.setOutputLimits(0x00, 0x70); + back_hidariusiro.setOutputLimits(0x00, 0x70); + //back_hidarimae.setOutputLimits(0x00, 0x7B); + //back_hidariusiro.setOutputLimits(0x00, 0x7B); } - /* - //左側が後に出ちゃった♥(右側だけ回して左側は停止) - else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){ - back_migimae.setOutputLimits(0x00, 0x7B); - back_migiusiro.setOutputLimits(0x00, 0x7B); - back_hidarimae.setOutputLimits(0x7C, 0x83); - back_hidariusiro.setOutputLimits(0x7C, 0x83); - } - //右側が後に出ちゃった♥(左側だけ回して右側は停止) - else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){ - back_migimae.setOutputLimits(0x7C, 0x83); - back_migiusiro.setOutputLimits(0x7C, 0x83); - back_hidarimae.setOutputLimits(0x00, 0x7B); - back_hidariusiro.setOutputLimits(0x00, 0x7B); - } - */ //停止(目標より行き過ぎ) else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { back_migimae.setOutputLimits(0x7C, 0x83); @@ -1243,22 +1737,6 @@ true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; } - /* - //左側が後に出ちゃった♥(右側だけ回して左側は停止) - else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){ - true_migimae_data[0] = 0x7B - migimae_data[0]; - true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; - true_hidarimae_data[0] = 0x80; - true_hidariusiro_data[0] = 0x80; - } - //右側が後に出ちゃった♥(左側だけ回して右側は停止) - else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){ - true_migimae_data[0] = 0x80; - true_migiusiro_data[0] = 0x80; - true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; - true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; - } - */ //停止(目標より行き過ぎ) else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { true_migimae_data[0] = 0x80; @@ -1279,30 +1757,14 @@ //制御量の最小、最大 //右進(目標まで達していない) if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { - // right_migimae.setOutputLimits(0x00, 0x6C); - right_migimae.setOutputLimits(0x7A, 0x7B); + right_migimae.setOutputLimits(0x6A, 0x6C); + //right_migimae.setOutputLimits(0x7A, 0x7B); right_migiusiro.setOutputLimits(0xFE, 0xFF); - //right_hidarimae.setOutputLimits(0x84, 0xF0); - right_hidarimae.setOutputLimits(0xFE, 0xFF); + right_hidarimae.setOutputLimits(0xEF, 0xF0); + //right_hidarimae.setOutputLimits(0xFE, 0xFF); right_hidariusiro.setOutputLimits(0x7A, 0x7B); } - /* - //前側が右に出ちゃった♥(後側だけ回して前側は停止) - else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){ - right_migimae.setOutputLimits(0x7C, 0x83); - right_migiusiro.setOutputLimits(0x00, 0x7B); - right_hidarimae.setOutputLimits(0x7C, 0x83); - right_hidariusiro.setOutputLimits(0x84, 0xFF); - } - //後側が右に出ちゃった♥(前側だけ回して後側は停止) - else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){ - right_migimae.setOutputLimits(0x84, 0xFF); - right_migiusiro.setOutputLimits(0x7C, 0x83); - right_hidarimae.setOutputLimits(0x00, 0x7B); - right_hidariusiro.setOutputLimits(0x7C, 0x83); - } - */ //停止(目標より行き過ぎ) else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { right_migimae.setOutputLimits(0x7C, 0x83); @@ -1343,22 +1805,6 @@ true_hidarimae_data[0] = hidarimae_data[0]; true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; } - /* - //前側が右に出ちゃった♥(後側だけ回して前側は停止) - else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){ - true_migimae_data[0] = 0x80; - true_migiusiro_data[0] = migiusiro_data[0]; - true_hidarimae_data[0] = 0x80; - true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; - } - //後側が右に出ちゃった♥(前側だけ回して後側は停止) - else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){ - true_migimae_data[0] = 0x7B - migimae_data[0]; - true_migiusiro_data[0] = 0x80; - true_hidarimae_data[0] = hidarimae_data[0]; - true_hidariusiro_data[0] = 0x80; - } - */ //左進(目標より行き過ぎ) else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { true_migimae_data[0] = 0x80; @@ -1378,31 +1824,16 @@ //制御量の最小、最大 //左進(目標まで達していない) - if((x_pulse1 < target) || (x_pulse2 < target)) { + if((x_pulse1 < target) && (x_pulse2 < target)) { left_migimae.setOutputLimits(0xEC, 0xED); - //left_migimae.setOutputLimits(0x84, 0xFF); - left_migiusiro.setOutputLimits(0x7A, 0x7B); + //left_migiusiro.setOutputLimits(0x7A, 0x7B); + left_migiusiro.setOutputLimits(0x77, 0x78); left_hidarimae.setOutputLimits(0x7A, 0x7B); + //left_hidarimae.setOutputLimits(0x77, 0x78); left_hidariusiro.setOutputLimits(0xFE, 0xFF); } - /* - //前側が左に出ちゃった♥(後側だけ回して前側は停止) - else if(x_pulse1 > (x_pulse2 + wheel_difference)){ - left_migimae.setOutputLimits(0x7C, 0x83); - left_migiusiro.setOutputLimits(0x7C, 0x83); - left_hidarimae.setOutputLimits(0x10, 0x7B); - left_hidariusiro.setOutputLimits(0x94, 0xFF); - } - //後側が左に出ちゃった♥(前側だけ回して後側は停止) - else if((x_pulse1 + wheel_difference) < x_pulse2){ - left_migimae.setOutputLimits(0x94, 0xFF); - left_migiusiro.setOutputLimits(0x10, 0x7B); - left_hidarimae.setOutputLimits(0x7C, 0x83); - left_hidariusiro.setOutputLimits(0x7C, 0x83); - } - */ //停止(目標より行き過ぎ) - else if((x_pulse1 > target) || (x_pulse2 > target)) { + else if((x_pulse1 > target) && (x_pulse2 > target)) { left_migimae.setOutputLimits(0x7C, 0x83); left_migiusiro.setOutputLimits(0x7C, 0x83); left_hidarimae.setOutputLimits(0x7C, 0x83); @@ -1435,30 +1866,14 @@ //制御量をPWM値に変換 //左進(目標まで達していない) - if((x_pulse1 < target) || (x_pulse2 < target)) { + if((x_pulse1 < target) && (x_pulse2 < target)) { true_migimae_data[0] = migimae_data[0]; true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; true_hidariusiro_data[0] = hidariusiro_data[0]; } - /* - //前側が左に出ちゃった♥(後側だけ回して前側は停止) - else if(x_pulse1 > (x_pulse2 + wheel_difference)){ - true_migimae_data[0] = 0x80; - true_migiusiro_data[0] = 0x80; - true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; - true_hidariusiro_data[0] = hidariusiro_data[0]; - } - //後側が左に出ちゃった♥(前側だけ回して後側は停止) - else if((x_pulse1 + wheel_difference) < x_pulse2){ - true_migimae_data[0] = migimae_data[0]; - true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; - true_hidarimae_data[0] = 0x80; - true_hidariusiro_data[0] = 0x80; - } - */ //停止(目標より行き過ぎ) - else if((x_pulse1 > target) || (x_pulse2 > target)) { + else if((x_pulse1 > target) && (x_pulse2 > target)) { true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; @@ -1476,14 +1891,14 @@ //制御量の最小、最大 //右旋回(目標に達してない) - if(x_pulse2 < target) { + if(sum_pulse < target) { turn_right_migimae.setOutputLimits(0x10, 0x7B); turn_right_migiusiro.setOutputLimits(0x10, 0x7B); turn_right_hidarimae.setOutputLimits(0x94, 0xFF); turn_right_hidariusiro.setOutputLimits(0x94, 0xFF); } //停止(目標より行き過ぎ) - else if(x_pulse2 > target) { + else if(sum_pulse > target) { turn_right_migimae.setOutputLimits(0x7C, 0x83); turn_right_migiusiro.setOutputLimits(0x7C, 0x83); turn_right_hidarimae.setOutputLimits(0x7C, 0x83); @@ -1503,10 +1918,10 @@ turn_right_hidariusiro.setSetPoint(target); //センサ出力 - turn_right_migimae.setProcessValue(x_pulse2); - turn_right_migiusiro.setProcessValue(x_pulse2); - turn_right_hidarimae.setProcessValue(x_pulse2); - turn_right_hidariusiro.setProcessValue(x_pulse2); + turn_right_migimae.setProcessValue(sum_pulse); + turn_right_migiusiro.setProcessValue(sum_pulse); + turn_right_hidarimae.setProcessValue(sum_pulse); + turn_right_hidariusiro.setProcessValue(sum_pulse); //制御量(計算結果) migimae_data[0] = turn_right_migimae.compute(); @@ -1516,14 +1931,14 @@ //制御量をPWM値に変換 //右旋回(目標に達してない) - if(x_pulse2 < target) { + if(sum_pulse < target) { true_migimae_data[0] = 0x7B - migimae_data[0]; true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; true_hidarimae_data[0] = hidarimae_data[0]; true_hidariusiro_data[0] = hidariusiro_data[0]; } //停止(目標より行き過ぎ) - else if(x_pulse2 > target) { + else if(sum_pulse > target) { true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; @@ -1541,14 +1956,14 @@ //制御量の最小、最大 //左旋回(目標に達してない) - if(x_pulse1 < target) { + if(sum_pulse < target) { turn_left_migimae.setOutputLimits(0x94, 0xFF); turn_left_migiusiro.setOutputLimits(0x94, 0xFF); turn_left_hidarimae.setOutputLimits(0x10, 0x7B); turn_left_hidariusiro.setOutputLimits(0x10, 0x7B); } //停止(目標より行き過ぎ) - else if(x_pulse1 > target) { + else if(sum_pulse > target) { turn_left_migimae.setOutputLimits(0x7C, 0x83); turn_left_migiusiro.setOutputLimits(0x7C, 0x83); turn_left_hidarimae.setOutputLimits(0x7C, 0x83); @@ -1568,10 +1983,10 @@ turn_left_hidariusiro.setSetPoint(target); //センサ出力 - turn_left_migimae.setProcessValue(x_pulse1); - turn_left_migiusiro.setProcessValue(x_pulse1); - turn_left_hidarimae.setProcessValue(x_pulse1); - turn_left_hidariusiro.setProcessValue(x_pulse1); + turn_left_migimae.setProcessValue(sum_pulse); + turn_left_migiusiro.setProcessValue(sum_pulse); + turn_left_hidarimae.setProcessValue(sum_pulse); + turn_left_hidariusiro.setProcessValue(sum_pulse); //制御量(計算結果) migimae_data[0] = turn_left_migimae.compute(); @@ -1581,14 +1996,14 @@ //制御量をPWM値に変換 //左旋回(目標に達してない) - if(x_pulse1 < target) { + if(sum_pulse < target) { true_migimae_data[0] = migimae_data[0]; true_migiusiro_data[0] = migiusiro_data[0]; true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; } //左旋回(目標より行き過ぎ) - else if(x_pulse1 > target) { + else if(sum_pulse > target) { true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80;