2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
Diff: main.cpp
- Revision:
- 17:de3bc1999ae7
- Parent:
- 16:05b26003da50
- Child:
- 18:851f783ec516
diff -r 05b26003da50 -r de3bc1999ae7 main.cpp
--- a/main.cpp Sun Sep 01 12:12:55 2019 +0000
+++ b/main.cpp Tue Sep 03 05:47:40 2019 +0000
@@ -4,7 +4,7 @@
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
/* Sensor: encorder*4 */
/* ------------------------------------------------------------------- */
-/* 4方向の直進補正をしてみた */
+/* 遠隔非常停止対応 & 移動時のバグを改善と */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
@@ -38,28 +38,28 @@
PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//右進
-PID right_migimae(10000000.0, 0.0, 0.0, 0.001);
-PID right_migiusiro(10000000.0, 0.0, 0.0, 0.001);
-PID right_hidarimae(10000000.0, 0.0, 0.0, 0.001);
-PID right_hidariusiro(10000000.0, 0.0, 0.0, 0.001);
+PID right_migimae(6000000.0, 0.0, 0.0, 0.001);
+PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001);
+PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001);
+PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
//左進
-PID left_migimae(10000000.0, 0.0, 0.0, 0.001);
-PID left_migiusiro(10000000.0, 0.0, 0.0, 0.001);
-PID left_hidarimae(10000000.0, 0.0, 0.0, 0.001);
-PID left_hidariusiro(10000000.0, 0.0, 0.0, 0.001);
+PID left_migimae(6000000.0, 0.0, 0.0, 0.001);
+PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001);
+PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001);
+PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
//右旋回
-PID turn_right_migimae(4500000.0, 0.0, 0.0, 0.001);
-PID turn_right_migiusiro(4500000.0, 0.0, 0.0, 0.001);
-PID turn_right_hidarimae(4500000.0, 0.0, 0.0, 0.001);
-PID turn_right_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
+PID turn_right_migimae(18000000.0, 0.0, 0.0, 0.001);
+PID turn_right_migiusiro(18000000.0, 0.0, 0.0, 0.001);
+PID turn_right_hidarimae(18000000.0, 0.0, 0.0, 0.001);
+PID turn_right_hidariusiro(18000000.0, 0.0, 0.0, 0.001);
//左旋回
-PID turn_left_migimae(4500000.0, 0.0, 0.0, 0.001);
-PID turn_left_migiusiro(4500000.0, 0.0, 0.0, 0.001);
-PID turn_left_hidarimae(4500000.0, 0.0, 0.0, 0.001);
-PID turn_left_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
+PID turn_left_migimae(18000000.0, 0.0, 0.0, 0.001);
+PID turn_left_migiusiro(18000000.0, 0.0, 0.0, 0.001);
+PID turn_left_hidarimae(18000000.0, 0.0, 0.0, 0.001);
+PID turn_left_hidariusiro(18000000.0, 0.0, 0.0, 0.001);
//MDとの通信ポート
I2C i2c(PB_9, PB_8); //SDA, SCL
@@ -67,6 +67,9 @@
//PCとの通信ポート
Serial pc(USBTX, USBRX); //TX, RX
+//特小モジュールとの通信ポート
+Serial pic(A0, A1);
+
//12V停止信号ピン
DigitalOut emergency(D11);
@@ -75,8 +78,10 @@
DigitalOut USR_LED3(PC_2);
DigitalOut USR_LED4(PC_3);
+//遠隔非常停止ユニットLED
+AnalogOut myled(A2);
+
DigitalIn start_switch(PB_12);
-//InterruptIn start_switch(PB_12);
QEI wheel_x1(PA_8 , PA_6 , NC, 624);
QEI wheel_x2(PB_14, PB_13, NC, 624);
@@ -85,7 +90,9 @@
//QEI wheel1(D2, D3, NC, 624);
//QEI wheel2(D5, D4, NC, 624);
-Ticker look_switch;
+Ticker look_switch;
+
+Timer counter;
//エンコーダ値格納変数
int x_pulse1, x_pulse2, y_pulse1, y_pulse2;
@@ -100,24 +107,31 @@
char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
+//非常停止関連変数
+char RDATA;
+char baff;
+int flug = 0;
+
//関数のプロトタイプ宣言
void incriment(void);
void init(void);
void init_send(void);
+void get(void);
void get_pulses(void);
void print_pulses(void);
-void front(unsigned int target);
-void back(unsigned int target);
-void right(unsigned int target);
-void left(unsigned int target);
-void turn_right(unsigned int target);
-void turn_left(unsigned int target);
-void front_PID(unsigned int target);
-void back_PID(unsigned int target);
-void right_PID(unsigned int target);
-void left_PID(unsigned int target);
-void turn_right_PID(unsigned int target);
-void turn_left_PID(unsigned int target);
+void get_emergency(void);
+void front(int target);
+void back(int target);
+void right(int target);
+void left(int target);
+void turn_right(int target);
+void turn_left(int target);
+void front_PID(int target);
+void back_PID(int target);
+void right_PID(int target);
+void left_PID(int target);
+void turn_right_PID(int target);
+void turn_left_PID(int target);
void dondonkasoku(void);
int main(void) {
@@ -126,77 +140,69 @@
init_send();
//look_switch.attach_us(&incriment, 100000.0);
- //start_switch.fall(&incriment);
+ /*
while(1) {
if(!start_switch)
break;
}
+ */
while(1) {
get_pulses();
print_pulses();
+ get_emergency();
if(start_switch == 0) {
USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1;
} else {
USR_LED1 = 0; USR_LED2 = 0; USR_LED3 = 0; USR_LED4 = 0;
}
-
- if(phase == 0) {
- front(10000);
+
+ //front(5000);
+ //back(-5000);
+ //right(-5000);
+ //left(5000);
+ //turn_right(550);
+ //turn_left(600);
+
+ if(counter.read() < 5.00f) {
+ counter.start();
+ front(1000);
}
- else if(phase == 1) {
- right(10000);
- }
- else if(phase == 2) {
- back(10000);
- }
- else if(phase == 3) {
- left(10000);
+ else if(counter.read() >= 5.00f && counter.read() < 10.00f) {
+ right(-1000);
}
-
- /*
- switch(phase) {
- case 0:
- turn_right(600);
- break;
- case 1:
- right(14000);
- break;
- case 2:
- front(5000);
- break;
- case 3:
- right(5000);
- break;
- case 4:
- front(20000);
- break;
- default:
- break;
+ else if(counter.read() >= 10.00f && counter.read() < 15.00f) {
+ back(-1000);
+ }
+ else if(counter.read() >= 15.00f && counter.read() < 20.00f) {
+ left(1000);
}
- */
-
- //back(5000);
- //right(14000);
- //left(14000); //直進補正済
- //turn_right(600);
- //turn_left(300);
+ else if(counter.read() >= 20.00f && counter.read() < 25.00f) {
+ turn_right(550);
+ }
+ else if(counter.read() >= 25.00f && counter.read() < 30.00f) {
+ turn_left(600);
+ }
+ else if(counter.read() >= 30.00f) {
+ counter.reset();
+ }
}
}
void incriment(void) {
+
if(start_switch == 0) {
phase++;
}
}
+
void init(void) {
//緊急停止用信号ピンをLow
- emergency = 0;
- //USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1;
+ //emergency = 0;
//通信ボーレートの設定
pc.baud(460800);
@@ -204,6 +210,11 @@
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;
@@ -219,6 +230,17 @@
wait(0.1);
}
+void get(void) {
+
+ baff = pic.getc();
+
+ for(; flug; flug--)
+ RDATA = baff;
+
+ if(baff == ':')
+ flug = 1;
+}
+
void get_pulses(void) {
x_pulse1 = wheel_x1.getPulses();
@@ -228,14 +250,26 @@
}
void print_pulses(void) {
-
- //pc.printf("phase: %d\n\r", phase);
- //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, p: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
+
+ //pc.printf("%f %d\n\r", counter.read(), phase);
+ //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2);
//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 front(unsigned int target) {
+void get_emergency(void) {
+ if(RDATA == '1') {
+ myled = 1;
+ emergency = 1;
+ }
+ else if(RDATA == '9'){
+ myled = 0.2;
+ emergency = 0;
+ }
+}
+
+void front(int target) {
+
front_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -244,7 +278,7 @@
wait_us(20);
}
-void back(unsigned int target) {
+void back(int target) {
back_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
@@ -254,7 +288,7 @@
wait_us(20);
}
-void right(unsigned int target) {
+void right(int target) {
right_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
@@ -264,7 +298,7 @@
wait_us(20);
}
-void left(unsigned int target) {
+void left(int target) {
left_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
@@ -274,7 +308,7 @@
wait_us(20);
}
-void turn_right(unsigned int target) {
+void turn_right(int target) {
turn_right_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
@@ -284,7 +318,7 @@
wait_us(20);
}
-void turn_left(unsigned int target) {
+void turn_left(int target) {
turn_left_PID(target);
i2c.write(0x10, true_migimae_data, 1, false);
@@ -294,7 +328,7 @@
wait_us(20);
}
-void front_PID(unsigned int target) {
+void front_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
front_migimae.setInputLimits(-2147483648, 2147483647);
@@ -304,22 +338,32 @@
//制御量の最小、最大
//正転(目標に達してない)
- if((y_pulse1 < target) || (y_pulse2 < target)) {
+ if((y_pulse1 < target) && (y_pulse2 < target)) {
front_migimae.setOutputLimits(0x84, 0xF7);
front_migiusiro.setOutputLimits(0x84, 0xF7);
front_hidarimae.setOutputLimits(0x84, 0xFF);
front_hidariusiro.setOutputLimits(0x84, 0xFF);
}
- //逆転(目標より行き過ぎ)
- else if((y_pulse1 > target) || (y_pulse2 > target)) {
- //front_migimae.setOutputLimits(0x00, /*0x7B*/0x79);
- //front_migiusiro.setOutputLimits(0x00, /*0x7B*/0x76);
- //front_hidarimae.setOutputLimits(0x00, /*0x7B*/0x79);
- //front_hidariusiro.setOutputLimits(0x00, /*0x7B*/0x79);
+ //左側が前に出ちゃった♥(右側だけ回して左側は停止)
+ else if((y_pulse1 < target) && (y_pulse2 > target)) {
+ front_migimae.setOutputLimits(0x84, 0xF7);
+ front_migiusiro.setOutputLimits(0x84, 0xF7);
+ front_hidarimae.setOutputLimits(0x7C, 0x83);
+ front_hidariusiro.setOutputLimits(0x7C, 0x83);
+ }
+ //右側が前に出ちゃった♥(左側だけ回して右側は停止)
+ 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_hidarimae.setOutputLimits(0x84, 0xFF);
+ front_hidariusiro.setOutputLimits(0x84, 0xFF);
+ }
+ //逆転(目標より行き過ぎ)
+ else if((y_pulse1 > target) && (y_pulse2 > target)) {
+ front_migimae.setOutputLimits(0x00, 0x7B);
+ front_migiusiro.setOutputLimits(0x00, 0x7B);
+ front_hidarimae.setOutputLimits(0x00, 0x73);
+ front_hidariusiro.setOutputLimits(0x00, 0x73);
}
//よくわからんやつ
@@ -348,31 +392,36 @@
//制御量をPWM値に変換
//正転(目標に達してない)
- if((y_pulse1 < target) || (y_pulse2 < target)) {
+ 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 > target) || (y_pulse2 > target)) {
- //true_migimae_data[0] = 0x79 - migimae_data[0];
- //true_migiusiro_data[0] = 0x76 - migiusiro_data[0];
- //true_hidarimae_data[0] = 0x79 - hidarimae_data[0];
- //true_hidariusiro_data[0] = 0x79 - hidariusiro_data[0];
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- } else {
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
+ //左側が前に出ちゃった♥(右側だけ回して左側は停止)
+ else 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] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
+ //右側が前に出ちゃった♥(左側だけ回して右側は停止)
+ else if((y_pulse1 > target) && (y_pulse2 < target)) {
+ 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] = 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];
+ }
}
-void back_PID(unsigned int target) {
+void back_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
back_migimae.setInputLimits(-2147483648, 2147483647);
@@ -382,22 +431,32 @@
//制御量の最小、最大
//逆転(目標に達してない)
- if((abs(y_pulse1) < target) || (abs(y_pulse2) < target)) {
+ 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);
}
- //正転(目標より行き過ぎ)
- else if((abs(y_pulse1) > target) || (abs(y_pulse2) > target)) {
- //back_migimae.setOutputLimits(0x84, 0xFF);
- //back_migiusiro.setOutputLimits(0x84, 0xFF);
- //back_hidarimae.setOutputLimits(0x84, 0xFF);
- //back_hidariusiro.setOutputLimits(0x84, 0xFF);
+ //左側が後に出ちゃった♥(右側だけ回して左側は停止)
+ else if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 > target*-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 > 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_hidarimae.setOutputLimits(0x00, 0x73);
+ back_hidariusiro.setOutputLimits(0x00, 0x73);
+ }
+ //正転(目標より行き過ぎ)
+ else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
+ back_migimae.setOutputLimits(0x84, 0xF7);
+ back_migiusiro.setOutputLimits(0x84, 0xF7);
+ back_hidarimae.setOutputLimits(0x84, 0xFF);
+ back_hidariusiro.setOutputLimits(0x84, 0xFF);
}
//よくわからんやつ
@@ -407,16 +466,16 @@
back_hidariusiro.setMode(AUTO_MODE);
//目標値
- back_migimae.setSetPoint(target);
- back_migiusiro.setSetPoint(target);
- back_hidarimae.setSetPoint(target);
- back_hidariusiro.setSetPoint(target);
+ back_migimae.setSetPoint(target*-1);
+ back_migiusiro.setSetPoint(target*-1);
+ back_hidarimae.setSetPoint(target*-1);
+ back_hidariusiro.setSetPoint(target*-1);
//センサ出力
- back_migimae.setProcessValue(abs(y_pulse1));
- back_migiusiro.setProcessValue(abs(y_pulse1));
- back_hidarimae.setProcessValue(abs(y_pulse2));
- back_hidariusiro.setProcessValue(abs(y_pulse2));
+ 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();
@@ -426,31 +485,36 @@
//制御量をPWM値に変換
//逆転(目標に達してない)
- if((abs(y_pulse1) < target) || (abs(y_pulse2) < target)) {
+ 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((abs(y_pulse1) > target) || (abs(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];
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- } else {
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
+ //左側が後に出ちゃった♥(右側だけ回して左側は停止)
+ else 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] = 0x80;
true_hidariusiro_data[0] = 0x80;
}
+ //右側が後に出ちゃった♥(左側だけ回して右側は停止)
+ 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] = 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] = migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+ }
}
-void right_PID(unsigned int target) {
+void right_PID(int target) {
//センサ出力値の最小、最大
right_migimae.setInputLimits(-2147483648, 2147483647);
@@ -460,24 +524,33 @@
//制御量の最小、最大
//右進(目標まで達していない)
- if((abs(x_pulse1) < target) || (abs(x_pulse2) < target)) {
+ if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
right_migimae.setOutputLimits(0x00, 0x6C);
right_migiusiro.setOutputLimits(0x84, 0xFF);
right_hidarimae.setOutputLimits(0x84, 0xF0);
right_hidariusiro.setOutputLimits(0x00, 0x7B);
- //right_migiusiro.setOutputLimits(0x84, 0xF1);
- //right_hidariusiro.setOutputLimits(0x0E, 0x7B);
+
+ }
+ //前側が右に出ちゃった♥(後側だけ回して前側は停止)
+ else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 < target*-1)) {
+ 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 < target*-1) && (x_pulse2*-1 > target*-1)) {
+ right_migimae.setOutputLimits(0x84, 0xED);
+ right_migiusiro.setOutputLimits(0x7C, 0x83);
+ right_hidarimae.setOutputLimits(0x00, 0x69);
+ right_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//左進(目標より行き過ぎ)
- else if((abs(x_pulse1) > target) || (abs(x_pulse2) > target)) {
- //right_migimae.setOutputLimits(0x84, 0xFF);
- //right_migiusiro.setOutputLimits(0x00, 0x7B);
- //right_hidarimae.setOutputLimits(0x00, 0x7B);
- //right_hidariusiro.setOutputLimits(0x84, 0xFF);
- right_migimae.setOutputLimits(0x7C, 0x83);
- right_migiusiro.setOutputLimits(0x7C, 0x83);
- right_hidarimae.setOutputLimits(0x7C, 0x83);
- right_hidariusiro.setOutputLimits(0x7C, 0x83);
+ else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
+ right_migimae.setOutputLimits(0x84, 0xED);
+ right_migiusiro.setOutputLimits(0x00, 0x7B);
+ right_hidarimae.setOutputLimits(0x00, 0x69);
+ right_hidariusiro.setOutputLimits(0x84, 0xFF);
}
//よくわからんやつ
@@ -487,16 +560,16 @@
right_hidariusiro.setMode(AUTO_MODE);
//目標値
- right_migimae.setSetPoint(target);
- right_migiusiro.setSetPoint(target);
- right_hidarimae.setSetPoint(target);
- right_hidariusiro.setSetPoint(target);
+ right_migimae.setSetPoint(target*-1);
+ right_migiusiro.setSetPoint(target*-1);
+ right_hidarimae.setSetPoint(target*-1);
+ right_hidariusiro.setSetPoint(target*-1);
//センサ出力
- right_migimae.setProcessValue(abs(x_pulse1));
- right_migiusiro.setProcessValue(abs(x_pulse2));
- right_hidarimae.setProcessValue(abs(x_pulse1));
- right_hidariusiro.setProcessValue(abs(x_pulse2));
+ right_migimae.setProcessValue(x_pulse1*-1);
+ right_migiusiro.setProcessValue(x_pulse2*-1);
+ right_hidarimae.setProcessValue(x_pulse1*-1);
+ right_hidariusiro.setProcessValue(x_pulse2*-1);
//制御量(計算結果)
migimae_data[0] = right_migimae.compute();
@@ -506,31 +579,36 @@
//制御量をPWM値に変換
//右進(目標まで達していない)
- if((abs(x_pulse1) < target) || (abs(x_pulse2) < target)) {
+ 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((abs(x_pulse1) > target) || (abs(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*-1 > target*-1) && (x_pulse2*-1 < target*-1)) {
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 < target*-1) && (x_pulse2*-1 > target*-1)) {
+ true_migimae_data[0] = 0x7B - migimae_data[0];
true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
+ true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = 0x80;
- } else {
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
+ }
+ //左進(目標より行き過ぎ)
+ else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
+ 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];
}
}
-void left_PID(unsigned int target) {
+void left_PID(int target) {
//センサ出力値の最小、最大
left_migimae.setInputLimits(-2147483648, 2147483647);
@@ -540,27 +618,32 @@
//制御量の最小、最大
//左進(目標まで達していない)
- if((x_pulse1 < target) || (x_pulse2 < target)) {
+ if((x_pulse1 < target) && (x_pulse2 < target)) {
left_migimae.setOutputLimits(0x84, 0xED);
left_migiusiro.setOutputLimits(0x00, 0x7B);
left_hidarimae.setOutputLimits(0x00, 0x69);
left_hidariusiro.setOutputLimits(0x84, 0xFF);
- //left_migimae.setOutputLimits(0x84, 0xF6);
- //left_hidarimae.setOutputLimits(0x09, 0x7B);
+ }
+ //前側が左に出ちゃった♥(後側だけ回して前側は停止)
+ else if((x_pulse1 > target) && (x_pulse2 < target)) {
+ left_migimae.setOutputLimits(0x7C, 0x83);
+ left_migiusiro.setOutputLimits(0x7C, 0x83);
+ left_hidarimae.setOutputLimits(0x00, 0x69);
+ left_hidariusiro.setOutputLimits(0x84, 0xFF);
+ }
+ //後側が左に出ちゃった♥(前側だけ回して後側は停止)
+ else if((x_pulse1 < target) && (x_pulse2 > target)) {
+ left_migimae.setOutputLimits(0x84, 0xED);
+ left_migiusiro.setOutputLimits(0x00, 0x7B);
+ left_hidarimae.setOutputLimits(0x7C, 0x83);
+ left_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//右進(目標より行き過ぎ)
- else if((x_pulse1 > target) || (x_pulse2 > target)) {
- //left_migimae.setOutputLimits(0x00, 0x7B);
- //left_migiusiro.setOutputLimits(0x84, 0xFF);
- //left_hidarimae.setOutputLimits(0x84, 0xFF);
- //left_hidariusiro.setOutputLimits(0x00, 0x7B);
- //left_migimae.setOutputLimits(0x09, 0x7B);
- //left_migiusiro.setOutputLimits(0x88, 0xFF);
- //left_hidarimae.setOutputLimits(0x84, 0xF6);
- left_migimae.setOutputLimits(0x7C, 0x83);
- left_migiusiro.setOutputLimits(0x7C, 0x83);
- left_hidarimae.setOutputLimits(0x7C, 0x83);
- left_hidariusiro.setOutputLimits(0x7C, 0x83);
+ else if((x_pulse1 > target) && (x_pulse2 > target)) {
+ left_migimae.setOutputLimits(0x00, 0x6C);
+ left_migiusiro.setOutputLimits(0x84, 0xFF);
+ left_hidarimae.setOutputLimits(0x84, 0xF0);
+ left_hidariusiro.setOutputLimits(0x00, 0x7B);
}
//よくわからんやつ
@@ -589,31 +672,36 @@
//制御量をPWM値に変換
//左進(目標まで達していない)
- if((x_pulse1 < target) || (x_pulse2 < target)) {
+ if((x_pulse1 < target) && (x_pulse2 < target)) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
}
- //右進(目標より行き過ぎ)
- else if((x_pulse1 > target) || (x_pulse2 > target)) {
- //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 > target) && (x_pulse2 < target)) {
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- } else {
- 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 < target) && (x_pulse2 > target)) {
+ 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] = 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];
+ }
}
-void turn_right_PID(unsigned int target) {
+void turn_right_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
turn_right_migimae.setInputLimits(-2147483648, 2147483647);
@@ -623,26 +711,18 @@
//制御量の最小、最大
//右旋回(目標に達してない)
- //if((abs(x_pulse1) < target) || (x_pulse2 < target)) {
- //if((x_pulse1 < target) || (abs(x_pulse2) < target) || (abs(y_pulse1) < target) || (y_pulse2 < target)) {
- if(abs(x_pulse1) < target) {
- turn_right_migimae.setOutputLimits(0x00, 0x7B);
- turn_right_migiusiro.setOutputLimits(0x00, 0x7B);
- turn_right_hidarimae.setOutputLimits(0x84, 0xFF);
- turn_right_hidariusiro.setOutputLimits(0x84, 0xFF);
+ 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((abs(x_pulse1) > target) || (x_pulse2 > target)) {
- //else if((x_pulse1 > target) || (abs(x_pulse2) > target) || (abs(y_pulse1) > target) || (y_pulse2 > target)) {
- else if(abs(x_pulse1) > target) {
- //turn_right_migimae.setOutputLimits(0x84, 0xFF);
- //turn_right_migiusiro.setOutputLimits(0x84, 0xFF);
- //turn_right_hidarimae.setOutputLimits(0x00, 0x7B);
- //turn_right_hidariusiro.setOutputLimits(0x00, 0x7B);
- turn_right_migimae.setOutputLimits(0x7C, 0x83);
- turn_right_migiusiro.setOutputLimits(0x7C, 0x83);
- turn_right_hidarimae.setOutputLimits(0x7C, 0x83);
- turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
+ else if(x_pulse2 > target) {
+ turn_right_migimae.setOutputLimits(0x94, 0xFF);
+ turn_right_migiusiro.setOutputLimits(0x94, 0xFF);
+ turn_right_hidarimae.setOutputLimits(0x10, 0x7B);
+ turn_right_hidariusiro.setOutputLimits(0x10, 0x7B);
}
//よくわからんやつ
@@ -658,10 +738,10 @@
turn_right_hidariusiro.setSetPoint(target);
//センサ出力
- turn_right_migimae.setProcessValue(abs(x_pulse1));
- turn_right_migiusiro.setProcessValue(abs(x_pulse1));
- turn_right_hidarimae.setProcessValue(abs(x_pulse1));
- turn_right_hidariusiro.setProcessValue(abs(x_pulse1));
+ turn_right_migimae.setProcessValue(x_pulse2);
+ turn_right_migiusiro.setProcessValue(x_pulse2);
+ turn_right_hidarimae.setProcessValue(x_pulse2);
+ turn_right_hidariusiro.setProcessValue(x_pulse2);
//制御量(計算結果)
migimae_data[0] = turn_right_migimae.compute();
@@ -671,35 +751,22 @@
//制御量をPWM値に変換
//右旋回(目標に達してない)
- //if((abs(x_pulse1) < target) || (x_pulse2 < target)) {
- //if((x_pulse1 < target) || (abs(x_pulse2) < target) || (abs(y_pulse1) < target) || (y_pulse1 < target)) {
- if(abs(x_pulse1) < target) {
+ 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((abs(x_pulse1) > target) || (x_pulse2 > target)) {
- //else if((x_pulse1 > target) || (abs(x_pulse2) > target) || (abs(y_pulse1) > target) || (y_pulse2 > target)) {
- else if(abs(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];
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
- } else {
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
+ else if(x_pulse2 > 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];
}
}
-void turn_left_PID(unsigned int target) {
+void turn_left_PID(int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
turn_left_migimae.setInputLimits(-2147483648, 2147483647);
@@ -709,22 +776,18 @@
//制御量の最小、最大
//右旋回(目標に達してない)
- //if((x_pulse1 < target) || (abs(x_pulse2) < target)) {
- //if((abs(x_pulse1) < target) || (x_pulse2 < target) || (y_pulse1 < target) || (abs(y_pulse2) < target)) {
if(x_pulse1 < target) {
- turn_left_migimae.setOutputLimits(0x84, 0xFF);
- turn_left_migiusiro.setOutputLimits(0x84, 0xFF);
- turn_left_hidarimae.setOutputLimits(0x00, 0x7B);
- turn_left_hidariusiro.setOutputLimits(0x00, 0x7B);
+ 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) || (abs(x_pulse2) > target)) {
- //else if((abs(x_pulse1) > target) || (x_pulse2 > target) || (y_pulse1 > target) || (abs(y_pulse2) > target)) {
else if(x_pulse1 > target) {
- turn_left_migimae.setOutputLimits(0x00, 0x7B);
- turn_left_migiusiro.setOutputLimits(0x00, 0x7B);
- turn_left_hidarimae.setOutputLimits(0x84, 0xFF);
- turn_left_hidariusiro.setOutputLimits(0x84, 0xFF);
+ turn_left_migimae.setOutputLimits(0x10, 0x7B);
+ turn_left_migiusiro.setOutputLimits(0x10, 0x7B);
+ turn_left_hidarimae.setOutputLimits(0x94, 0xFF);
+ turn_left_hidariusiro.setOutputLimits(0x94, 0xFF);
}
//よくわからんやつ
@@ -753,8 +816,6 @@
//制御量をPWM値に変換
//右旋回(目標に達してない)
- //if((x_pulse1 < target) || (abs(x_pulse2) < target)) {
- //if((abs(x_pulse1) < target) || (x_pulse2 < target) || (y_pulse1 < target) || (abs(y_pulse2) < target)) {
if(x_pulse1 < target) {
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
@@ -762,18 +823,11 @@
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//左旋回(目標より行き過ぎ)
- //else if((x_pulse1 > target) || (abs(x_pulse2) > target)) {
- //else if((abs(x_pulse1) > target) || (x_pulse2 > target) || (y_pulse1 > target) || (abs(y_pulse2) > target)) {
else if(x_pulse1 > 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 {
- true_migimae_data[0] = 0x80;
- true_migiusiro_data[0] = 0x80;
- true_hidarimae_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
}
}