2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
Diff: main.cpp
- Revision:
- 20:ac4954be1fe0
- Parent:
- 19:f17d2e585973
- Child:
- 21:123520676121
--- a/main.cpp Tue Sep 17 01:33:16 2019 +0000
+++ b/main.cpp Wed Sep 18 03:36:25 2019 +0000
@@ -105,6 +105,7 @@
char arm_moter[1], drop_moter[1];
char fan_data[1];
char servo_data[1];
+char right_arm_data[1], left_arm_data[1];
//非常停止関連変数
char RDATA;
@@ -123,26 +124,32 @@
int right_limit = 0;
//後部が壁に当たっているか
int back_limit = 0;
+//回収機構の下限(引っ込めてるほう)
+bool kaisyu_limit = 0;
+//右腕の下限
+bool right_arm_lower_limit = 0;
//右腕の上限
bool right_arm_upper_limit = 0;
-//右腕の下限
-bool right_arm_lower_limit = 0;
-//左腕の上限
-bool left_arm_upper_limit = 0;
//左腕の下限
bool left_arm_lower_limit = 0;
-//吐き出し機構の下限
-bool tyokudo_usiro = 0;
+//左腕の上限
+bool left_arm_upper_limit = 0;
//吐き出し機構の上限
-bool tyokudo_mae = 0;
-//回収機構の上限
-bool kaisyu_mae = 0;
-//回収機構の下限
-bool kaisyu_usiro = 0;
+bool tyokudo_mae_limit = 0;
+//吐き出し機構の下限
+bool tyokudo_usiro_limit = 0;
+
-int masked_lower_front_limit_data = 0;
-int masked_lower_back_limit_data = 0;
-int masked_lower_right_limit_data = 0;
+int masked_lower_front_limit_data = 0xFF;
+int masked_lower_back_limit_data = 0xFF;
+int masked_lower_right_limit_data = 0xFF;
+int masked_kaisyu_limit_data = 0xFF;
+int masked_right_arm_lower_limit_data = 0xFF;
+int masked_right_arm_upper_limit_data = 0xFF;
+int masked_left_arm_lower_limit_data = 0xFF;
+int masked_left_arm_upper_limit_data = 0xFF;
+int masked_tyokudo_mae_limit_data = 0xFF;
+int masked_tyokudo_usiro_limit_data = 0xFF;
//関数のプロトタイプ宣言
void init(void);
@@ -155,6 +162,7 @@
void wheel_reset(void);
void kaisyu(int pulse);
void tyokudo(int pulse);
+void arm_up(void);
void front(int target);
void back(int target);
void right(int target);
@@ -171,30 +179,31 @@
void dondonkasoku(void);
int main(void) {
-
+
init();
init_send();
-
+
//とりあえず(後で消してね)
zone = BLUE;
-
+
while(1) {
-
+
get_pulses();
print_pulses();
get_emergency();
read_limit();
+
+ //if(start_switch == 1) {
+ //tyokudo(arm_enc.getPulses());
+ //}
//青ゾーン
if(zone == BLUE) {
-
+
switch(phase) {
-
+
//スタート位置へセット
case 0:
- servo_data[0] = 0x03;
- i2c.write(0x30, servo_data, 1);
-
//リミットが洗濯物台に触れているか
if(right_limit == 3) {
USR_LED1 = 1;
@@ -207,23 +216,32 @@
USR_LED1 = 0;
}
break;
-
+
//回収
case 1:
- //ここに回収動作が来るが今回は飛ばす
- phase = 2;
+ //回収アームのリミットが押されているか
+ if(kaisyu_limit == 1) {
+ kaisyu(arm_enc.getPulses());
+ USR_LED2 = 1;
+ } else {
+ USR_LED2 = 0;
+ }
+ servo_data[0] = 0x03;
+ i2c.write(0x30, servo_data, 1);
break;
-
+
//1秒停止
case 2:
stop();
+ servo_data[0] = 0x04;
+ i2c.write(0x30, servo_data, 1);
counter.start();
if(counter.read() > 1.0f) {
phase = 3;
wheel_reset();
}
break;
-
+
//左移動
case 3:
counter.reset();
@@ -232,7 +250,7 @@
phase = 4;
}
break;
-
+
//1秒停止
case 4:
stop();
@@ -242,16 +260,16 @@
wheel_reset();
}
break;
-
+
//右旋回(180°)
case 5:
counter.reset();
- turn_right(520);
- if(x_pulse2 > 520) {
+ turn_right(518);
+ if(x_pulse2 > 518) {
phase = 6;
}
break;
-
+
//1秒停止
case 6:
stop();
@@ -261,12 +279,12 @@
wheel_reset();
}
break;
-
+
//ちょっくら右移動
case 7:
counter.reset();
right(-1000);
-
+
if((x_pulse1*-1 > 500) && (x_pulse2*-1 > 500)) {
true_migimae_data[0] = 0x94;
true_migiusiro_data[0] = 0x10;
@@ -277,7 +295,7 @@
phase = 8;
}
break;
-
+
//1秒停止
case 8:
stop();
@@ -287,20 +305,21 @@
wheel_reset();
}
break;
-
+
//排出
case 9:
counter.reset();
+ tyokudo(arm_enc.getPulses());
//ここに排出動作が来るが今回は飛ばす
- phase = 10;
+ //phase = 10;
break;
-
+
//排出機構引っ込める
case 10:
//ここに排出機構引っ込める動作が来るが今回は飛ばす
phase = 11;
break;
-
+
//1秒停止
case 11:
stop();
@@ -310,7 +329,7 @@
wheel_reset();
}
break;
-
+
//前進
case 12:
counter.reset();
@@ -319,7 +338,7 @@
phase = 13;
}
break;
-
+
//1秒停止
case 13:
stop();
@@ -329,7 +348,7 @@
wheel_reset();
}
break;
-
+
//右移動
case 14:
counter.reset();
@@ -344,7 +363,7 @@
phase = 15;
}
break;
-
+
//シーツ装填
case 15:
if(start_switch == 1) {
@@ -353,7 +372,7 @@
stop();
}
break;
-
+
//竿のラインまで前進
case 16:
counter.reset();
@@ -362,7 +381,7 @@
phase = 17;
}
break;
-
+
//1秒停止
case 17:
stop();
@@ -372,8 +391,8 @@
wheel_reset();
}
break;
-
- //掛けるところまで左移動
+
+ //掛けるところまで左移動
case 18:
counter.reset();
left(10000);
@@ -381,7 +400,7 @@
phase = 19;
}
break;
-
+
//1秒停止
case 19:
stop();
@@ -390,17 +409,17 @@
phase = 20;
wheel_reset();
}
- break;
-
+ break;
+
//妨害防止の右旋回
- case 20:
+ case 20:
counter.reset();
turn_right(300);
if(x_pulse2 > 300) {
phase = 21;
}
break;
-
+
//1秒停止
case 21:
stop();
@@ -409,19 +428,19 @@
phase = 22;
wheel_reset();
}
- break;
-
- //カウンターリセット
+ break;
+
+ //アームアップ
case 22:
stop();
counter.reset();
- phase = 23;
+ arm_up();
break;
-
+
//シーツを掛ける
case 23:
counter.start();
-
+
//5秒間ファン送風
if(counter.read() <= 4.0f) {
fan_data[0] = 0xFF;
@@ -445,13 +464,13 @@
phase = 24;
}
break;
-
+
//終了っ！(守衛さん風)
case 24:
//駆動系統OFF
emergency = 0;
break;
-
+
default:
//駆動系統OFF
emergency = 0;
@@ -465,46 +484,52 @@
//通信ボーレートの設定
pc.baud(460800);
-
+
limit_serial.baud(115200);
-
+
start_switch.mode(PullUp);
-
+
//非常停止関連
pic.baud(19200);
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;
fan_data[0] = 0x80;
servo_data[0] = 0x80;
+ right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
}
void init_send(void) {
-
+
init_send_data[0] = 0x80;
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);
+ i2c.write(0x18, init_send_data, 1);
+ i2c.write(0x20, init_send_data, 1);
+ i2c.write(0x22, init_send_data, 1);
+ i2c.write(0x24, init_send_data, 1);
+ i2c.write(0x30, init_send_data, 1);
wait(0.1);
}
void get(void) {
-
- baff = pic.getc();
-
+
+ baff = pic.getc();
+
for(; flug; flug--)
RDATA = baff;
-
+
if(baff == ':')
flug = 1;
}
void get_pulses(void) {
-
+
x_pulse1 = wheel_x1.getPulses();
x_pulse2 = wheel_x2.getPulses();
y_pulse1 = wheel_y1.getPulses();
@@ -513,14 +538,15 @@
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("upper: 0x%x, low: 0x%x, f: %d, b: %d, r: %d\n\r", upper_limit_data, lower_limit_data, front_limit, back_limit, right_limit);
//pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
//pc.printf("RF: %x, RB: %x, LF: %x, LB: %x\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]);
}
void get_emergency(void) {
-
+
if(RDATA == '1') {
myled = 1;
emergency = 1;
@@ -532,24 +558,32 @@
}
void read_limit(void) {
-
+
limit_data = limit_serial.getc();
-
+
//上位1bitが1ならば下のリミットのデータだと判断
if((limit_data & 0b10000000) == 0b10000000) {
lower_limit_data = limit_data;
- //upper_limit_data = 0b01111111;
-
+
//上位1bitが0ならば上のリミットのデータだと判断
} else {
upper_limit_data = limit_data;
- //lower_limit_data = 0b11111111;
}
-
+
+ //下リミット基板からのデータのマスク処理
masked_lower_front_limit_data = lower_limit_data & 0b00000011;
masked_lower_back_limit_data = lower_limit_data & 0b00001100;
masked_lower_right_limit_data = lower_limit_data & 0b00110000;
-
+ 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;
+
//前部リミット
switch(masked_lower_front_limit_data) {
//両方押された
@@ -568,7 +602,7 @@
front_limit = 0;
break;
}
-
+
//後部リミット
switch(masked_lower_back_limit_data) {
//両方押された
@@ -587,7 +621,7 @@
back_limit = 0;
break;
}
-
+
//右部リミット
switch(masked_lower_right_limit_data) {
//両方押された
@@ -606,68 +640,108 @@
right_limit = 0;
break;
}
-
- /*
- //回収機構の上限
- if((lower_limit_data & 0b01000000) == 0b01000000) {
- withdrawal_upper_limit = 1;
- } else {
- withdrawal_upper_limit = 0;
+
+ //回収機構リミット
+ switch(masked_kaisyu_limit_data) {
+ //押された
+ case 0b00000000:
+ kaisyu_limit = 1;
+ break;
+ case 0b01000000:
+ kaisyu_limit = 0;
+ break;
+ default:
+ kaisyu_limit = 0;
+ break;
}
-
- //回収機構の下限
- if((lower_limit_data & 0b10000000) == 0b10000000) {
- withdrawal_lower_limit = 1;
- } else {
- withdrawal_lower_limit = 0;
+
+ //右腕下部リミット
+ 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;
}
-
- //右腕の上限
- if((upper_limit_data & 0b00000001) == 0b00000001) {
- right_arm_upper_limit = 1;
- } else {
- right_arm_upper_limit = 0;
- }
-
- //右腕の下限
- if((upper_limit_data & 0b00000010) == 0b00000010) {
- right_arm_lower_limit = 1;
- } else {
- right_arm_lower_limit = 0;
+
+ //右腕上部リミット
+ 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;
}
-
- //左腕の上限
- if((upper_limit_data & 0b00000100) == 0b00000100) {
- left_arm_upper_limit = 1;
- } else {
- left_arm_upper_limit = 0;
+
+ //左腕下部リミット
+ 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;
}
-
- //左腕の下限
- if((upper_limit_data & 0b00001000) == 0b00001000) {
- left_arm_lower_limit = 1;
- } else {
- left_arm_lower_limit = 0;
+
+ //左腕上部リミット
+ 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;
}
-
- //吐き出し機構の上限
- if((upper_limit_data & 0b00010000) == 0b00010000) {
- force_out_upper_limit = 1;
- } else {
- force_out_upper_limit = 0;
+
+ //直動の前
+ 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;
}
-
- //吐き出し機構の下限
- if((upper_limit_data & 0b00100000) == 0b00100000) {
- force_out_lower_limit = 1;
- } else {
- force_out_lower_limit = 0;
+
+ //直動の後
+ 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) {
-
+
wheel_x1.reset();
wheel_x2.reset();
wheel_y1.reset();
@@ -677,104 +751,186 @@
void kaisyu(int pulse) {
switch (kaisyu_phase) {
+
case 0:
- arm_moter[0] = 0x80;
- kaisyu_phase = 0;
- break;
- case 1:
//前進->減速
- if(pulse < 2000) {
+ //3000pulseまで高速前進
+ if(pulse < 3000) {
arm_moter[0] = 0xFF;
+ //kaisyu_phase = 1;
+ }
+
+ //3000pulse超えたら低速前進
+ else if(pulse >= 3000) {
+ arm_moter[0] = 0xB3;
kaisyu_phase = 1;
- } else {
- arm_moter[0] = 0xB3;
- kaisyu_phase = 2;
}
break;
- case 2:
+
+ case 1:
//前進->停止->後進
- if(kaisyu_usiro == 1) {
+ //3600pulseまで低速前進
+ if(pulse < 3600) {
arm_moter[0] = 0xB3;
- kaisyu_phase = 2;
- } else {
- arm_moter[0] = 0x80;
+ //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まで高速後進
+ counter.reset();
+ if(pulse > 500) {
+ arm_moter[0] = 0x00;
+ //kaisyu_phase = 3;
+
+ }
+ //500pulse以下になったら低速後進
+ else if(pulse <= 500) {
+ arm_moter[0] = 0x4C;
kaisyu_phase = 3;
- i2c.write(0x18, arm_moter,1);
- wait(1);
}
break;
+
case 3:
- //後進->減速
- if(pulse > 1600) {
- arm_moter[0] = 0x00;
- kaisyu_phase = 3;
- } else {
+ //後進->停止
+ //リミット押されるまで低速後進
+ if(pulse <= 500) {
arm_moter[0] = 0x4C;
+ //kaisyu_phase = 4;
+ }
+
+ //リミット押されたら停止
+ if(kaisyu_limit == 1) {
+ arm_moter[0] = 0x80;
kaisyu_phase = 4;
+ phase = 2;
}
break;
- case 4:
- //後進->停止
- if(kaisyu_mae == 1) {
- arm_moter[0] = 0x4C;
- kaisyu_phase = 4;
- } else {
- arm_moter[0] = 0x80;
- kaisyu_phase = 0;
- phase = 1;
- }
- break;
+
default:
+ arm_moter[0] = 0x80;
break;
}
+ //回収MDへ書き込み
i2c.write(0x18, arm_moter, 1);
}
void tyokudo(int pulse) {
-
- switch (tyokudo_phase) {
+
+ switch(tyokudo_phase) {
+
case 0:
- arm_moter[0] = 0x80;
- drop_moter[0] = 0x80;
- break;
- case 1:
//前進->減速
- arm_moter[0] = (pulse < 2000)? 0xCD:0xC0;
- drop_moter[0] = (pulse < 2000)? 0xE6:0xCD;
- tyokudo_phase = (pulse < 2000)? 1:2;
- break;
- case 2:
- //前進(遅い)->停止->後進(早い)
- if(tyokudo_mae == 1) {
- arm_moter[0] = 0xC0;
- drop_moter[0] = 0xCD;
- tyokudo_phase = 2;
- } else {
- arm_moter[0] = 0x80;
+
+ //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
+ if(tyokudo_mae_limit == 0) {
+ //2000pulseまで高速前進
+ if(pulse < 2000) {
+ arm_moter[0] = 0xCD;
+ 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;
- tyokudo_phase = 3;
- i2c.write(0x18, arm_moter,1);
- i2c.write(0x20, drop_moter,1);
- wait(1);
+
+ //2000pulseまで高速前進
+ if(pulse < 2000) {
+ arm_moter[0] = 0xCD;
+ }
+ //2000pulse以上で低速前進
+ else if(pulse >= 2000) {
+ arm_moter[0] = 0xC0;
+ }
+ //パルスが3600を終えたらアームも強制停止
+ else if(pulse > 3600) {
+ arm_moter[0] = 0x80;
+ tyokudo_phase = 1;
+ }
}
break;
- case 3:
+
+ case 1:
//後進->減速
- arm_moter[0] = (pulse > 1600)? 0x33:0x33;
- drop_moter[0] = (pulse > 1600)? 0x19:0x19;
- tyokudo_phase = (pulse > 1600)? 3:4;
+
+ //1600pulseより大きいとき高速後進
+ if(pulse > 1600) {
+
+ if(kaisyu_limit == 0) {
+ arm_moter[0] = 0x00;
+ }
+ //リミットが押されたら強制停止
+ else if(kaisyu_limit == 1) {
+ arm_moter[0] = 0x80;
+ }
+
+ if(tyokudo_usiro_limit == 0) {
+ drop_moter[0] = 0x19;
+ }
+ //リミットが押されたら強制停止
+ else if(tyokudo_usiro_limit == 1) {
+ drop_moter[0] = 0x80;
+ }
+ }
+ //500pulse以下でphase2へ移行
+ else if(pulse <= 500) {
+ tyokudo_phase = 2;
+ }
break;
- case 4:
+
+ case 2:
//後進->停止
- arm_moter[0] = (tyokudo_usiro == 1)? 0x33:0x80;
- drop_moter[0] = (tyokudo_usiro == 1)? 0x19:0x80;
- tyokudo_phase = (tyokudo_usiro == 1)? 4:0;
+
+ //直動後リミットが押されるまで後進
+ if(tyokudo_usiro_limit == 0) {
+ drop_moter[0] = 0x19;
+ }
+ //直動後リミットが押されたら停止
+ else if(tyokudo_usiro_limit == 1) {
+ drop_moter[0] = 0x80;
+ }
+
+ //リミット押されるまで低速後進
+ if(kaisyu_limit == 0) {
+ arm_moter[0] = 0x4C;
+ }
+ else if(kaisyu_limit == 1) {
+ arm_moter[1] = 0x80;
+ tyokudo_phase = 3;
+ phase = 10;
+ }
+
break;
+
default:
arm_moter[0] = 0x80;
drop_moter[0] = 0x80;
- tyokudo_phase = 0;
break;
}
@@ -782,8 +938,32 @@
i2c.write(0x20, drop_moter, 1);
}
+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;
+ }
+ //右腕のみリミットが押されたら左腕のみ上昇
+ else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
+ 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;
+ }
+ //両腕、上限リミットが押されたら停止
+ else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
+ 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) {
-
+
front_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -793,7 +973,7 @@
}
void back(int target) {
-
+
back_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -803,7 +983,7 @@
}
void right(int target) {
-
+
right_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -813,7 +993,7 @@
}
void left(int target) {
-
+
left_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -823,7 +1003,7 @@
}
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);
@@ -833,7 +1013,7 @@
}
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);
@@ -843,12 +1023,12 @@
}
void stop(void) {
-
+
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
- true_hidariusiro_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);
@@ -952,7 +1132,7 @@
}
void back_PID(int target) {
-
+
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
back_migimae.setInputLimits(-2147483648, 2147483647);
back_migiusiro.setInputLimits(-2147483648, 2147483647);
@@ -1143,7 +1323,7 @@
}
void left_PID(int target) {
-
+
//センサ出力値の最小、最大
left_migimae.setInputLimits(-2147483648, 2147483647);
left_migiusiro.setInputLimits(-2147483648, 2147483647);
@@ -1179,7 +1359,7 @@
left_hidarimae.setOutputLimits(0x7C, 0x83);
left_hidariusiro.setOutputLimits(0x7C, 0x83);
}
-
+
//よくわからんやつ
left_migimae.setMode(AUTO_MODE);
left_migiusiro.setMode(AUTO_MODE);
@@ -1301,7 +1481,7 @@
}
void turn_left_PID(int target) {
-
+
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
turn_left_migimae.setInputLimits(-2147483648, 2147483647);
turn_left_migiusiro.setInputLimits(-2147483648, 2147483647);