2019NHK_teamA
/
2019_A_ver7-2
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.
Revision 22:5682246f9409, committed 2019-09-25
- Comitter:
- yuron
- Date:
- Wed Sep 25 02:07:26 2019 +0000
- Parent:
- 21:89db2a19e52e
- Commit message:
- aaaaaaa;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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;