2019NHK_teamA
/
2019_A_ver6-2_fixed_phase1
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:123520676121, committed 2019-09-19
- Comitter:
- st17099ng
- Date:
- Thu Sep 19 07:35:49 2019 +0000
- Parent:
- 20:ac4954be1fe0
- Commit message:
- 2019/09/18fixed; change point; phase1; kaisyu; tyokudo
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
diff -r ac4954be1fe0 -r 123520676121 main.cpp
--- a/main.cpp Wed Sep 18 03:36:25 2019 +0000
+++ b/main.cpp Thu Sep 19 07:35:49 2019 +0000
@@ -79,11 +79,11 @@
DigitalIn start_switch(PB_12);
-QEI wheel_x1(PA_8 , PA_6 , NC, 624);
+QEI wheel_x1(PA_8, PA_6, NC, 624);
QEI wheel_x2(PB_14, PB_13, NC, 624);
-QEI wheel_y1(PB_1 , PB_15, NC, 624);
+QEI wheel_y1(PB_1, PB_15, NC, 624);
QEI wheel_y2(PA_12, PA_11, NC, 624);
-QEI arm_enc(PB_5, PB_4 , NC, 624);
+QEI arm_enc(PB_5, PB_4, NC, 624);
//移動後n秒停止タイマー
Timer counter;
@@ -178,7 +178,8 @@
void turn_left_PID(int target);
void dondonkasoku(void);
-int main(void) {
+int main(void)
+{
init();
init_send();
@@ -194,9 +195,9 @@
read_limit();
//if(start_switch == 1) {
- //tyokudo(arm_enc.getPulses());
+ //tyokudo(arm_enc.getPulses());
//}
-
+
//青ゾーン
if(zone == BLUE) {
@@ -219,19 +220,25 @@
//回収
case 1:
- //回収アームのリミットが押されているか
- if(kaisyu_limit == 1) {
- kaisyu(arm_enc.getPulses());
- USR_LED2 = 1;
- } else {
- USR_LED2 = 0;
- }
+ /*
+ //回収アームのリミットが押されているか
+
+ //一度伸ばし始めると止まらないバグあり(09/18)
+ if(kaisyu_limit == 1) {
+ kaisyu(arm_enc.getPulses());
+ USR_LED2 = 1;
+ } else if(kaisyu_phase >= 1){
+ USR_LED2 = 0;
+ }
+ */
+ kaisyu(arm_enc.getPulses());
servo_data[0] = 0x03;
i2c.write(0x30, servo_data, 1);
break;
//1秒停止
case 2:
+ USR_LED2 = 0;
stop();
servo_data[0] = 0x04;
i2c.write(0x30, servo_data, 1);
@@ -436,7 +443,7 @@
counter.reset();
arm_up();
break;
-
+
//シーツを掛ける
case 23:
counter.start();
@@ -480,13 +487,14 @@
}
}
-void init(void) {
+void init(void)
+{
//通信ボーレートの設定
pc.baud(460800);
limit_serial.baud(115200);
-
+// emergency = 0;
start_switch.mode(PullUp);
//非常停止関連
@@ -494,15 +502,26 @@
pic.format(8, Serial::None, 1);
pic.attach(get, Serial::RxIrq);
- 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;
+ 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;
- right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
+ right_arm_data[0] = 0x80;
+ left_arm_data[0] = 0x80;
}
-void init_send(void) {
+void init_send(void)
+{
init_send_data[0] = 0x80;
i2c.write(0x10, init_send_data, 1);
@@ -517,7 +536,8 @@
wait(0.1);
}
-void get(void) {
+void get(void)
+{
baff = pic.getc();
@@ -528,219 +548,221 @@
flug = 1;
}
-void get_pulses(void) {
+void get_pulses(void)
+{
- x_pulse1 = wheel_x1.getPulses();
- x_pulse2 = wheel_x2.getPulses();
- y_pulse1 = wheel_y1.getPulses();
- y_pulse2 = wheel_y2.getPulses();
+ x_pulse1 = wheel_x1.getPulses();
+ x_pulse2 = wheel_x2.getPulses();
+ y_pulse1 = wheel_y1.getPulses();
+ y_pulse2 = wheel_y2.getPulses();
}
-void print_pulses(void) {
-
- pc.printf("%d\n\r", tyokudo_phase);
- //pc.printf("%d\n\r", arm_enc.getPulses());
- //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]);
+void print_pulses(void)
+{
+ pc.printf("%d\n",phase);
+ //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]);
}
-void get_emergency(void) {
+void get_emergency(void)
+{
if(RDATA == '1') {
myled = 1;
emergency = 1;
- }
- else if(RDATA == '9'){
+ } else if(RDATA == '9') {
myled = 0.2;
emergency = 0;
}
}
-void read_limit(void) {
-
- limit_data = limit_serial.getc();
+void read_limit(void)
+{
- //上位1bitが1ならば下のリミットのデータだと判断
- if((limit_data & 0b10000000) == 0b10000000) {
- lower_limit_data = limit_data;
+ limit_data = limit_serial.getc();
+
+ //上位1bitが1ならば下のリミットのデータだと判断
+ if((limit_data & 0b10000000) == 0b10000000) {
+ lower_limit_data = limit_data;
//上位1bitが0ならば上のリミットのデータだと判断
- } else {
- upper_limit_data = limit_data;
- }
+ } else {
+ upper_limit_data = limit_data;
+ }
- //下リミット基板からのデータのマスク処理
- 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_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_right_arm_lower_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;
- masked_tyokudo_mae_limit_data = upper_limit_data & 0b00010000;
- masked_tyokudo_usiro_limit_data = upper_limit_data & 0b00100000;
+ //上リミット基板からのデータのマスク処理
+ masked_right_arm_lower_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;
+ masked_tyokudo_mae_limit_data = upper_limit_data & 0b00010000;
+ masked_tyokudo_usiro_limit_data = upper_limit_data & 0b00100000;
- //前部リミット
- 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_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;
- }
+ //後部リミット
+ 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;
+ }
- //右部リミット
- 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;
- }
+ //右部リミット
+ 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;
+ }
- //回収機構リミット
- switch(masked_kaisyu_limit_data) {
- //押された
- case 0b00000000:
- kaisyu_limit = 1;
- break;
- case 0b01000000:
- kaisyu_limit = 0;
- break;
- default:
- kaisyu_limit = 0;
- break;
- }
+ //回収機構リミット
+ switch(masked_kaisyu_limit_data) {
+ //押された
+ case 0b00000000:
+ kaisyu_limit = 1;
+ break;
+ case 0b01000000:
+ kaisyu_limit = 0;
+ break;
+ default:
+ kaisyu_limit = 0;
+ break;
+ }
- //右腕下部リミット
- switch(masked_right_arm_lower_limit_data) {
- //押された
- case 0b00000000:
- right_arm_lower_limit = 1;
- break;
- case 0b00000001:
- right_arm_lower_limit = 0;
- break;
- default:
- right_arm_lower_limit = 0;
- break;
- }
+ //右腕下部リミット
+ switch(masked_right_arm_lower_limit_data) {
+ //押された
+ case 0b00000000:
+ right_arm_lower_limit = 1;
+ break;
+ case 0b00000001:
+ right_arm_lower_limit = 0;
+ break;
+ default:
+ right_arm_lower_limit = 0;
+ break;
+ }
- //右腕上部リミット
- switch(masked_right_arm_upper_limit_data) {
- //押された
- case 0b00000000:
- right_arm_upper_limit = 1;
- break;
- case 0b00000010:
- right_arm_upper_limit = 0;
- break;
- default:
- right_arm_upper_limit = 0;
- break;
- }
+ //右腕上部リミット
+ switch(masked_right_arm_upper_limit_data) {
+ //押された
+ case 0b00000000:
+ right_arm_upper_limit = 1;
+ break;
+ case 0b00000010:
+ right_arm_upper_limit = 0;
+ break;
+ default:
+ right_arm_upper_limit = 0;
+ break;
+ }
- //左腕下部リミット
- switch(masked_left_arm_lower_limit_data) {
- //押された
- case 0b00000000:
- left_arm_lower_limit = 1;
- break;
- case 0b00000100:
- left_arm_lower_limit = 0;
- break;
- default:
- left_arm_lower_limit = 0;
- break;
- }
+ //左腕下部リミット
+ switch(masked_left_arm_lower_limit_data) {
+ //押された
+ case 0b00000000:
+ left_arm_lower_limit = 1;
+ break;
+ case 0b00000100:
+ left_arm_lower_limit = 0;
+ break;
+ default:
+ left_arm_lower_limit = 0;
+ break;
+ }
- //左腕上部リミット
- switch(masked_left_arm_upper_limit_data) {
- //押された
- case 0b00000000:
- left_arm_upper_limit = 1;
- break;
- case 0b00001000:
- left_arm_upper_limit = 0;
- break;
- default:
- left_arm_upper_limit = 0;
- break;
- }
+ //左腕上部リミット
+ switch(masked_left_arm_upper_limit_data) {
+ //押された
+ case 0b00000000:
+ left_arm_upper_limit = 1;
+ break;
+ case 0b00001000:
+ left_arm_upper_limit = 0;
+ break;
+ default:
+ left_arm_upper_limit = 0;
+ break;
+ }
- //直動の前
- switch(masked_tyokudo_mae_limit_data) {
- //押された
- case 0b00000000:
- tyokudo_mae_limit = 1;
- break;
- case 0b00010000:
- tyokudo_mae_limit = 0;
- break;
- default:
- tyokudo_mae_limit = 0;
- break;
- }
+ //直動の前
+ switch(masked_tyokudo_mae_limit_data) {
+ //押された
+ case 0b00000000:
+ tyokudo_mae_limit = 1;
+ break;
+ case 0b00010000:
+ tyokudo_mae_limit = 0;
+ break;
+ default:
+ tyokudo_mae_limit = 0;
+ break;
+ }
- //直動の後
- switch(masked_tyokudo_usiro_limit_data) {
- //押された
- case 0b00000000:
- tyokudo_usiro_limit = 1;
- break;
- case 0b00100000:
- tyokudo_usiro_limit = 0;
- break;
- default:
- tyokudo_usiro_limit = 0;
- break;
- }
+ //直動の後
+ switch(masked_tyokudo_usiro_limit_data) {
+ //押された
+ case 0b00000000:
+ tyokudo_usiro_limit = 1;
+ break;
+ case 0b00100000:
+ tyokudo_usiro_limit = 0;
+ break;
+ default:
+ tyokudo_usiro_limit = 0;
+ break;
+ }
}
-void wheel_reset(void) {
+void wheel_reset(void)
+{
wheel_x1.reset();
wheel_x2.reset();
@@ -748,46 +770,47 @@
wheel_y2.reset();
}
-void kaisyu(int pulse) {
+void kaisyu(int pulse)
+{
+ switch (kaisyu_phase) {
- switch (kaisyu_phase) {
-
case 0:
//前進->減速
//3000pulseまで高速前進
if(pulse < 3000) {
arm_moter[0] = 0xFF;
//kaisyu_phase = 1;
- }
-
+ }
+
//3000pulse超えたら低速前進
else if(pulse >= 3000) {
arm_moter[0] = 0xB3;
kaisyu_phase = 1;
}
break;
-
+
case 1:
+ USR_LED2 = 1;
//前進->停止->後進
//3600pulseまで低速前進
if(pulse < 3600) {
arm_moter[0] = 0xB3;
//kaisyu_phase = 2;
- }
-
+ }
+
//3600pulse超えたら停止
else if(pulse >= 3600) {
arm_moter[0] = 0x80;
-
+
//1秒待ってから引っ込める
counter.start();
if(counter.read() > 1.0f) {
kaisyu_phase = 2;
}
}
-
+
break;
-
+
case 2:
//後進->減速
//500pulseまで高速後進
@@ -795,15 +818,15 @@
if(pulse > 500) {
arm_moter[0] = 0x00;
//kaisyu_phase = 3;
-
- }
+
+ }
//500pulse以下になったら低速後進
else if(pulse <= 500) {
arm_moter[0] = 0x4C;
kaisyu_phase = 3;
}
break;
-
+
case 3:
//後進->停止
//リミット押されるまで低速後進
@@ -811,772 +834,739 @@
arm_moter[0] = 0x4C;
//kaisyu_phase = 4;
}
-
+
//リミット押されたら停止
if(kaisyu_limit == 1) {
arm_moter[0] = 0x80;
kaisyu_phase = 4;
- phase = 2;
+ phase = 2;
}
break;
-
+
default:
arm_moter[0] = 0x80;
break;
}
-
+
//回収MDへ書き込み
i2c.write(0x18, arm_moter, 1);
}
-void tyokudo(int pulse) {
+void tyokudo(int pulse)
+{
switch(tyokudo_phase) {
-
+
case 0:
//前進->減速
-
+
+ /* エンコーダー読まずにリミットだけ(修正必須) */
//3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
if(tyokudo_mae_limit == 0) {
//2000pulseまで高速前進
if(pulse < 2000) {
- arm_moter[0] = 0xCD;
+ arm_moter[0] = 0xC0;
drop_moter[0] = 0xE6;
- }
- //2000pulse以上で低速前進
- else if(pulse >= 2000) {
- arm_moter[0] = 0xC0;
- drop_moter[0] = 0xCD;
- }
- //パルスが3600を終えたらアームのみ強制停止
- else if(pulse > 3600) {
- arm_moter[0] = 0x80;
- drop_moter[0] = 0xCD;
- }
- }
-
- //直動の前リミットが押されたら
- 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;
+ drop_moter[0] = 0xE6;
}
- //パルスが3600を終えたらアームも強制停止
+ //パルスが3600を終えたらアームのみ強制停止
else if(pulse > 3600) {
arm_moter[0] = 0x80;
- tyokudo_phase = 1;
- }
- }
- 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;
+ drop_moter[0] = 0xE6;
}
}
- //500pulse以下でphase2へ移行
- else if(pulse <= 500) {
- tyokudo_phase = 2;
+
+ //直動の前リミットが押されたら
+ else if(tyokudo_mae_limit == 1) {
+ //高速後進
+ arm_moter[0] = 0x40;
+ drop_moter[0] = 0x00;
+ tyokudo_phase = 1;
}
break;
-
- case 2:
- //後進->停止
-
- //直動後リミットが押されるまで後進
- if(tyokudo_usiro_limit == 0) {
- drop_moter[0] = 0x19;
- }
- //直動後リミットが押されたら停止
- else if(tyokudo_usiro_limit == 1) {
+
+ case 1:
+ //後進->減速
+ //リミットが押されたら強制停止
+ if(tyokudo_usiro_limit == 1) {
+ arm_moter[0] = 0x80;
drop_moter[0] = 0x80;
- }
-
- //リミット押されるまで低速後進
- if(kaisyu_limit == 0) {
- arm_moter[0] = 0x4C;
- }
- else if(kaisyu_limit == 1) {
- arm_moter[1] = 0x80;
- tyokudo_phase = 3;
+ tyokudo_phase = 2;
phase = 10;
}
-
break;
-
default:
arm_moter[0] = 0x80;
drop_moter[0] = 0x80;
break;
}
-
+
i2c.write(0x18, arm_moter, 1);
i2c.write(0x20, drop_moter, 1);
}
-void arm_up(void) {
-
+void arm_up(void)
+{
+
//両腕、上限リミットが押されてなかったら上昇
if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
- right_arm_data[0] = 0xFF; left_arm_data[0] = 0xFF;
+ right_arm_data[0] = 0xFF;
+ left_arm_data[0] = 0xFF;
}
//右腕のみリミットが押されたら左腕のみ上昇
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
- right_arm_data[0] = 0x80; left_arm_data[0] = 0xFF;
+ right_arm_data[0] = 0x80;
+ left_arm_data[0] = 0xFF;
}
//左腕のみリミットが押されたら右腕のみ上昇
else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
- right_arm_data[0] = 0xFF; left_arm_data[0] = 0x80;
+ right_arm_data[0] = 0xFF;
+ left_arm_data[0] = 0x80;
}
//両腕、上限リミットが押されたら停止
else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
- right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
+ right_arm_data[0] = 0x80;
+ left_arm_data[0] = 0x80;
phase = 23;
}
-
+
i2c.write(0x22, right_arm_data, 1);
i2c.write(0x24, left_arm_data, 1);
}
-void front(int target) {
+void front(int target)
+{
+
+ front_PID(target);
+ 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 back(int target)
+{
- front_PID(target);
- 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);
+ back_PID(target);
+ 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 right(int target)
+{
+
+ right_PID(target);
+ 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 back(int target) {
+void left(int target)
+{
- back_PID(target);
- 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);
+ left_PID(target);
+ 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 turn_right(int target)
+{
+
+ turn_right_PID(target);
+ 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 right(int target) {
+void turn_left(int target)
+{
+
+ turn_left_PID(target);
+ 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);
+}
- right_PID(target);
- 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 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 left(int target) {
+void front_PID(int target)
+{
+
+ //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
+ front_migimae.setInputLimits(-2147483648, 2147483647);
+ front_migiusiro.setInputLimits(-2147483648, 2147483647);
+ front_hidarimae.setInputLimits(-2147483648, 2147483647);
+ front_hidariusiro.setInputLimits(-2147483648, 2147483647);
+
+ //制御量の最小、最大
+ //正転(目標に達してない)
+ 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 + 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);
+ front_migiusiro.setOutputLimits(0x7C, 0x83);
+ front_hidarimae.setOutputLimits(0x7C, 0x83);
+ front_hidariusiro.setOutputLimits(0x7C, 0x83);
+ }
+
+ //よくわからんやつ
+ front_migimae.setMode(AUTO_MODE);
+ front_migiusiro.setMode(AUTO_MODE);
+ front_hidarimae.setMode(AUTO_MODE);
+ front_hidariusiro.setMode(AUTO_MODE);
- left_PID(target);
- 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);
+ //目標値
+ front_migimae.setSetPoint(target);
+ front_migiusiro.setSetPoint(target);
+ front_hidarimae.setSetPoint(target);
+ front_hidariusiro.setSetPoint(target);
+
+ //センサ出力
+ front_migimae.setProcessValue(y_pulse1);
+ front_migiusiro.setProcessValue(y_pulse1);
+ front_hidarimae.setProcessValue(y_pulse2);
+ front_hidariusiro.setProcessValue(y_pulse2);
+
+ //制御量(計算結果)
+ migimae_data[0] = front_migimae.compute();
+ migiusiro_data[0] = front_migiusiro.compute();
+ hidarimae_data[0] = front_hidarimae.compute();
+ hidariusiro_data[0] = front_hidariusiro.compute();
+
+ //制御量をPWM値に変換
+ //正転(目標に達してない)
+ 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 + 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;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
}
-void turn_right(int target) {
+void back_PID(int target)
+{
+
+ //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
+ back_migimae.setInputLimits(-2147483648, 2147483647);
+ back_migiusiro.setInputLimits(-2147483648, 2147483647);
+ back_hidarimae.setInputLimits(-2147483648, 2147483647);
+ back_hidariusiro.setInputLimits(-2147483648, 2147483647);
- turn_right_PID(target);
- 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);
-}
+ //制御量の最小、最大
+ //逆転(目標に達してない)
+ 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);
+ }
+ //左側が後に出ちゃった♥(右側だけ回して左側は停止)
+ 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);
+ back_migiusiro.setOutputLimits(0x7C, 0x83);
+ back_hidarimae.setOutputLimits(0x7C, 0x83);
+ back_hidariusiro.setOutputLimits(0x7C, 0x83);
+ }
-void turn_left(int target) {
+ //よくわからんやつ
+ back_migimae.setMode(AUTO_MODE);
+ back_migiusiro.setMode(AUTO_MODE);
+ back_hidarimae.setMode(AUTO_MODE);
+ back_hidariusiro.setMode(AUTO_MODE);
+
+ //目標値
+ back_migimae.setSetPoint(target*-1);
+ back_migiusiro.setSetPoint(target*-1);
+ back_hidarimae.setSetPoint(target*-1);
+ back_hidariusiro.setSetPoint(target*-1);
+
+ //センサ出力
+ back_migimae.setProcessValue(y_pulse1*-1);
+ back_migiusiro.setProcessValue(y_pulse1*-1);
+ back_hidarimae.setProcessValue(y_pulse2*-1);
+ back_hidariusiro.setProcessValue(y_pulse2*-1);
+
+ //制御量(計算結果)
+ migimae_data[0] = back_migimae.compute();
+ migiusiro_data[0] = back_migiusiro.compute();
+ hidarimae_data[0] = back_hidarimae.compute();
+ hidariusiro_data[0] = back_hidariusiro.compute();
- turn_left_PID(target);
- 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 stop(void) {
-
+ //制御量をPWM値に変換
+ //逆転(目標に達してない)
+ 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 + 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;
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) {
+void right_PID(int target)
+{
+
+ //センサ出力値の最小、最大
+ right_migimae.setInputLimits(-2147483648, 2147483647);
+ right_migiusiro.setInputLimits(-2147483648, 2147483647);
+ right_hidarimae.setInputLimits(-2147483648, 2147483647);
+ right_hidariusiro.setInputLimits(-2147483648, 2147483647);
- //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
- front_migimae.setInputLimits(-2147483648, 2147483647);
- front_migiusiro.setInputLimits(-2147483648, 2147483647);
- front_hidarimae.setInputLimits(-2147483648, 2147483647);
- front_hidariusiro.setInputLimits(-2147483648, 2147483647);
+ //制御量の最小、最大
+ //右進(目標まで達していない)
+ 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, 0xFF);
+ right_hidariusiro.setOutputLimits(0x00, 0x7B);
- //制御量の最小、最大
- //正転(目標に達してない)
- 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 + 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);
- front_migiusiro.setOutputLimits(0x7C, 0x83);
- front_hidarimae.setOutputLimits(0x7C, 0x83);
- front_hidariusiro.setOutputLimits(0x7C, 0x83);
- }
+ }
+ //前側が右に出ちゃった♥(後側だけ回して前側は停止)
+ 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);
+ right_migiusiro.setOutputLimits(0x7C, 0x83);
+ right_hidarimae.setOutputLimits(0x7C, 0x83);
+ right_hidariusiro.setOutputLimits(0x7C, 0x83);
+ }
- //よくわからんやつ
- front_migimae.setMode(AUTO_MODE);
- front_migiusiro.setMode(AUTO_MODE);
- front_hidarimae.setMode(AUTO_MODE);
- front_hidariusiro.setMode(AUTO_MODE);
-
- //目標値
- front_migimae.setSetPoint(target);
- front_migiusiro.setSetPoint(target);
- front_hidarimae.setSetPoint(target);
- front_hidariusiro.setSetPoint(target);
-
- //センサ出力
- front_migimae.setProcessValue(y_pulse1);
- front_migiusiro.setProcessValue(y_pulse1);
- front_hidarimae.setProcessValue(y_pulse2);
- front_hidariusiro.setProcessValue(y_pulse2);
-
- //制御量(計算結果)
- migimae_data[0] = front_migimae.compute();
- migiusiro_data[0] = front_migiusiro.compute();
- hidarimae_data[0] = front_hidarimae.compute();
- hidariusiro_data[0] = front_hidariusiro.compute();
+ //よくわからんやつ
+ right_migimae.setMode(AUTO_MODE);
+ right_migiusiro.setMode(AUTO_MODE);
+ right_hidarimae.setMode(AUTO_MODE);
+ right_hidariusiro.setMode(AUTO_MODE);
- //制御量をPWM値に変換
- //正転(目標に達してない)
- 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 + 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;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- }
-}
+ //目標値
+ right_migimae.setSetPoint(target*-1);
+ right_migiusiro.setSetPoint(target*-1);
+ right_hidarimae.setSetPoint(target*-1);
+ right_hidariusiro.setSetPoint(target*-1);
-void back_PID(int target) {
+ //センサ出力
+ right_migimae.setProcessValue(x_pulse1*-1);
+ right_migiusiro.setProcessValue(x_pulse2*-1);
+ right_hidarimae.setProcessValue(x_pulse1*-1);
+ right_hidariusiro.setProcessValue(x_pulse2*-1);
- //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
- back_migimae.setInputLimits(-2147483648, 2147483647);
- back_migiusiro.setInputLimits(-2147483648, 2147483647);
- back_hidarimae.setInputLimits(-2147483648, 2147483647);
- back_hidariusiro.setInputLimits(-2147483648, 2147483647);
+ //制御量(計算結果)
+ migimae_data[0] = right_migimae.compute();
+ migiusiro_data[0] = right_migiusiro.compute();
+ hidarimae_data[0] = right_hidarimae.compute();
+ hidariusiro_data[0] = right_hidariusiro.compute();
- //制御量の最小、最大
- //逆転(目標に達してない)
- 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);
- }
- //左側が後に出ちゃった♥(右側だけ回して左側は停止)
- 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);
- back_migiusiro.setOutputLimits(0x7C, 0x83);
- back_hidarimae.setOutputLimits(0x7C, 0x83);
- back_hidariusiro.setOutputLimits(0x7C, 0x83);
- }
-
- //よくわからんやつ
- back_migimae.setMode(AUTO_MODE);
- back_migiusiro.setMode(AUTO_MODE);
- back_hidarimae.setMode(AUTO_MODE);
- back_hidariusiro.setMode(AUTO_MODE);
-
- //目標値
- back_migimae.setSetPoint(target*-1);
- back_migiusiro.setSetPoint(target*-1);
- back_hidarimae.setSetPoint(target*-1);
- back_hidariusiro.setSetPoint(target*-1);
-
- //センサ出力
- back_migimae.setProcessValue(y_pulse1*-1);
- back_migiusiro.setProcessValue(y_pulse1*-1);
- back_hidarimae.setProcessValue(y_pulse2*-1);
- back_hidariusiro.setProcessValue(y_pulse2*-1);
-
- //制御量(計算結果)
- migimae_data[0] = back_migimae.compute();
- migiusiro_data[0] = back_migiusiro.compute();
- hidarimae_data[0] = back_hidarimae.compute();
- hidariusiro_data[0] = back_hidariusiro.compute();
-
- //制御量をPWM値に変換
- //逆転(目標に達してない)
- 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 + 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;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- }
+ //制御量をPWM値に変換
+ //右進(目標まで達していない)
+ 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 > (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;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
}
-void right_PID(int target) {
-
- //センサ出力値の最小、最大
- right_migimae.setInputLimits(-2147483648, 2147483647);
- right_migiusiro.setInputLimits(-2147483648, 2147483647);
- right_hidarimae.setInputLimits(-2147483648, 2147483647);
- right_hidariusiro.setInputLimits(-2147483648, 2147483647);
+void left_PID(int target)
+{
- //制御量の最小、最大
- //右進(目標まで達していない)
- 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, 0xFF);
- right_hidariusiro.setOutputLimits(0x00, 0x7B);
+ //センサ出力値の最小、最大
+ left_migimae.setInputLimits(-2147483648, 2147483647);
+ left_migiusiro.setInputLimits(-2147483648, 2147483647);
+ left_hidarimae.setInputLimits(-2147483648, 2147483647);
+ left_hidariusiro.setInputLimits(-2147483648, 2147483647);
- }
- //前側が右に出ちゃった♥(後側だけ回して前側は停止)
- 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);
- right_migiusiro.setOutputLimits(0x7C, 0x83);
- right_hidarimae.setOutputLimits(0x7C, 0x83);
- right_hidariusiro.setOutputLimits(0x7C, 0x83);
- }
+ //制御量の最小、最大
+ //左進(目標まで達していない)
+ 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 > (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)) {
+ left_migimae.setOutputLimits(0x7C, 0x83);
+ left_migiusiro.setOutputLimits(0x7C, 0x83);
+ left_hidarimae.setOutputLimits(0x7C, 0x83);
+ left_hidariusiro.setOutputLimits(0x7C, 0x83);
+ }
- //よくわからんやつ
- right_migimae.setMode(AUTO_MODE);
- right_migiusiro.setMode(AUTO_MODE);
- right_hidarimae.setMode(AUTO_MODE);
- right_hidariusiro.setMode(AUTO_MODE);
+ //よくわからんやつ
+ left_migimae.setMode(AUTO_MODE);
+ left_migiusiro.setMode(AUTO_MODE);
+ left_hidarimae.setMode(AUTO_MODE);
+ left_hidariusiro.setMode(AUTO_MODE);
- //目標値
- right_migimae.setSetPoint(target*-1);
- right_migiusiro.setSetPoint(target*-1);
- right_hidarimae.setSetPoint(target*-1);
- right_hidariusiro.setSetPoint(target*-1);
+ //目標値
+ left_migimae.setSetPoint(target);
+ left_migiusiro.setSetPoint(target);
+ left_hidarimae.setSetPoint(target);
+ left_hidariusiro.setSetPoint(target);
- //センサ出力
- right_migimae.setProcessValue(x_pulse1*-1);
- right_migiusiro.setProcessValue(x_pulse2*-1);
- right_hidarimae.setProcessValue(x_pulse1*-1);
- right_hidariusiro.setProcessValue(x_pulse2*-1);
+ //センサ出力
+ left_migimae.setProcessValue(x_pulse1);
+ left_migiusiro.setProcessValue(x_pulse2);
+ left_hidarimae.setProcessValue(x_pulse1);
+ left_hidariusiro.setProcessValue(x_pulse2);
- //制御量(計算結果)
- migimae_data[0] = right_migimae.compute();
- migiusiro_data[0] = right_migiusiro.compute();
- hidarimae_data[0] = right_hidarimae.compute();
- hidariusiro_data[0] = right_hidariusiro.compute();
+ //制御量(計算結果)
+ migimae_data[0] = left_migimae.compute();
+ migiusiro_data[0] = left_migiusiro.compute();
+ hidarimae_data[0] = left_hidarimae.compute();
+ hidariusiro_data[0] = left_hidariusiro.compute();
- //制御量をPWM値に変換
- //右進(目標まで達していない)
- 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 > (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;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- }
+ //制御量をPWM値に変換
+ //左進(目標まで達していない)
+ 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)) {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
}
-void left_PID(int target) {
+void turn_right_PID(int target)
+{
- //センサ出力値の最小、最大
- left_migimae.setInputLimits(-2147483648, 2147483647);
- left_migiusiro.setInputLimits(-2147483648, 2147483647);
- left_hidarimae.setInputLimits(-2147483648, 2147483647);
- left_hidariusiro.setInputLimits(-2147483648, 2147483647);
+ //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
+ turn_right_migimae.setInputLimits(-2147483648, 2147483647);
+ turn_right_migiusiro.setInputLimits(-2147483648, 2147483647);
+ turn_right_hidarimae.setInputLimits(-2147483648, 2147483647);
+ turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);
- //制御量の最小、最大
- //左進(目標まで達していない)
- 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 > (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)) {
- left_migimae.setOutputLimits(0x7C, 0x83);
- left_migiusiro.setOutputLimits(0x7C, 0x83);
- left_hidarimae.setOutputLimits(0x7C, 0x83);
- left_hidariusiro.setOutputLimits(0x7C, 0x83);
- }
+ //制御量の最小、最大
+ //右旋回(目標に達してない)
+ if(x_pulse2 < 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) {
+ turn_right_migimae.setOutputLimits(0x7C, 0x83);
+ turn_right_migiusiro.setOutputLimits(0x7C, 0x83);
+ turn_right_hidarimae.setOutputLimits(0x7C, 0x83);
+ turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
+ }
- //よくわからんやつ
- left_migimae.setMode(AUTO_MODE);
- left_migiusiro.setMode(AUTO_MODE);
- left_hidarimae.setMode(AUTO_MODE);
- left_hidariusiro.setMode(AUTO_MODE);
+ //よくわからんやつ
+ turn_right_migimae.setMode(AUTO_MODE);
+ turn_right_migiusiro.setMode(AUTO_MODE);
+ turn_right_hidarimae.setMode(AUTO_MODE);
+ turn_right_hidariusiro.setMode(AUTO_MODE);
- //目標値
- left_migimae.setSetPoint(target);
- left_migiusiro.setSetPoint(target);
- left_hidarimae.setSetPoint(target);
- left_hidariusiro.setSetPoint(target);
+ //目標値
+ turn_right_migimae.setSetPoint(target);
+ turn_right_migiusiro.setSetPoint(target);
+ turn_right_hidarimae.setSetPoint(target);
+ turn_right_hidariusiro.setSetPoint(target);
- //センサ出力
- left_migimae.setProcessValue(x_pulse1);
- left_migiusiro.setProcessValue(x_pulse2);
- left_hidarimae.setProcessValue(x_pulse1);
- left_hidariusiro.setProcessValue(x_pulse2);
-
- //制御量(計算結果)
- migimae_data[0] = left_migimae.compute();
- migiusiro_data[0] = left_migiusiro.compute();
- hidarimae_data[0] = left_hidarimae.compute();
- hidariusiro_data[0] = left_hidariusiro.compute();
+ //センサ出力
+ turn_right_migimae.setProcessValue(x_pulse2);
+ turn_right_migiusiro.setProcessValue(x_pulse2);
+ turn_right_hidarimae.setProcessValue(x_pulse2);
+ turn_right_hidariusiro.setProcessValue(x_pulse2);
- //制御量をPWM値に変換
- //左進(目標まで達していない)
- 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)) {
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- }
+ //制御量(計算結果)
+ migimae_data[0] = turn_right_migimae.compute();
+ migiusiro_data[0] = turn_right_migiusiro.compute();
+ hidarimae_data[0] = turn_right_hidarimae.compute();
+ hidariusiro_data[0] = turn_right_hidariusiro.compute();
+
+ //制御量をPWM値に変換
+ //右旋回(目標に達してない)
+ if(x_pulse2 < 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) {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
}
-void turn_right_PID(int target) {
+void turn_left_PID(int target)
+{
- //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
- turn_right_migimae.setInputLimits(-2147483648, 2147483647);
- turn_right_migiusiro.setInputLimits(-2147483648, 2147483647);
- turn_right_hidarimae.setInputLimits(-2147483648, 2147483647);
- turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);
+ //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
+ turn_left_migimae.setInputLimits(-2147483648, 2147483647);
+ turn_left_migiusiro.setInputLimits(-2147483648, 2147483647);
+ turn_left_hidarimae.setInputLimits(-2147483648, 2147483647);
+ turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);
- //制御量の最小、最大
- //右旋回(目標に達してない)
- if(x_pulse2 < 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) {
- turn_right_migimae.setOutputLimits(0x7C, 0x83);
- turn_right_migiusiro.setOutputLimits(0x7C, 0x83);
- turn_right_hidarimae.setOutputLimits(0x7C, 0x83);
- turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
- }
+ //制御量の最小、最大
+ //左旋回(目標に達してない)
+ 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(0x7C, 0x83);
+ turn_left_migiusiro.setOutputLimits(0x7C, 0x83);
+ turn_left_hidarimae.setOutputLimits(0x7C, 0x83);
+ turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
+ }
- //よくわからんやつ
- turn_right_migimae.setMode(AUTO_MODE);
- turn_right_migiusiro.setMode(AUTO_MODE);
- turn_right_hidarimae.setMode(AUTO_MODE);
- turn_right_hidariusiro.setMode(AUTO_MODE);
+ //よくわからんやつ
+ turn_left_migimae.setMode(AUTO_MODE);
+ turn_left_migiusiro.setMode(AUTO_MODE);
+ turn_left_hidarimae.setMode(AUTO_MODE);
+ turn_left_hidariusiro.setMode(AUTO_MODE);
- //目標値
- turn_right_migimae.setSetPoint(target);
- turn_right_migiusiro.setSetPoint(target);
- turn_right_hidarimae.setSetPoint(target);
- turn_right_hidariusiro.setSetPoint(target);
+ //目標値
+ turn_left_migimae.setSetPoint(target);
+ turn_left_migiusiro.setSetPoint(target);
+ turn_left_hidarimae.setSetPoint(target);
+ turn_left_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_left_migimae.setProcessValue(x_pulse1);
+ turn_left_migiusiro.setProcessValue(x_pulse1);
+ turn_left_hidarimae.setProcessValue(x_pulse1);
+ turn_left_hidariusiro.setProcessValue(x_pulse1);
- //制御量(計算結果)
- migimae_data[0] = turn_right_migimae.compute();
- migiusiro_data[0] = turn_right_migiusiro.compute();
- hidarimae_data[0] = turn_right_hidarimae.compute();
- hidariusiro_data[0] = turn_right_hidariusiro.compute();
+ //制御量(計算結果)
+ migimae_data[0] = turn_left_migimae.compute();
+ migiusiro_data[0] = turn_left_migiusiro.compute();
+ hidarimae_data[0] = turn_left_hidarimae.compute();
+ hidariusiro_data[0] = turn_left_hidariusiro.compute();
- //制御量をPWM値に変換
- //右旋回(目標に達してない)
- if(x_pulse2 < 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) {
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- }
+ //制御量をPWM値に変換
+ //左旋回(目標に達してない)
+ if(x_pulse1 < 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) {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
}
-void turn_left_PID(int target) {
-
- //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
- turn_left_migimae.setInputLimits(-2147483648, 2147483647);
- turn_left_migiusiro.setInputLimits(-2147483648, 2147483647);
- turn_left_hidarimae.setInputLimits(-2147483648, 2147483647);
- 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(0x7C, 0x83);
- turn_left_migiusiro.setOutputLimits(0x7C, 0x83);
- turn_left_hidarimae.setOutputLimits(0x7C, 0x83);
- turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
- }
-
- //よくわからんやつ
- turn_left_migimae.setMode(AUTO_MODE);
- turn_left_migiusiro.setMode(AUTO_MODE);
- turn_left_hidarimae.setMode(AUTO_MODE);
- turn_left_hidariusiro.setMode(AUTO_MODE);
-
- //目標値
- turn_left_migimae.setSetPoint(target);
- turn_left_migiusiro.setSetPoint(target);
- turn_left_hidarimae.setSetPoint(target);
- 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);
-
- //制御量(計算結果)
- migimae_data[0] = turn_left_migimae.compute();
- migiusiro_data[0] = turn_left_migiusiro.compute();
- hidarimae_data[0] = turn_left_hidarimae.compute();
- hidariusiro_data[0] = turn_left_hidariusiro.compute();
+void dondonkasoku(void)
+{
- //制御量をPWM値に変換
- //左旋回(目標に達してない)
- if(x_pulse1 < 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) {
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- }
+ //どんどん加速(正転)
+ 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);
+ }
}
-
-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);
- }
-}
diff -r ac4954be1fe0 -r 123520676121 mbed.bld --- a/mbed.bld Wed Sep 18 03:36:25 2019 +0000 +++ b/mbed.bld Thu Sep 19 07:35:49 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file