2019NHK_teamA
/
2019_A_ver7
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 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);
- }
-}