2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
Diff: main.cpp
- Revision:
- 18:851f783ec516
- Parent:
- 17:de3bc1999ae7
- Child:
- 19:f17d2e585973
--- a/main.cpp Tue Sep 03 05:47:40 2019 +0000
+++ b/main.cpp Sat Sep 07 13:17:32 2019 +0000
@@ -50,16 +50,16 @@
PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
//右旋回
-PID turn_right_migimae(18000000.0, 0.0, 0.0, 0.001);
-PID turn_right_migiusiro(18000000.0, 0.0, 0.0, 0.001);
-PID turn_right_hidarimae(18000000.0, 0.0, 0.0, 0.001);
-PID turn_right_hidariusiro(18000000.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_left_migimae(18000000.0, 0.0, 0.0, 0.001);
-PID turn_left_migiusiro(18000000.0, 0.0, 0.0, 0.001);
-PID turn_left_hidarimae(18000000.0, 0.0, 0.0, 0.001);
-PID turn_left_hidariusiro(18000000.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);
//MDとの通信ポート
I2C i2c(PB_9, PB_8); //SDA, SCL
@@ -70,6 +70,9 @@
//特小モジュールとの通信ポート
Serial pic(A0, A1);
+//リミットスイッチ基板との通信ポート
+Serial limit_serial(PC_12, PD_2);
+
//12V停止信号ピン
DigitalOut emergency(D11);
@@ -90,8 +93,6 @@
//QEI wheel1(D2, D3, NC, 624);
//QEI wheel2(D5, D4, NC, 624);
-Ticker look_switch;
-
Timer counter;
//エンコーダ値格納変数
@@ -112,20 +113,29 @@
char baff;
int flug = 0;
+int limit_data = 0;
+
+unsigned int start_switch_counter = 0;
+
+bool front_limit = 0;
+bool right_limit = 0;
+bool back_limit = 0;
+
//関数のプロトタイプ宣言
-void incriment(void);
void init(void);
void init_send(void);
void get(void);
void get_pulses(void);
void print_pulses(void);
void get_emergency(void);
+void read_limit(void);
void front(int target);
void back(int target);
void right(int target);
void left(int target);
void turn_right(int target);
void turn_left(int target);
+void stop(void);
void front_PID(int target);
void back_PID(int target);
void right_PID(int target);
@@ -138,64 +148,174 @@
init();
init_send();
-
- //look_switch.attach_us(&incriment, 100000.0);
-
- /*
- while(1) {
- if(!start_switch)
- break;
- }
- */
+ zone = BLUE;
+ phase = 1;
while(1) {
-
+
get_pulses();
print_pulses();
get_emergency();
-
- if(start_switch == 0) {
- USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1;
- } else {
- USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 0;
+ read_limit();
+
+ if(zone == BLUE) {
+ switch(phase) {
+ case 0:
+ if(!start_switch) {
+ phase = 1;
+ }
+ case 1:
+ left(12000);
+ if((x_pulse1 > 12000) || (x_pulse2 > 12000)) {
+ phase = 2;
+ }
+ break;
+ case 2:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 3;
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
+ }
+ break;
+ case 3:
+ counter.reset();
+ turn_right(535);
+ if(x_pulse2 > 535) {
+ phase = 4;
+ }
+ break;
+ case 4:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ //本当は5だけど今はリミットスイッチ無の為phase7まで飛ばす
+ //phase = 5;
+ phase = 7;
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
+ }
+ break;
+ case 5:
+ counter.reset();
+ right(-500);
+ if((x_pulse1*-1 > 500) || (x_pulse2*-1 > 500)) {
+ phase = 6;
+ }
+ //if(!right_limit) {
+ //phase = 6;
+ //}
+ break;
+ case 6:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 7;
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
+ }
+ break;
+ case 7:
+ counter.reset();
+ front(3000);
+ if((y_pulse1 > 3000) || (y_pulse2 > 3000)) {
+ phase = 8;
+ }
+ break;
+ case 8:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 9;
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
+ }
+ break;
+ case 9:
+ counter.reset();
+ right(-3000);
+ if((x_pulse1*-1 > 3000) || (x_pulse2*-1 > 3000)) {
+ phase = 10;
+ }
+ //if(!right_limit) {
+ //phase = 10;
+ //}
+ break;
+ case 10:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 11;
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
+ }
+ break;
+ case 11:
+ counter.reset();
+ front(21500);
+ if((y_pulse1 > 21500) || (y_pulse2 > 21500)) {
+ phase = 12;
+ }
+ break;
+ case 12:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 13;
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
+ }
+ break;
+ case 13:
+ left(8000);
+ if((x_pulse1 > 8000) || (x_pulse2 > 8000)) {
+ phase = 14;
+ }
+ break;
+ case 14:
+ stop();
+ default:
+ break;
+ }
}
-
- //front(5000);
- //back(-5000);
- //right(-5000);
- //left(5000);
- //turn_right(550);
- //turn_left(600);
-
+
+ /*
if(counter.read() < 5.00f) {
counter.start();
- front(1000);
+ front(3000);
}
else if(counter.read() >= 5.00f && counter.read() < 10.00f) {
- right(-1000);
+ right(-3000);
}
else if(counter.read() >= 10.00f && counter.read() < 15.00f) {
- back(-1000);
+ back(-3000);
}
else if(counter.read() >= 15.00f && counter.read() < 20.00f) {
- left(1000);
+ left(3000);
}
else if(counter.read() >= 20.00f && counter.read() < 25.00f) {
- turn_right(550);
+ turn_right(535);
}
else if(counter.read() >= 25.00f && counter.read() < 30.00f) {
- turn_left(600);
+ turn_left(674);
}
else if(counter.read() >= 30.00f) {
counter.reset();
}
- }
-}
-
-void incriment(void) {
-
- if(start_switch == 0) {
- phase++;
+ */
}
}
@@ -208,6 +328,8 @@
pc.baud(460800);
//pc.baud(9600);
+ limit_serial.baud(115200);
+
start_switch.mode(PullUp);
//非常停止関連
@@ -251,8 +373,7 @@
void print_pulses(void) {
- //pc.printf("%f %d\n\r", counter.read(), phase);
- //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2);
+ //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\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]);
}
@@ -268,6 +389,33 @@
}
}
+void read_limit(void) {
+
+ limit_data = limit_serial.getc();
+
+ if((limit_data & 0b00000001) == 0x01) {
+ USR_LED1 = 1; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 0;
+ }
+ if((limit_data & 0b00000010) == 0x02) {
+ USR_LED1 = 0; USR_LED2 = 1; USR_LED3 = 0; USR_LED4 = 0;
+ }
+ if((limit_data & 0b00000011) == 0x03) {
+ USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 0; USR_LED4 = 0;
+ }
+ if((limit_data & 0b00000100) == 0x04) {
+ USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 1; USR_LED4 = 0;
+ }
+ if((limit_data & 0b00001000) == 0x08) {
+ USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 1;
+ }
+ if((limit_data & 0b00001100) == 0x0C) {
+ USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 1; USR_LED4 = 1;
+ }
+ if(limit_data == 0x00) {
+ USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 0;
+ }
+}
+
void front(int target) {
front_PID(target);
@@ -328,6 +476,20 @@
wait_us(20);
}
+void stop(void) {
+
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+
+ 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);
+}
+
void front_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
@@ -338,16 +500,19 @@
//制御量の最小、最大
//正転(目標に達してない)
- if((y_pulse1 < target) && (y_pulse2 < target)) {
+ 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_hidarimae.setOutputLimits(0x84, 0xFF);
front_hidariusiro.setOutputLimits(0x84, 0xFF);
}
+ /*
//左側が前に出ちゃった♥(右側だけ回して左側は停止)
else 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_hidarimae.setOutputLimits(0x7C, 0x83);
front_hidariusiro.setOutputLimits(0x7C, 0x83);
}
@@ -358,12 +523,17 @@
front_hidarimae.setOutputLimits(0x84, 0xFF);
front_hidariusiro.setOutputLimits(0x84, 0xFF);
}
- //逆転(目標より行き過ぎ)
- else if((y_pulse1 > target) && (y_pulse2 > target)) {
- front_migimae.setOutputLimits(0x00, 0x7B);
- front_migiusiro.setOutputLimits(0x00, 0x7B);
- front_hidarimae.setOutputLimits(0x00, 0x73);
- front_hidariusiro.setOutputLimits(0x00, 0x73);
+ */
+ //停止(目標より行き過ぎ)
+ else if((y_pulse1 > target) || (y_pulse2 > target)) {
+ front_migimae.setOutputLimits(0x7C, 0x83);
+ front_migiusiro.setOutputLimits(0x7C, 0x83);
+ front_hidarimae.setOutputLimits(0x7C, 0x83);
+ front_hidariusiro.setOutputLimits(0x7C, 0x83);
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
}
//よくわからんやつ
@@ -392,12 +562,13 @@
//制御量をPWM値に変換
//正転(目標に達してない)
- if((y_pulse1 < target) && (y_pulse2 < target)) {
+ if((y_pulse1 < target) || (y_pulse2 < target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
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] = migimae_data[0];
@@ -412,12 +583,13 @@
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] = 0x7B - migimae_data[0];
- true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
- true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
- true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
+ */
+ //停止(目標より行き過ぎ)
+ else if((y_pulse1 > target) || (y_pulse2 > target)) {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
}
}
@@ -431,12 +603,15 @@
//制御量の最小、最大
//逆転(目標に達してない)
- if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
+ 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, 0x73);
+ //back_hidariusiro.setOutputLimits(0x00, 0x73);
+ 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(0x00, 0x7B);
@@ -448,15 +623,20 @@
else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 < target*-1)) {
back_migimae.setOutputLimits(0x7C, 0x83);
back_migiusiro.setOutputLimits(0x7C, 0x83);
- back_hidarimae.setOutputLimits(0x00, 0x73);
- back_hidariusiro.setOutputLimits(0x00, 0x73);
+ 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(0x84, 0xF7);
- back_migiusiro.setOutputLimits(0x84, 0xF7);
- back_hidarimae.setOutputLimits(0x84, 0xFF);
- back_hidariusiro.setOutputLimits(0x84, 0xFF);
+ */
+ //停止(目標より行き過ぎ)
+ else if((y_pulse1*-1 > target*-1) || (y_pulse2*-1 > target*-1)) {
+ back_migimae.setOutputLimits(0x7C, 0x83);
+ back_migiusiro.setOutputLimits(0x7C, 0x83);
+ back_hidarimae.setOutputLimits(0x7C, 0x83);
+ back_hidariusiro.setOutputLimits(0x7C, 0x83);
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
}
//よくわからんやつ
@@ -485,12 +665,13 @@
//制御量をPWM値に変換
//逆転(目標に達してない)
- if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
+ if((y_pulse1*-1 < target*-1) || (y_pulse2*-1 < target*-1)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
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] = 0x7B - migimae_data[0];
@@ -505,12 +686,13 @@
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] = migimae_data[0];
- true_migiusiro_data[0] = migiusiro_data[0];
- true_hidarimae_data[0] = hidarimae_data[0];
- true_hidariusiro_data[0] = hidariusiro_data[0];
+ */
+ //停止(目標より行き過ぎ)
+ else if((y_pulse1*-1 > target*-1) || (y_pulse2*-1 > target*-1)) {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
}
}
@@ -524,13 +706,16 @@
//制御量の最小、最大
//右進(目標まで達していない)
- if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
- right_migimae.setOutputLimits(0x00, 0x6C);
+ if((x_pulse1*-1 < target*-1) || (x_pulse2*-1 < target*-1)) {
+ // right_migimae.setOutputLimits(0x00, 0x6C);
+ right_migimae.setOutputLimits(0x00, 0x7B);
right_migiusiro.setOutputLimits(0x84, 0xFF);
- right_hidarimae.setOutputLimits(0x84, 0xF0);
+ //right_hidarimae.setOutputLimits(0x84, 0xF0);
+ right_hidarimae.setOutputLimits(0x84, 0xFF);
right_hidariusiro.setOutputLimits(0x00, 0x7B);
}
+ /*
//前側が右に出ちゃった♥(後側だけ回して前側は停止)
else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 < target*-1)) {
right_migimae.setOutputLimits(0x7C, 0x83);
@@ -540,17 +725,22 @@
}
//後側が右に出ちゃった♥(前側だけ回して後側は停止)
else if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 > target*-1)) {
- right_migimae.setOutputLimits(0x84, 0xED);
+ right_migimae.setOutputLimits(0x84, 0xFF);
right_migiusiro.setOutputLimits(0x7C, 0x83);
- right_hidarimae.setOutputLimits(0x00, 0x69);
+ 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(0x84, 0xED);
- right_migiusiro.setOutputLimits(0x00, 0x7B);
- right_hidarimae.setOutputLimits(0x00, 0x69);
- right_hidariusiro.setOutputLimits(0x84, 0xFF);
+ */
+ //停止(目標より行き過ぎ)
+ else if((x_pulse1*-1 > target*-1) || (x_pulse2*-1 > target*-1)) {
+ right_migimae.setOutputLimits(0x7C, 0x83);
+ right_migiusiro.setOutputLimits(0x7C, 0x83);
+ right_hidarimae.setOutputLimits(0x7C, 0x83);
+ right_hidariusiro.setOutputLimits(0x7C, 0x83);
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
}
//よくわからんやつ
@@ -579,12 +769,13 @@
//制御量をPWM値に変換
//右進(目標まで達していない)
- if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
+ if((x_pulse1*-1 < target*-1) || (x_pulse2*-1 < target*-1)) {
true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
+ /*
//前側が右に出ちゃった♥(後側だけ回して前側は停止)
else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 < target*-1)) {
true_migimae_data[0] = 0x80;
@@ -599,12 +790,13 @@
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] = 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*-1 > target*-1) || (x_pulse2*-1 > target*-1)) {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
}
}
@@ -618,32 +810,38 @@
//制御量の最小、最大
//左進(目標まで達していない)
- if((x_pulse1 < target) && (x_pulse2 < target)) {
+ if((x_pulse1 < target) || (x_pulse2 < target)) {
left_migimae.setOutputLimits(0x84, 0xED);
left_migiusiro.setOutputLimits(0x00, 0x7B);
- left_hidarimae.setOutputLimits(0x00, 0x69);
+ left_hidarimae.setOutputLimits(0x00, 0x7B);
left_hidariusiro.setOutputLimits(0x84, 0xFF);
}
+ /*
//前側が左に出ちゃった♥(後側だけ回して前側は停止)
else if((x_pulse1 > target) && (x_pulse2 < target)) {
left_migimae.setOutputLimits(0x7C, 0x83);
left_migiusiro.setOutputLimits(0x7C, 0x83);
- left_hidarimae.setOutputLimits(0x00, 0x69);
- left_hidariusiro.setOutputLimits(0x84, 0xFF);
+ left_hidarimae.setOutputLimits(0x10, 0x7B);
+ left_hidariusiro.setOutputLimits(0x94, 0xFF);
}
//後側が左に出ちゃった♥(前側だけ回して後側は停止)
else if((x_pulse1 < target) && (x_pulse2 > target)) {
- left_migimae.setOutputLimits(0x84, 0xED);
- left_migiusiro.setOutputLimits(0x00, 0x7B);
+ 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)) {
- left_migimae.setOutputLimits(0x00, 0x6C);
- left_migiusiro.setOutputLimits(0x84, 0xFF);
- left_hidarimae.setOutputLimits(0x84, 0xF0);
- left_hidariusiro.setOutputLimits(0x00, 0x7B);
+ */
+ //停止(目標より行き過ぎ)
+ else if((x_pulse1 > target) || (x_pulse2 > target)) {
+ left_migimae.setOutputLimits(0x7C, 0x83);
+ left_migiusiro.setOutputLimits(0x7C, 0x83);
+ left_hidarimae.setOutputLimits(0x7C, 0x83);
+ left_hidariusiro.setOutputLimits(0x7C, 0x83);
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
}
//よくわからんやつ
@@ -672,12 +870,13 @@
//制御量を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 > target) && (x_pulse2 < target)) {
true_migimae_data[0] = 0x80;
@@ -692,12 +891,13 @@
true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
- //右進(目標より行き過ぎ)
- else if((x_pulse1 > target) && (x_pulse2 > target)) {
- true_migimae_data[0] = 0x7B - migimae_data[0];
- true_migiusiro_data[0] = migiusiro_data[0];
- true_hidarimae_data[0] = hidarimae_data[0];
- true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
+ */
+ //停止(目標より行き過ぎ)
+ else if((x_pulse1 > target) || (x_pulse2 > target)) {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
}
}
@@ -717,12 +917,16 @@
turn_right_hidarimae.setOutputLimits(0x94, 0xFF);
turn_right_hidariusiro.setOutputLimits(0x94, 0xFF);
}
- //左旋回(目標より行き過ぎ)
+ //停止(目標より行き過ぎ)
else if(x_pulse2 > target) {
- turn_right_migimae.setOutputLimits(0x94, 0xFF);
- turn_right_migiusiro.setOutputLimits(0x94, 0xFF);
- turn_right_hidarimae.setOutputLimits(0x10, 0x7B);
- turn_right_hidariusiro.setOutputLimits(0x10, 0x7B);
+ turn_right_migimae.setOutputLimits(0x7C, 0x83);
+ turn_right_migiusiro.setOutputLimits(0x7C, 0x83);
+ turn_right_hidarimae.setOutputLimits(0x7C, 0x83);
+ turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
}
//よくわからんやつ
@@ -757,12 +961,12 @@
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
- //左旋回(目標より行き過ぎ)
+ //停止(目標より行き過ぎ)
else if(x_pulse2 > 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];
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
}
}
@@ -775,19 +979,23 @@
turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
- //右旋回(目標に達してない)
+ //左旋回(目標に達してない)
if(x_pulse1 < 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) {
- turn_left_migimae.setOutputLimits(0x10, 0x7B);
- turn_left_migiusiro.setOutputLimits(0x10, 0x7B);
- turn_left_hidarimae.setOutputLimits(0x94, 0xFF);
- turn_left_hidariusiro.setOutputLimits(0x94, 0xFF);
+ turn_left_migimae.setOutputLimits(0x7C, 0x83);
+ turn_left_migiusiro.setOutputLimits(0x7C, 0x83);
+ turn_left_hidarimae.setOutputLimits(0x7C, 0x83);
+ turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
}
//よくわからんやつ
@@ -815,7 +1023,7 @@
hidariusiro_data[0] = turn_left_hidariusiro.compute();
//制御量をPWM値に変換
- //右旋回(目標に達してない)
+ //左旋回(目標に達してない)
if(x_pulse1 < target) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
@@ -824,10 +1032,10 @@
}
//左旋回(目標より行き過ぎ)
else if(x_pulse1 > 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];
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
}
}