2019NHK_teamA
1日体験入学のやつ
Revision 21:89db2a19e52e, committed 2019-09-21
- Comitter:
- yuron
- Date:
- Sat Sep 21 14:38:10 2019 +0000
- Parent:
- 20:ac4954be1fe0
- Commit message:
- aaaaaaa;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Sep 18 03:36:25 2019 +0000
+++ b/main.cpp Sat Sep 21 14:38:10 2019 +0000
@@ -4,7 +4,7 @@
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
/* Sensor: encorder*4 */
/* ------------------------------------------------------------------- */
-/* ファンとサーボの動作を追加した */
+/* blue zone is ok, added back phase */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
@@ -70,14 +70,18 @@
DigitalOut emergency(D11);
DigitalOut USR_LED1(PB_7);
-DigitalOut USR_LED2(PC_13);
+//DigitalOut USR_LED2(PC_13);
DigitalOut USR_LED3(PC_2);
DigitalOut USR_LED4(PC_3);
+DigitalOut GREEN_LED(D8);
+DigitalOut RED_LED(D10);
//遠隔非常停止ユニットLED
AnalogOut myled(A2);
DigitalIn start_switch(PB_12);
+DigitalIn USR_SWITCH(PC_13);
+DigitalIn zone_switch(PC_10);
QEI wheel_x1(PA_8 , PA_6 , NC, 624);
QEI wheel_x2(PB_14, PB_13, NC, 624);
@@ -102,7 +106,7 @@
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 arm_motor[1], drop_motor[1];
char fan_data[1];
char servo_data[1];
char right_arm_data[1], left_arm_data[1];
@@ -125,7 +129,10 @@
//後部が壁に当たっているか
int back_limit = 0;
//回収機構の下限(引っ込めてるほう)
-bool kaisyu_limit = 0;
+bool kaisyu_mae_limit = 0;
+
+bool kaisyu_usiro_limit = 0;
+
//右腕の下限
bool right_arm_lower_limit = 0;
//右腕の上限
@@ -139,17 +146,17 @@
//吐き出し機構の下限
bool tyokudo_usiro_limit = 0;
-
-int masked_lower_front_limit_data = 0xFF;
-int masked_lower_back_limit_data = 0xFF;
-int masked_lower_right_limit_data = 0xFF;
-int masked_kaisyu_limit_data = 0xFF;
+int masked_lower_front_limit_data = 0xFF;
+int masked_lower_back_limit_data = 0xFF;
+int masked_lower_right_limit_data = 0xFF;
+int masked_kaisyu_mae_limit_data = 0xFF;
+int masked_kaisyu_usiro_limit_data = 0xFF;
int masked_right_arm_lower_limit_data = 0xFF;
int masked_right_arm_upper_limit_data = 0xFF;
-int masked_left_arm_lower_limit_data = 0xFF;
-int masked_left_arm_upper_limit_data = 0xFF;
-int masked_tyokudo_mae_limit_data = 0xFF;
-int masked_tyokudo_usiro_limit_data = 0xFF;
+int masked_left_arm_lower_limit_data = 0xFF;
+int masked_left_arm_upper_limit_data = 0xFF;
+int masked_tyokudo_mae_limit_data = 0xFF;
+int masked_tyokudo_usiro_limit_data = 0xFF;
//関数のプロトタイプ宣言
void init(void);
@@ -160,9 +167,9 @@
void get_emergency(void);
void read_limit(void);
void wheel_reset(void);
-void kaisyu(int pulse);
-void tyokudo(int pulse);
-void arm_up(void);
+void kaisyu(int pulse, int next_phase);
+void tyokudo(int pulse, int next_phase);
+void arm_up(int next_phase);
void front(int target);
void back(int target);
void right(int target);
@@ -176,7 +183,6 @@
void left_PID(int target);
void turn_right_PID(int target);
void turn_left_PID(int target);
-void dondonkasoku(void);
int main(void) {
@@ -184,21 +190,45 @@
init_send();
//とりあえず(後で消してね)
- zone = BLUE;
-
+ //zone = BLUE;
+ //phase = 16;
+ //phase = 23;
+ phase = 30;
+
+ //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
+ while(1) {
+ if(zone_switch == 0) {
+ zone = BLUE;
+ } else {
+ zone = RED;
+ }
+ break;
+ }
+
while(1) {
get_pulses();
print_pulses();
get_emergency();
read_limit();
-
- //if(start_switch == 1) {
- //tyokudo(arm_enc.getPulses());
- //}
+
+ //move_servo_with_using_onboard-switch
+ if(USR_SWITCH == 0) {
+ servo_data[0] = 0x03;
+ i2c.write(0x30, servo_data, 1);
+ } else {
+ servo_data[0] = 0x04;
+ i2c.write(0x30, servo_data, 1);
+ }
+ if(start_switch == 1) {
+ phase = 23;
+ }
+
//青ゾーン
if(zone == BLUE) {
+ GREEN_LED = 1;
+ RED_LED = 0;
switch(phase) {
@@ -218,14 +248,8 @@
break;
//回収
- case 1:
- //回収アームのリミットが押されているか
- if(kaisyu_limit == 1) {
- kaisyu(arm_enc.getPulses());
- USR_LED2 = 1;
- } else {
- USR_LED2 = 0;
- }
+ case 1:
+ kaisyu(arm_enc.getPulses(), 2);
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
@@ -245,8 +269,8 @@
//左移動
case 3:
counter.reset();
- left(12000);
- if((x_pulse1 > 12000) && (x_pulse2 > 12000)) {
+ left(10000);
+ if((x_pulse1 > 10000) && (x_pulse2 > 10000)) {
phase = 4;
}
break;
@@ -283,14 +307,8 @@
//ちょっくら右移動
case 7:
counter.reset();
- 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;
- }
+ right(-2000);
+
if(right_limit == 3) {
phase = 8;
}
@@ -309,164 +327,195 @@
//排出
case 9:
counter.reset();
- tyokudo(arm_enc.getPulses());
- //ここに排出動作が来るが今回は飛ばす
- //phase = 10;
- break;
-
- //排出機構引っ込める
- case 10:
- //ここに排出機構引っ込める動作が来るが今回は飛ばす
- phase = 11;
+ tyokudo(arm_enc.getPulses(), 10);
break;
//1秒停止
- case 11:
+ case 10:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 12;
+ phase = 11;
wheel_reset();
}
break;
//前進
- case 12:
+ case 11:
counter.reset();
- front(3000);
- if((y_pulse1 > 3000) && (y_pulse2 > 3000)) {
- phase = 13;
+ front(3500);
+ if((y_pulse1 > 3500) && (y_pulse2 > 3500)) {
+ phase = 12;
}
break;
//1秒停止
- case 13:
+ case 12:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 14;
+ phase = 13;
wheel_reset();
}
break;
//右移動
- case 14:
+ case 13:
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 = 14;
}
- if(right_limit == 3) {
- phase = 15;
+ break;
+
+ //1秒停止
+ case 14:
+ stop();
+ counter.start();
+ if(counter.read() > 1.0f) {
+ phase = 16;
+ wheel_reset();
}
break;
- //シーツ装填
+ /*
+ //後進
case 15:
+ counter.reset();
+ back(-1000);
+
+ if(back_limit == 3) {
+ phase = 16;
+ }
+ */
+
+ //シーツ装填
+ case 16:
if(start_switch == 1) {
- phase = 16;
+ wheel_reset();
+ phase = 17;
} else {
stop();
}
break;
//竿のラインまで前進
- case 16:
+ case 17:
counter.reset();
- front(21500);
- if((y_pulse1 > 21500) && (y_pulse2 > 21500)) {
- phase = 17;
+ front(22000);
+ if((y_pulse1 > 22000) && (y_pulse2 > 22000)) {
+ phase = 18;
}
break;
//1秒停止
- case 17:
+ case 18:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 18;
+ phase = 19;
wheel_reset();
}
break;
//掛けるところまで左移動
- case 18:
+ case 19:
counter.reset();
left(10000);
if((x_pulse1 > 10000) && (x_pulse2 > 10000)) {
- phase = 19;
+ phase = 20;
}
break;
//1秒停止
- case 19:
+ case 20:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 20;
+ phase = 21;
wheel_reset();
}
break;
//妨害防止の右旋回
- case 20:
+ case 21:
counter.reset();
- turn_right(300);
- if(x_pulse2 > 300) {
- phase = 21;
+ turn_right(280);
+ if(x_pulse2 > 280) {
+ phase = 22;
}
break;
//1秒停止
- case 21:
+ case 22:
stop();
counter.start();
if(counter.read() > 1.0f) {
- phase = 22;
+ phase = 23;
wheel_reset();
}
break;
+ //カウンターリセット
+ case 23:
+ counter.reset();
+ counter.start();
+ phase = 24;
+
//アームアップ
- case 22:
+ case 24:
stop();
+
+ 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);
+ }
+ break;
+
+ //カウンターリセット
+ case 25:
counter.reset();
- arm_up();
- break;
+ phase = 26;
//シーツを掛ける
- case 23:
+ case 26:
counter.start();
-
- //5秒間ファン送風
- if(counter.read() <= 4.0f) {
+
+ //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);
}
- //4~5秒の間でサーボを放す
- else if((counter.read() > 4.0f) && (counter.read() <= 5.0f)) {
+ //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);
}
- //5秒過ぎたらファン停止
- else if(counter.read() > 5.0f) {
+ //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 = 24;
+ phase = 27;
}
break;
//終了っ！(守衛さん風)
- case 24:
+ case 27:
//駆動系統OFF
emergency = 0;
break;
@@ -477,6 +526,12 @@
break;
}
}
+
+ //REDゾーン
+ else if(zone == RED) {
+ GREEN_LED = 0;
+ RED_LED = 1;
+ }
}
}
@@ -488,6 +543,7 @@
limit_serial.baud(115200);
start_switch.mode(PullUp);
+ zone_switch.mode(PullDown);
//非常停止関連
pic.baud(19200);
@@ -499,6 +555,7 @@
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;
+ arm_motor[0] = 0x80; drop_motor[0] = 0x80;
right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
}
@@ -538,11 +595,14 @@
void print_pulses(void) {
- pc.printf("%d\n\r", tyokudo_phase);
- //pc.printf("%d\n\r", arm_enc.getPulses());
+ 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("%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\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]);
+ //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) {
@@ -574,10 +634,11 @@
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;
- masked_kaisyu_limit_data = lower_limit_data & 0b01000000;
+ masked_kaisyu_mae_limit_data = lower_limit_data & 0b01000000;
//上リミット基板からのデータのマスク処理
- masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001;
+ //masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001;
+ masked_kaisyu_usiro_limit_data = upper_limit_data & 0b00000001;
masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010;
masked_left_arm_lower_limit_data = upper_limit_data & 0b00000100;
masked_left_arm_upper_limit_data = upper_limit_data & 0b00001000;
@@ -642,20 +703,21 @@
}
//回収機構リミット
- switch(masked_kaisyu_limit_data) {
+ switch(masked_kaisyu_mae_limit_data) {
//押された
case 0b00000000:
- kaisyu_limit = 1;
+ kaisyu_mae_limit = 1;
break;
case 0b01000000:
- kaisyu_limit = 0;
+ kaisyu_mae_limit = 0;
break;
default:
- kaisyu_limit = 0;
+ kaisyu_mae_limit = 0;
break;
}
//右腕下部リミット
+ /*
switch(masked_right_arm_lower_limit_data) {
//押された
case 0b00000000:
@@ -668,7 +730,21 @@
right_arm_lower_limit = 0;
break;
}
-
+ */
+
+ //回収後リミット
+ switch(masked_kaisyu_usiro_limit_data) {
+ case 0b00000000:
+ kaisyu_usiro_limit = 1;
+ break;
+ case 0b00000001:
+ kaisyu_usiro_limit = 0;
+ break;
+ default:
+ kaisyu_usiro_limit = 0;
+ break;
+ }
+
//右腕上部リミット
switch(masked_right_arm_upper_limit_data) {
//押された
@@ -748,197 +824,154 @@
wheel_y2.reset();
}
-void kaisyu(int pulse) {
+void kaisyu(int pulse, int next_phase) {
switch (kaisyu_phase) {
-
+
case 0:
//前進->減速
//3000pulseまで高速前進
if(pulse < 3000) {
- arm_moter[0] = 0xFF;
+ arm_motor[0] = 0xFF;
//kaisyu_phase = 1;
- }
-
+ }
+
//3000pulse超えたら低速前進
else if(pulse >= 3000) {
- arm_moter[0] = 0xB3;
+ arm_motor[0] = 0xB3;
kaisyu_phase = 1;
}
break;
-
+
case 1:
+ USR_LED3 = 1;
//前進->停止->後進
//3600pulseまで低速前進
if(pulse < 3600) {
- arm_moter[0] = 0xB3;
+ arm_motor[0] = 0xB3;
//kaisyu_phase = 2;
- }
-
+ }
+
//3600pulse超えたら停止
else if(pulse >= 3600) {
- arm_moter[0] = 0x80;
-
+ arm_motor[0] = 0x80;
+
//1秒待ってから引っ込める
counter.start();
if(counter.read() > 1.0f) {
kaisyu_phase = 2;
}
}
-
+ //後ろのリミットが押されたら強制停止
+ if(kaisyu_usiro_limit == 1) {
+ arm_motor[0] = 0x80;
+ }
break;
-
+
case 2:
//後進->減速
//500pulseまで高速後進
counter.reset();
if(pulse > 500) {
- arm_moter[0] = 0x00;
+ arm_motor[0] = 0x00;
//kaisyu_phase = 3;
-
- }
+
+ }
//500pulse以下になったら低速後進
else if(pulse <= 500) {
- arm_moter[0] = 0x4C;
+ arm_motor[0] = 0x4C;
kaisyu_phase = 3;
}
break;
-
+
case 3:
//後進->停止
//リミット押されるまで低速後進
if(pulse <= 500) {
- arm_moter[0] = 0x4C;
+ arm_motor[0] = 0x4C;
//kaisyu_phase = 4;
}
-
+
//リミット押されたら停止
- if(kaisyu_limit == 1) {
- arm_moter[0] = 0x80;
+ if(kaisyu_mae_limit == 1) {
+ arm_motor[0] = 0x80;
kaisyu_phase = 4;
- phase = 2;
+ phase = next_phase;
}
break;
-
+
default:
- arm_moter[0] = 0x80;
+ arm_motor[0] = 0x80;
break;
}
//回収MDへ書き込み
- i2c.write(0x18, arm_moter, 1);
+ i2c.write(0x18, arm_motor, 1);
}
-void tyokudo(int pulse) {
+void tyokudo(int pulse, int next_phase) {
switch(tyokudo_phase) {
-
+
case 0:
//前進->減速
-
+
+ /* エンコーダー読まずにリミットだけ(修正必須) */
//3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
if(tyokudo_mae_limit == 0) {
//2000pulseまで高速前進
if(pulse < 2000) {
- arm_moter[0] = 0xCD;
- drop_moter[0] = 0xE6;
- }
+ arm_motor[0] = 0xC0;
+ drop_motor[0] = 0xE6;
+ }
//2000pulse以上で低速前進
else if(pulse >= 2000) {
- arm_moter[0] = 0xC0;
- drop_moter[0] = 0xCD;
+ arm_motor[0] = 0xC0;
+ drop_motor[0] = 0xE6;
}
//パルスが3600を終えたらアームのみ強制停止
else if(pulse > 3600) {
- arm_moter[0] = 0x80;
- drop_moter[0] = 0xCD;
+ arm_motor[0] = 0x80;
+ drop_motor[0] = 0xE6;
+ }
+
+ //後ろのリミットが押されたら強制停止
+ if(kaisyu_usiro_limit == 1) {
+ arm_motor[0] = 0x80;
}
}
-
+
//直動の前リミットが押されたら
else if(tyokudo_mae_limit == 1) {
- //直動のみ強制停止
- drop_moter[0] = 0x80;
-
- //2000pulseまで高速前進
- if(pulse < 2000) {
- arm_moter[0] = 0xCD;
- }
- //2000pulse以上で低速前進
- else if(pulse >= 2000) {
- arm_moter[0] = 0xC0;
- }
- //パルスが3600を終えたらアームも強制停止
- else if(pulse > 3600) {
- arm_moter[0] = 0x80;
- tyokudo_phase = 1;
- }
+ //高速後進
+ arm_motor[0] = 0x40;
+ drop_motor[0] = 0x00;
+ tyokudo_phase = 1;
+ }
+ break;
+
+ case 1:
+ //後進->減速
+ //リミットが押されたら強制停止
+ if(tyokudo_usiro_limit == 1) {
+ arm_motor[0] = 0x80;
+ drop_motor[0] = 0x80;
+ tyokudo_phase = 2;
+ phase = next_phase;
}
break;
- case 1:
- //後進->減速
-
- //1600pulseより大きいとき高速後進
- if(pulse > 1600) {
-
- if(kaisyu_limit == 0) {
- arm_moter[0] = 0x00;
- }
- //リミットが押されたら強制停止
- else if(kaisyu_limit == 1) {
- arm_moter[0] = 0x80;
- }
-
- if(tyokudo_usiro_limit == 0) {
- drop_moter[0] = 0x19;
- }
- //リミットが押されたら強制停止
- else if(tyokudo_usiro_limit == 1) {
- drop_moter[0] = 0x80;
- }
- }
- //500pulse以下でphase2へ移行
- else if(pulse <= 500) {
- tyokudo_phase = 2;
- }
- break;
-
- case 2:
- //後進->停止
-
- //直動後リミットが押されるまで後進
- if(tyokudo_usiro_limit == 0) {
- drop_moter[0] = 0x19;
- }
- //直動後リミットが押されたら停止
- else if(tyokudo_usiro_limit == 1) {
- drop_moter[0] = 0x80;
- }
-
- //リミット押されるまで低速後進
- if(kaisyu_limit == 0) {
- arm_moter[0] = 0x4C;
- }
- else if(kaisyu_limit == 1) {
- arm_moter[1] = 0x80;
- tyokudo_phase = 3;
- phase = 10;
- }
-
- break;
-
default:
- arm_moter[0] = 0x80;
- drop_moter[0] = 0x80;
+ arm_motor[0] = 0x80;
+ drop_motor[0] = 0x80;
break;
}
- i2c.write(0x18, arm_moter, 1);
- i2c.write(0x20, drop_moter, 1);
+ i2c.write(0x18, arm_motor, 1);
+ i2c.write(0x20, drop_motor, 1);
}
-void arm_up(void) {
+void arm_up(int next_phase) {
//両腕、上限リミットが押されてなかったら上昇
if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
@@ -955,11 +988,12 @@
//両腕、上限リミットが押されたら停止
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
- phase = 23;
+ phase = next_phase;
}
i2c.write(0x22, right_arm_data, 1);
i2c.write(0x24, left_arm_data, 1);
+ wait_us(20);
}
void front(int target) {
@@ -1054,6 +1088,7 @@
front_hidarimae.setOutputLimits(0x84, 0xFF);
front_hidariusiro.setOutputLimits(0x84, 0xFF);
}
+ /*
//左側が前に出ちゃった♥(右側だけ回して左側は停止)
else if((y_pulse1 + wheel_difference) < y_pulse2) {
front_migimae.setOutputLimits(0x84, 0xFF);
@@ -1068,6 +1103,7 @@
front_hidarimae.setOutputLimits(0x84, 0xFF);
front_hidariusiro.setOutputLimits(0x84, 0xFF);
}
+ */
//停止(目標より行き過ぎ)
else if((y_pulse1 > target) && (y_pulse2 > target)) {
front_migimae.setOutputLimits(0x7C, 0x83);
@@ -1108,6 +1144,7 @@
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];
@@ -1122,6 +1159,7 @@
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;
@@ -1149,6 +1187,7 @@
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);
@@ -1163,6 +1202,7 @@
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);
@@ -1203,6 +1243,7 @@
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];
@@ -1217,6 +1258,7 @@
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;
@@ -1238,13 +1280,14 @@
//右進(目標まで達していない)
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_migimae.setOutputLimits(0x7A, 0x7B);
+ right_migiusiro.setOutputLimits(0xFE, 0xFF);
//right_hidarimae.setOutputLimits(0x84, 0xF0);
- right_hidarimae.setOutputLimits(0x84, 0xFF);
- right_hidariusiro.setOutputLimits(0x00, 0x7B);
+ 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);
@@ -1259,6 +1302,7 @@
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);
@@ -1299,6 +1343,7 @@
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;
@@ -1313,6 +1358,7 @@
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;
@@ -1332,12 +1378,14 @@
//制御量の最小、最大
//左進(目標まで達していない)
- 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);
+ if((x_pulse1 < target) || (x_pulse2 < target)) {
+ left_migimae.setOutputLimits(0xEC, 0xED);
+ //left_migimae.setOutputLimits(0x84, 0xFF);
+ left_migiusiro.setOutputLimits(0x7A, 0x7B);
+ left_hidarimae.setOutputLimits(0x7A, 0x7B);
+ left_hidariusiro.setOutputLimits(0xFE, 0xFF);
}
+ /*
//前側が左に出ちゃった♥(後側だけ回して前側は停止)
else if(x_pulse1 > (x_pulse2 + wheel_difference)){
left_migimae.setOutputLimits(0x7C, 0x83);
@@ -1352,8 +1400,9 @@
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);
@@ -1386,12 +1435,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 > (x_pulse2 + wheel_difference)){
true_migimae_data[0] = 0x80;
@@ -1406,8 +1456,9 @@
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;
@@ -1544,39 +1595,3 @@
true_hidariusiro_data[0] = 0x80;
}
}
-
-void dondonkasoku(void) {
-
- //どんどん加速(正転)
- for(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++){
- i2c.write(0x10, init_send_data, 1);
- i2c.write(0x12, init_send_data, 1);
- i2c.write(0x14, init_send_data, 1);
- i2c.write(0x16, init_send_data, 1);
- wait(0.05);
- }
- //どんどん減速(正転)
- for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--){
- i2c.write(0x10, init_send_data, 1);
- i2c.write(0x12, init_send_data, 1);
- i2c.write(0x14, init_send_data, 1);
- i2c.write(0x16, init_send_data, 1);
- wait(0.05);
- }
- //だんだん加速(逆転)
- for(init_send_data[0] = 0x7B; init_send_data[0] > 0x00; init_send_data[0]--){
- i2c.write(0x10, init_send_data, 1);
- i2c.write(0x12, init_send_data, 1);
- i2c.write(0x14, init_send_data, 1);
- i2c.write(0x16, init_send_data, 1);
- wait(0.05);
- }
- //だんだん減速(逆転)
- for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++){
- i2c.write(0x10, init_send_data, 1);
- i2c.write(0x12, init_send_data, 1);
- i2c.write(0x14, init_send_data, 1);
- i2c.write(0x16, init_send_data, 1);
- wait(0.05);
- }
-}