2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
Diff: main.cpp
- Revision:
- 19:f17d2e585973
- Parent:
- 18:851f783ec516
- Child:
- 20:ac4954be1fe0
--- a/main.cpp Sat Sep 07 13:17:32 2019 +0000
+++ b/main.cpp Tue Sep 17 01:33:16 2019 +0000
@@ -4,27 +4,20 @@
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
/* Sensor: encorder*4 */
/* ------------------------------------------------------------------- */
-/* 遠隔非常停止対応 & 移動時のバグを改善と */
+/* ファンとサーボの動作を追加した */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
-//PIDGain of wheels
-#define Kp 4500000.0
-//#define Kp 10000000.0
-#define Ti 0.0
-#define Td 0.0
+//直進補正の為の前後・左右の回転差の許容値
+#define wheel_difference 100
#define RED 0
#define BLUE 1
-PID migimae(Kp, Ti, Td, 0.001);
-PID migiusiro(Kp, Ti, Td, 0.001);
-PID hidarimae(Kp, Ti, Td, 0.001);
-PID hidariusiro(Kp, Ti, Td, 0.001);
-
+//PID Gain of wheels(Kp, Ti, Td, control cycle)
//前進
PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001);
@@ -90,9 +83,9 @@
QEI wheel_x2(PB_14, PB_13, NC, 624);
QEI wheel_y1(PB_1 , PB_15, NC, 624);
QEI wheel_y2(PA_12, PA_11, NC, 624);
-//QEI wheel1(D2, D3, NC, 624);
-//QEI wheel2(D5, D4, NC, 624);
+QEI arm_enc(PB_5, PB_4 , NC, 624);
+//移動後n秒停止タイマー
Timer counter;
//エンコーダ値格納変数
@@ -100,26 +93,56 @@
//操作の段階変数
unsigned int phase = 0;
+int kaisyu_phase = 0;
+int tyokudo_phase = 0;
unsigned int start_zone = 1;
bool zone = RED;
-//MD送信データ変数
+//i2c送信データ変数
char init_send_data[1];
char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
+char arm_moter[1], drop_moter[1];
+char fan_data[1];
+char servo_data[1];
//非常停止関連変数
char RDATA;
char baff;
int flug = 0;
+//リミット基板からの受信データ
int limit_data = 0;
-
-unsigned int start_switch_counter = 0;
+int upper_limit_data = 0;
+int lower_limit_data = 0;
-bool front_limit = 0;
-bool right_limit = 0;
-bool back_limit = 0;
+//各辺のスイッチが押されたかのフラグ
+//前部が壁に当たっているか
+int front_limit = 0;
+//右部が壁にあたあっているか
+int right_limit = 0;
+//後部が壁に当たっているか
+int back_limit = 0;
+//右腕の上限
+bool right_arm_upper_limit = 0;
+//右腕の下限
+bool right_arm_lower_limit = 0;
+//左腕の上限
+bool left_arm_upper_limit = 0;
+//左腕の下限
+bool left_arm_lower_limit = 0;
+//吐き出し機構の下限
+bool tyokudo_usiro = 0;
+//吐き出し機構の上限
+bool tyokudo_mae = 0;
+//回収機構の上限
+bool kaisyu_mae = 0;
+//回収機構の下限
+bool kaisyu_usiro = 0;
+
+int masked_lower_front_limit_data = 0;
+int masked_lower_back_limit_data = 0;
+int masked_lower_right_limit_data = 0;
//関数のプロトタイプ宣言
void init(void);
@@ -129,6 +152,9 @@
void print_pulses(void);
void get_emergency(void);
void read_limit(void);
+void wheel_reset(void);
+void kaisyu(int pulse);
+void tyokudo(int pulse);
void front(int target);
void back(int target);
void right(int target);
@@ -148,185 +174,297 @@
init();
init_send();
+
+ //とりあえず(後で消してね)
zone = BLUE;
- phase = 1;
while(1) {
-
+
get_pulses();
print_pulses();
get_emergency();
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;
+ servo_data[0] = 0x03;
+ i2c.write(0x30, servo_data, 1);
+
+ //リミットが洗濯物台に触れているか
+ if(right_limit == 3) {
+ USR_LED1 = 1;
+ //スタートスイッチが押されたか
+ if(start_switch == 1) {
+ wheel_reset();
+ phase = 1;
+ }
+ } else {
+ USR_LED1 = 0;
}
break;
+
+ //回収
+ case 1:
+ //ここに回収動作が来るが今回は飛ばす
+ phase = 2;
+ break;
+
+ //1秒停止
case 2:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 3;
- wheel_x1.reset();
- wheel_x2.reset();
- wheel_y1.reset();
- wheel_y2.reset();
+ wheel_reset();
}
break;
+
+ //左移動
case 3:
counter.reset();
- turn_right(535);
- if(x_pulse2 > 535) {
+ left(12000);
+ if((x_pulse1 > 12000) && (x_pulse2 > 12000)) {
phase = 4;
}
break;
+
+ //1秒停止
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();
+ phase = 5;
+ wheel_reset();
}
break;
+
+ //右旋回(180°)
case 5:
counter.reset();
- right(-500);
- if((x_pulse1*-1 > 500) || (x_pulse2*-1 > 500)) {
+ turn_right(520);
+ if(x_pulse2 > 520) {
phase = 6;
}
- //if(!right_limit) {
- //phase = 6;
- //}
break;
+
+ //1秒停止
case 6:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 7;
- wheel_x1.reset();
- wheel_x2.reset();
- wheel_y1.reset();
- wheel_y2.reset();
+ wheel_reset();
}
break;
+
+ //ちょっくら右移動
case 7:
counter.reset();
- front(3000);
- if((y_pulse1 > 3000) || (y_pulse2 > 3000)) {
+ right(-1000);
+
+ if((x_pulse1*-1 > 500) && (x_pulse2*-1 > 500)) {
+ true_migimae_data[0] = 0x94;
+ true_migiusiro_data[0] = 0x10;
+ true_hidarimae_data[0] = 0x10;
+ true_hidariusiro_data[0] = 0x94;
+ }
+ if(right_limit == 3) {
phase = 8;
}
break;
+
+ //1秒停止
case 8:
stop();
counter.start();
if(counter.read() > 1.0f) {
phase = 9;
- wheel_x1.reset();
- wheel_x2.reset();
- wheel_y1.reset();
- wheel_y2.reset();
+ wheel_reset();
}
break;
+
+ //排出
case 9:
counter.reset();
- right(-3000);
- if((x_pulse1*-1 > 3000) || (x_pulse2*-1 > 3000)) {
- phase = 10;
+ //ここに排出動作が来るが今回は飛ばす
+ phase = 10;
+ break;
+
+ //排出機構引っ込める
+ case 10:
+ //ここに排出機構引っ込める動作が来るが今回は飛ばす
+ phase = 11;
+ break;
+
+ //1秒停止
+ case 11:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 12;
+ wheel_reset();
}
- //if(!right_limit) {
- //phase = 10;
- //}
break;
- case 10:
+
+ //前進
+ case 12:
+ counter.reset();
+ front(3000);
+ if((y_pulse1 > 3000) && (y_pulse2 > 3000)) {
+ phase = 13;
+ }
+ break;
+
+ //1秒停止
+ case 13:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 11;
- wheel_x1.reset();
- wheel_x2.reset();
- wheel_y1.reset();
- wheel_y2.reset();
+ phase = 14;
+ wheel_reset();
+ }
+ break;
+
+ //右移動
+ case 14:
+ counter.reset();
+ right(-4000);
+ if((x_pulse1*-1 > 3000) && (x_pulse2*-1 > 3000)) {
+ true_migimae_data[0] = 0x94;
+ true_migiusiro_data[0] = 0x10;
+ true_hidarimae_data[0] = 0x10;
+ true_hidariusiro_data[0] = 0x94;
+ }
+ if(right_limit == 3) {
+ phase = 15;
}
break;
- case 11:
+
+ //シーツ装填
+ case 15:
+ if(start_switch == 1) {
+ phase = 16;
+ } else {
+ stop();
+ }
+ break;
+
+ //竿のラインまで前進
+ case 16:
counter.reset();
front(21500);
- if((y_pulse1 > 21500) || (y_pulse2 > 21500)) {
- phase = 12;
+ if((y_pulse1 > 21500) && (y_pulse2 > 21500)) {
+ phase = 17;
}
break;
- case 12:
+
+ //1秒停止
+ case 17:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 13;
- wheel_x1.reset();
- wheel_x2.reset();
- wheel_y1.reset();
- wheel_y2.reset();
+ phase = 18;
+ wheel_reset();
+ }
+ break;
+
+ //掛けるところまで左移動
+ case 18:
+ counter.reset();
+ left(10000);
+ if((x_pulse1 > 10000) && (x_pulse2 > 10000)) {
+ phase = 19;
}
- break;
- case 13:
- left(8000);
- if((x_pulse1 > 8000) || (x_pulse2 > 8000)) {
- phase = 14;
+ break;
+
+ //1秒停止
+ case 19:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 20;
+ wheel_reset();
+ }
+ break;
+
+ //妨害防止の右旋回
+ case 20:
+ counter.reset();
+ turn_right(300);
+ if(x_pulse2 > 300) {
+ phase = 21;
}
break;
- case 14:
+
+ //1秒停止
+ case 21:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 22;
+ wheel_reset();
+ }
+ break;
+
+ //カウンターリセット
+ case 22:
stop();
+ counter.reset();
+ phase = 23;
+ break;
+
+ //シーツを掛ける
+ case 23:
+ counter.start();
+
+ //5秒間ファン送風
+ if(counter.read() <= 4.0f) {
+ fan_data[0] = 0xFF;
+ i2c.write(0x26, fan_data, 1);
+ servo_data[0] = 0x04;
+ i2c.write(0x30, servo_data, 1);
+ }
+ //4~5秒の間でサーボを放す
+ else if((counter.read() > 4.0f) && (counter.read() <= 5.0f)) {
+ fan_data[0] = 0xFF;
+ i2c.write(0x26, fan_data, 1);
+ servo_data[0] = 0x03;
+ i2c.write(0x30, servo_data, 1);
+ }
+ //5秒過ぎたらファン停止
+ else if(counter.read() > 5.0f) {
+ fan_data[0] = 0x80;
+ i2c.write(0x26, fan_data, 1);
+ servo_data[0] = 0x04;
+ i2c.write(0x30, servo_data, 1);
+ phase = 24;
+ }
+ break;
+
+ //終了っ！(守衛さん風)
+ case 24:
+ //駆動系統OFF
+ emergency = 0;
+ break;
+
default:
+ //駆動系統OFF
+ emergency = 0;
break;
}
}
-
- /*
- if(counter.read() < 5.00f) {
- counter.start();
- front(3000);
- }
- else if(counter.read() >= 5.00f && counter.read() < 10.00f) {
- right(-3000);
- }
- else if(counter.read() >= 10.00f && counter.read() < 15.00f) {
- back(-3000);
- }
- else if(counter.read() >= 15.00f && counter.read() < 20.00f) {
- left(3000);
- }
- else if(counter.read() >= 20.00f && counter.read() < 25.00f) {
- turn_right(535);
- }
- else if(counter.read() >= 25.00f && counter.read() < 30.00f) {
- turn_left(674);
- }
- else if(counter.read() >= 30.00f) {
- counter.reset();
- }
- */
}
}
void init(void) {
-
- //緊急停止用信号ピンをLow
- //emergency = 0;
//通信ボーレートの設定
pc.baud(460800);
- //pc.baud(9600);
limit_serial.baud(115200);
@@ -340,6 +478,8 @@
x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 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;
+ servo_data[0] = 0x80;
}
void init_send(void) {
@@ -373,6 +513,8 @@
void print_pulses(void) {
+ //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
+ pc.printf("upper: 0x%x, low: 0x%x, f: %d, b: %d, r: %d\n\r", upper_limit_data, lower_limit_data, front_limit, back_limit, right_limit);
//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]);
}
@@ -393,27 +535,251 @@
limit_data = limit_serial.getc();
- if((limit_data & 0b00000001) == 0x01) {
- USR_LED1 = 1; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 0;
+ //上位1bitが1ならば下のリミットのデータだと判断
+ if((limit_data & 0b10000000) == 0b10000000) {
+ lower_limit_data = limit_data;
+ //upper_limit_data = 0b01111111;
+
+ //上位1bitが0ならば上のリミットのデータだと判断
+ } else {
+ upper_limit_data = limit_data;
+ //lower_limit_data = 0b11111111;
+ }
+
+ masked_lower_front_limit_data = lower_limit_data & 0b00000011;
+ masked_lower_back_limit_data = lower_limit_data & 0b00001100;
+ masked_lower_right_limit_data = lower_limit_data & 0b00110000;
+
+ //前部リミット
+ switch(masked_lower_front_limit_data) {
+ //両方押された
+ case 0x00:
+ front_limit = 3;
+ break;
+ //右が押された
+ case 0b00000010:
+ front_limit = 1;
+ break;
+ //左が押された
+ case 0b00000001:
+ front_limit = 2;
+ break;
+ default:
+ front_limit = 0;
+ break;
+ }
+
+ //後部リミット
+ switch(masked_lower_back_limit_data) {
+ //両方押された
+ case 0x00:
+ back_limit = 3;
+ break;
+ //右が押された
+ case 0b00001000:
+ back_limit = 1;
+ break;
+ //左が押された
+ case 0b00000100:
+ back_limit = 2;
+ break;
+ default:
+ back_limit = 0;
+ break;
}
- if((limit_data & 0b00000010) == 0x02) {
- USR_LED1 = 0; USR_LED2 = 1; USR_LED3 = 0; USR_LED4 = 0;
+
+ //右部リミット
+ switch(masked_lower_right_limit_data) {
+ //両方押された
+ case 0x00:
+ right_limit = 3;
+ break;
+ //右が押された
+ case 0b00100000:
+ right_limit = 1;
+ break;
+ //左が押された
+ case 0b00010000:
+ right_limit = 2;
+ break;
+ default:
+ right_limit = 0;
+ break;
+ }
+
+ /*
+ //回収機構の上限
+ if((lower_limit_data & 0b01000000) == 0b01000000) {
+ withdrawal_upper_limit = 1;
+ } else {
+ withdrawal_upper_limit = 0;
+ }
+
+ //回収機構の下限
+ if((lower_limit_data & 0b10000000) == 0b10000000) {
+ withdrawal_lower_limit = 1;
+ } else {
+ withdrawal_lower_limit = 0;
}
- if((limit_data & 0b00000011) == 0x03) {
- USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 0; USR_LED4 = 0;
+
+ //右腕の上限
+ if((upper_limit_data & 0b00000001) == 0b00000001) {
+ right_arm_upper_limit = 1;
+ } else {
+ right_arm_upper_limit = 0;
+ }
+
+ //右腕の下限
+ if((upper_limit_data & 0b00000010) == 0b00000010) {
+ right_arm_lower_limit = 1;
+ } else {
+ right_arm_lower_limit = 0;
+ }
+
+ //左腕の上限
+ if((upper_limit_data & 0b00000100) == 0b00000100) {
+ left_arm_upper_limit = 1;
+ } else {
+ left_arm_upper_limit = 0;
+ }
+
+ //左腕の下限
+ if((upper_limit_data & 0b00001000) == 0b00001000) {
+ left_arm_lower_limit = 1;
+ } else {
+ left_arm_lower_limit = 0;
+ }
+
+ //吐き出し機構の上限
+ if((upper_limit_data & 0b00010000) == 0b00010000) {
+ force_out_upper_limit = 1;
+ } else {
+ force_out_upper_limit = 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((upper_limit_data & 0b00100000) == 0b00100000) {
+ force_out_lower_limit = 1;
+ } else {
+ force_out_lower_limit = 0;
}
- 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 wheel_reset(void) {
+
+ wheel_x1.reset();
+ wheel_x2.reset();
+ wheel_y1.reset();
+ wheel_y2.reset();
+}
+
+void kaisyu(int pulse) {
+
+ switch (kaisyu_phase) {
+ case 0:
+ arm_moter[0] = 0x80;
+ kaisyu_phase = 0;
+ break;
+ case 1:
+ //前進->減速
+ if(pulse < 2000) {
+ arm_moter[0] = 0xFF;
+ kaisyu_phase = 1;
+ } else {
+ arm_moter[0] = 0xB3;
+ kaisyu_phase = 2;
+ }
+ break;
+ case 2:
+ //前進->停止->後進
+ if(kaisyu_usiro == 1) {
+ arm_moter[0] = 0xB3;
+ kaisyu_phase = 2;
+ } else {
+ arm_moter[0] = 0x80;
+ kaisyu_phase = 3;
+ i2c.write(0x18, arm_moter,1);
+ wait(1);
+ }
+ break;
+ case 3:
+ //後進->減速
+ if(pulse > 1600) {
+ arm_moter[0] = 0x00;
+ kaisyu_phase = 3;
+ } else {
+ arm_moter[0] = 0x4C;
+ kaisyu_phase = 4;
+ }
+ break;
+ case 4:
+ //後進->停止
+ if(kaisyu_mae == 1) {
+ arm_moter[0] = 0x4C;
+ kaisyu_phase = 4;
+ } else {
+ arm_moter[0] = 0x80;
+ kaisyu_phase = 0;
+ phase = 1;
+ }
+ break;
+ default:
+ break;
+ }
+
+ i2c.write(0x18, arm_moter, 1);
+}
+
+void tyokudo(int pulse) {
+
+ switch (tyokudo_phase) {
+ case 0:
+ arm_moter[0] = 0x80;
+ drop_moter[0] = 0x80;
+ break;
+ case 1:
+ //前進->減速
+ arm_moter[0] = (pulse < 2000)? 0xCD:0xC0;
+ drop_moter[0] = (pulse < 2000)? 0xE6:0xCD;
+ tyokudo_phase = (pulse < 2000)? 1:2;
+ break;
+ case 2:
+ //前進(遅い)->停止->後進(早い)
+ if(tyokudo_mae == 1) {
+ arm_moter[0] = 0xC0;
+ drop_moter[0] = 0xCD;
+ tyokudo_phase = 2;
+ } else {
+ arm_moter[0] = 0x80;
+ drop_moter[0] = 0x80;
+ tyokudo_phase = 3;
+ i2c.write(0x18, arm_moter,1);
+ i2c.write(0x20, drop_moter,1);
+ wait(1);
+ }
+ break;
+ case 3:
+ //後進->減速
+ arm_moter[0] = (pulse > 1600)? 0x33:0x33;
+ drop_moter[0] = (pulse > 1600)? 0x19:0x19;
+ tyokudo_phase = (pulse > 1600)? 3:4;
+ break;
+ case 4:
+ //後進->停止
+ arm_moter[0] = (tyokudo_usiro == 1)? 0x33:0x80;
+ drop_moter[0] = (tyokudo_usiro == 1)? 0x19:0x80;
+ tyokudo_phase = (tyokudo_usiro == 1)? 4:0;
+ break;
+ default:
+ arm_moter[0] = 0x80;
+ drop_moter[0] = 0x80;
+ tyokudo_phase = 0;
+ break;
+ }
+
+ i2c.write(0x18, arm_moter, 1);
+ i2c.write(0x20, drop_moter, 1);
}
void front(int target) {
@@ -500,7 +866,7 @@
//制御量の最小、最大
//正転(目標に達してない)
- 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);
@@ -508,32 +874,26 @@
front_hidarimae.setOutputLimits(0x84, 0xFF);
front_hidariusiro.setOutputLimits(0x84, 0xFF);
}
- /*
//左側が前に出ちゃった♥(右側だけ回して左側は停止)
- else if((y_pulse1 < target) && (y_pulse2 > target)) {
+ 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 > target) && (y_pulse2 < target)) {
+ 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)) {
+ 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();
}
//よくわからんやつ
@@ -562,30 +922,28 @@
//制御量を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)) {
+ 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 > target) && (y_pulse2 < target)) {
+ 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)) {
+ else if((y_pulse1 > target) && (y_pulse2 > target)) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
@@ -603,7 +961,7 @@
//制御量の最小、最大
//逆転(目標に達してない)
- 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);
@@ -611,32 +969,26 @@
back_hidarimae.setOutputLimits(0x00, 0x7B);
back_hidariusiro.setOutputLimits(0x00, 0x7B);
}
- /*
//左側が後に出ちゃった♥(右側だけ回して左側は停止)
- else if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 > target*-1)) {
+ 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 > target*-1) && (y_pulse2*-1 < target*-1)) {
+ 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)) {
+ 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();
}
//よくわからんやつ
@@ -665,30 +1017,28 @@
//制御量を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)) {
+ 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 > target*-1) && (y_pulse2*-1 < target*-1)) {
+ 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)) {
+ 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;
@@ -706,7 +1056,7 @@
//制御量の最小、最大
//右進(目標まで達していない)
- if((x_pulse1*-1 < target*-1) || (x_pulse2*-1 < target*-1)) {
+ 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);
@@ -715,32 +1065,26 @@
right_hidariusiro.setOutputLimits(0x00, 0x7B);
}
- /*
//前側が右に出ちゃった♥(後側だけ回して前側は停止)
- else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 < target*-1)) {
+ 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 < target*-1) && (x_pulse2*-1 > target*-1)) {
+ 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)) {
+ 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();
}
//よくわからんやつ
@@ -769,30 +1113,28 @@
//制御量を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)) {
+ 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 < target*-1) && (x_pulse2*-1 > target*-1)) {
+ 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)) {
+ 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;
@@ -810,38 +1152,32 @@
//制御量の最小、最大
//左進(目標まで達していない)
- 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, 0x7B);
left_hidariusiro.setOutputLimits(0x84, 0xFF);
}
- /*
//前側が左に出ちゃった♥(後側だけ回して前側は停止)
- else if((x_pulse1 > target) && (x_pulse2 < target)) {
+ 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 < target) && (x_pulse2 > target)) {
+ 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);
left_hidariusiro.setOutputLimits(0x7C, 0x83);
- wheel_x1.reset();
- wheel_x2.reset();
- wheel_y1.reset();
- wheel_y2.reset();
}
//よくわからんやつ
@@ -870,30 +1206,28 @@
//制御量を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)) {
+ 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 < target) && (x_pulse2 > target)) {
+ 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;
@@ -923,10 +1257,6 @@
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();
}
//よくわからんやつ
@@ -992,10 +1322,6 @@
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();
}
//よくわからんやつ