2019NHK_teamA
/
2019_A_ver7-7
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 22:5682246f9409
- Parent:
- 21:89db2a19e52e
- Child:
- 23:1e4d7540715f
diff -r 89db2a19e52e -r 5682246f9409 main.cpp
--- 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;