2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
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