2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
Diff: main.cpp
- Revision:
- 16:05b26003da50
- Parent:
- 15:d022288aec51
- Child:
- 17:de3bc1999ae7
--- a/main.cpp Mon Aug 26 02:23:20 2019 +0000
+++ b/main.cpp Sun Sep 01 12:12:55 2019 +0000
@@ -4,7 +4,7 @@
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
/* Sensor: encorder*4 */
/* ------------------------------------------------------------------- */
-/* PID関数を各方向毎に追加した */
+/* 4方向の直進補正をしてみた */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
@@ -17,6 +17,9 @@
#define Ti 0.0
#define Td 0.0
+#define RED 0
+#define BLUE 1
+
PID migimae(Kp, Ti, Td, 0.001);
PID migiusiro(Kp, Ti, Td, 0.001);
PID hidarimae(Kp, Ti, Td, 0.001);
@@ -35,16 +38,16 @@
PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
//右進
-PID right_migimae(4500000.0, 0.0, 0.0, 0.001);
-PID right_migiusiro(4500000.0, 0.0, 0.0, 0.001);
-PID right_hidarimae(4500000.0, 0.0, 0.0, 0.001);
-PID right_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 left_migimae(4500000.0, 0.0, 0.0, 0.001);
-PID left_migiusiro(4500000.0, 0.0, 0.0, 0.001);
-PID left_hidarimae(4500000.0, 0.0, 0.0, 0.001);
-PID left_hidariusiro(4500000.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 turn_right_migimae(4500000.0, 0.0, 0.0, 0.001);
@@ -67,10 +70,13 @@
//12V停止信号ピン
DigitalOut emergency(D11);
-DigitalOut USR_LED1(PC_9);
-DigitalOut USR_LED2(PC_8);
-DigitalOut USR_LED3(PC_6);
-DigitalOut USR_LED4(PC_5);
+DigitalOut USR_LED1(PB_7);
+DigitalOut USR_LED2(PC_13);
+DigitalOut USR_LED3(PC_2);
+DigitalOut USR_LED4(PC_3);
+
+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);
@@ -79,11 +85,15 @@
//QEI wheel1(D2, D3, NC, 624);
//QEI wheel2(D5, D4, NC, 624);
+Ticker look_switch;
+
//エンコーダ値格納変数
int x_pulse1, x_pulse2, y_pulse1, y_pulse2;
//操作の段階変数
unsigned int phase = 0;
+unsigned int start_zone = 1;
+bool zone = RED;
//MD送信データ変数
char init_send_data[1];
@@ -91,6 +101,7 @@
char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
//関数のプロトタイプ宣言
+void incriment(void);
void init(void);
void init_send(void);
void get_pulses(void);
@@ -113,20 +124,74 @@
init();
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();
- front(500);
- //back(500);
- //right(500);
- //left(500);
- //turn_right(500);
- //turn_left(500);
+
+ 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);
+ }
+ else if(phase == 1) {
+ right(10000);
+ }
+ else if(phase == 2) {
+ back(10000);
+ }
+ else if(phase == 3) {
+ left(10000);
+ }
+
+ /*
+ 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;
+ }
+ */
+
+ //back(5000);
+ //right(14000);
+ //left(14000); //直進補正済
+ //turn_right(600);
+ //turn_left(300);
}
}
+void incriment(void) {
+ if(start_switch == 0) {
+ phase++;
+ }
+}
void init(void) {
//緊急停止用信号ピンをLow
@@ -134,8 +199,10 @@
//USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1;
//通信ボーレートの設定
- //pc.baud(460800);
- pc.baud(9600);
+ pc.baud(460800);
+ //pc.baud(9600);
+
+ start_switch.mode(PullUp);
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;
@@ -162,7 +229,9 @@
void print_pulses(void) {
- pc.printf("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2);
+ //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("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) {
@@ -228,70 +297,73 @@
void front_PID(unsigned int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
- migimae.setInputLimits(-2147483648, 2147483647);
- migiusiro.setInputLimits(-2147483648, 2147483647);
- hidarimae.setInputLimits(-2147483648, 2147483647);
- hidariusiro.setInputLimits(-2147483648, 2147483647);
+ 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)) {
- migimae.setOutputLimits(0x84, 0xFF);
- migiusiro.setOutputLimits(0x84, 0xFF);
- hidarimae.setOutputLimits(0x84, 0xFF);
- hidariusiro.setOutputLimits(0x84, 0xFF);
+ 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)) {
- migimae.setOutputLimits(0x00, 0x7B);
- migiusiro.setOutputLimits(0x00, 0x7B);
- hidarimae.setOutputLimits(0x00, 0x7B);
- hidariusiro.setOutputLimits(0x00, 0x7B);
- } else {
- migimae.setOutputLimits(0x7C, 0x83);
- migiusiro.setOutputLimits(0x7C, 0x83);
- hidarimae.setOutputLimits(0x7C, 0x83);
- hidariusiro.setOutputLimits(0x7C, 0x83);
+ 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);
+ front_migimae.setOutputLimits(0x7C, 0x83);
+ front_migiusiro.setOutputLimits(0x7C, 0x83);
+ front_hidarimae.setOutputLimits(0x7C, 0x83);
+ front_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
- migimae.setMode(AUTO_MODE);
- migiusiro.setMode(AUTO_MODE);
- hidarimae.setMode(AUTO_MODE);
- hidariusiro.setMode(AUTO_MODE);
+ front_migimae.setMode(AUTO_MODE);
+ front_migiusiro.setMode(AUTO_MODE);
+ front_hidarimae.setMode(AUTO_MODE);
+ front_hidariusiro.setMode(AUTO_MODE);
//目標値
- migimae.setSetPoint(target);
- migiusiro.setSetPoint(target);
- hidarimae.setSetPoint(target);
- hidariusiro.setSetPoint(target);
+ front_migimae.setSetPoint(target);
+ front_migiusiro.setSetPoint(target);
+ front_hidarimae.setSetPoint(target);
+ front_hidariusiro.setSetPoint(target);
//センサ出力
- migimae.setProcessValue(y_pulse1);
- migiusiro.setProcessValue(y_pulse1);
- hidarimae.setProcessValue(y_pulse2);
- hidariusiro.setProcessValue(y_pulse2);
+ front_migimae.setProcessValue(y_pulse1);
+ front_migiusiro.setProcessValue(y_pulse1);
+ front_hidarimae.setProcessValue(y_pulse2);
+ front_hidariusiro.setProcessValue(y_pulse2);
//制御量(計算結果)
- migimae_data[0] = migimae.compute();
- migiusiro_data[0] = migiusiro.compute();
- hidarimae_data[0] = hidarimae.compute();
- hidariusiro_data[0] = hidariusiro.compute();
+ 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)) {
+ 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] = 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 > 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;
@@ -303,70 +375,73 @@
void back_PID(unsigned int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
- migimae.setInputLimits(-2147483648, 2147483647);
- migiusiro.setInputLimits(-2147483648, 2147483647);
- hidarimae.setInputLimits(-2147483648, 2147483647);
- hidariusiro.setInputLimits(-2147483648, 2147483647);
+ back_migimae.setInputLimits(-2147483648, 2147483647);
+ back_migiusiro.setInputLimits(-2147483648, 2147483647);
+ back_hidarimae.setInputLimits(-2147483648, 2147483647);
+ back_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//逆転(目標に達してない)
- if((abs(y_pulse1) < target) && (abs(y_pulse2) < target)) {
- migimae.setOutputLimits(0x00, 0x7B);
- migiusiro.setOutputLimits(0x00, 0x7B);
- hidarimae.setOutputLimits(0x00, 0x7B);
- hidariusiro.setOutputLimits(0x0, 0x7B);
+ if((abs(y_pulse1) < target) || (abs(y_pulse2) < target)) {
+ 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)) {
- migimae.setOutputLimits(0x84, 0xFF);
- migiusiro.setOutputLimits(0x84, 0xFF);
- hidarimae.setOutputLimits(0x84, 0xFF);
- hidariusiro.setOutputLimits(0x84, 0xFF);
- } else {
- migimae.setOutputLimits(0x7C, 0x83);
- migiusiro.setOutputLimits(0x7C, 0x83);
- hidarimae.setOutputLimits(0x7C, 0x83);
- hidariusiro.setOutputLimits(0x7C, 0x83);
+ 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);
+ back_migimae.setOutputLimits(0x7C, 0x83);
+ back_migiusiro.setOutputLimits(0x7C, 0x83);
+ back_hidarimae.setOutputLimits(0x7C, 0x83);
+ back_hidariusiro.setOutputLimits(0x7C, 0x83);
}
//よくわからんやつ
- migimae.setMode(AUTO_MODE);
- migiusiro.setMode(AUTO_MODE);
- hidarimae.setMode(AUTO_MODE);
- hidariusiro.setMode(AUTO_MODE);
+ back_migimae.setMode(AUTO_MODE);
+ back_migiusiro.setMode(AUTO_MODE);
+ back_hidarimae.setMode(AUTO_MODE);
+ back_hidariusiro.setMode(AUTO_MODE);
//目標値
- migimae.setSetPoint(target);
- migiusiro.setSetPoint(target);
- hidarimae.setSetPoint(target);
- hidariusiro.setSetPoint(target);
+ back_migimae.setSetPoint(target);
+ back_migiusiro.setSetPoint(target);
+ back_hidarimae.setSetPoint(target);
+ back_hidariusiro.setSetPoint(target);
//センサ出力
- migimae.setProcessValue(abs(y_pulse1));
- migiusiro.setProcessValue(abs(y_pulse1));
- hidarimae.setProcessValue(abs(y_pulse2));
- hidariusiro.setProcessValue(abs(y_pulse2));
+ back_migimae.setProcessValue(abs(y_pulse1));
+ back_migiusiro.setProcessValue(abs(y_pulse1));
+ back_hidarimae.setProcessValue(abs(y_pulse2));
+ back_hidariusiro.setProcessValue(abs(y_pulse2));
//制御量(計算結果)
- migimae_data[0] = migimae.compute();
- migiusiro_data[0] = migiusiro.compute();
- hidarimae_data[0] = hidarimae.compute();
- hidariusiro_data[0] = hidariusiro.compute();
+ 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((abs(y_pulse1) < target) && (abs(y_pulse2) < target)) {
+ if((abs(y_pulse1) < target) || (abs(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];
}
//正転(目標より行き過ぎ)
- 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];
+ 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;
@@ -378,65 +453,75 @@
void right_PID(unsigned int target) {
//センサ出力値の最小、最大
- migimae.setInputLimits(-2147483648, 2147483647);
- migiusiro.setInputLimits(-2147483648, 2147483647);
- hidarimae.setInputLimits(-2147483648, 2147483647);
- hidariusiro.setInputLimits(-2147483648, 2147483647);
+ right_migimae.setInputLimits(-2147483648, 2147483647);
+ right_migiusiro.setInputLimits(-2147483648, 2147483647);
+ right_hidarimae.setInputLimits(-2147483648, 2147483647);
+ right_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//右進(目標まで達していない)
- if((x_pulse1 < target) && (x_pulse2 < target)) {
- migimae.setOutputLimits(0x00, 0x7B);
- migiusiro.setOutputLimits(0x84, 0xFF);
- hidarimae.setOutputLimits(0x84, 0xFF);
- hidariusiro.setOutputLimits(0x00, 0x7B);
+ if((abs(x_pulse1) < target) || (abs(x_pulse2) < target)) {
+ 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 > target) && (x_pulse2 > target)) {
- migimae.setOutputLimits(0x84, 0xFF);
- migiusiro.setOutputLimits(0x00, 0x7B);
- hidarimae.setOutputLimits(0x00, 0x7B);
- hidariusiro.setOutputLimits(0x84, 0xFF);
+ 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);
}
//よくわからんやつ
- migimae.setMode(AUTO_MODE);
- migiusiro.setMode(AUTO_MODE);
- hidarimae.setMode(AUTO_MODE);
- hidariusiro.setMode(AUTO_MODE);
+ right_migimae.setMode(AUTO_MODE);
+ right_migiusiro.setMode(AUTO_MODE);
+ right_hidarimae.setMode(AUTO_MODE);
+ right_hidariusiro.setMode(AUTO_MODE);
//目標値
- migimae.setSetPoint(target);
- migiusiro.setSetPoint(target);
- hidarimae.setSetPoint(target);
- hidariusiro.setSetPoint(target);
+ right_migimae.setSetPoint(target);
+ right_migiusiro.setSetPoint(target);
+ right_hidarimae.setSetPoint(target);
+ right_hidariusiro.setSetPoint(target);
//センサ出力
- migimae.setProcessValue(x_pulse1);
- migiusiro.setProcessValue(x_pulse1);
- hidarimae.setProcessValue(x_pulse2);
- hidariusiro.setProcessValue(x_pulse2);
+ right_migimae.setProcessValue(abs(x_pulse1));
+ right_migiusiro.setProcessValue(abs(x_pulse2));
+ right_hidarimae.setProcessValue(abs(x_pulse1));
+ right_hidariusiro.setProcessValue(abs(x_pulse2));
//制御量(計算結果)
- migimae_data[0] = migimae.compute();
- migiusiro_data[0] = migiusiro.compute();
- hidarimae_data[0] = hidarimae.compute();
- hidariusiro_data[0] = hidariusiro.compute();
+ migimae_data[0] = right_migimae.compute();
+ migiusiro_data[0] = right_migiusiro.compute();
+ hidarimae_data[0] = right_hidarimae.compute();
+ hidariusiro_data[0] = right_hidariusiro.compute();
//制御量をPWM値に変換
//右進(目標まで達していない)
- if((x_pulse1 < target) && (x_pulse2 < target)) {
+ if((abs(x_pulse1) < target) || (abs(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] = 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((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];
+ 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;
@@ -448,65 +533,78 @@
void left_PID(unsigned int target) {
//センサ出力値の最小、最大
- migimae.setInputLimits(-2147483648, 2147483647);
- migiusiro.setInputLimits(-2147483648, 2147483647);
- hidarimae.setInputLimits(-2147483648, 2147483647);
- hidariusiro.setInputLimits(-2147483648, 2147483647);
+ left_migimae.setInputLimits(-2147483648, 2147483647);
+ left_migiusiro.setInputLimits(-2147483648, 2147483647);
+ left_hidarimae.setInputLimits(-2147483648, 2147483647);
+ left_hidariusiro.setInputLimits(-2147483648, 2147483647);
//制御量の最小、最大
//左進(目標まで達していない)
- if((abs(x_pulse1) < target) && (abs(x_pulse2) < target)) {
- migimae.setOutputLimits(0x84, 0xFF);
- migiusiro.setOutputLimits(0x00, 0x7B);
- hidarimae.setOutputLimits(0x00, 0x7B);
- hidariusiro.setOutputLimits(0x84, 0xFF);
+ 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((abs(x_pulse1) > target) && (abs(x_pulse2) > target)) {
- migimae.setOutputLimits(0x00, 0x7B);
- migiusiro.setOutputLimits(0x84, 0xFF);
- hidarimae.setOutputLimits(0x84, 0xFF);
- hidariusiro.setOutputLimits(0x00, 0x7B);
+ 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);
}
-
+
//よくわからんやつ
- migimae.setMode(AUTO_MODE);
- migiusiro.setMode(AUTO_MODE);
- hidarimae.setMode(AUTO_MODE);
- hidariusiro.setMode(AUTO_MODE);
+ left_migimae.setMode(AUTO_MODE);
+ left_migiusiro.setMode(AUTO_MODE);
+ left_hidarimae.setMode(AUTO_MODE);
+ left_hidariusiro.setMode(AUTO_MODE);
//目標値
- migimae.setSetPoint(target);
- migiusiro.setSetPoint(target);
- hidarimae.setSetPoint(target);
- hidariusiro.setSetPoint(target);
+ left_migimae.setSetPoint(target);
+ left_migiusiro.setSetPoint(target);
+ left_hidarimae.setSetPoint(target);
+ left_hidariusiro.setSetPoint(target);
//センサ出力
- migimae.setProcessValue(abs(x_pulse1));
- migiusiro.setProcessValue(abs(x_pulse1));
- hidarimae.setProcessValue(abs(x_pulse2));
- hidariusiro.setProcessValue(abs(x_pulse2));
+ left_migimae.setProcessValue(x_pulse1);
+ left_migiusiro.setProcessValue(x_pulse2);
+ left_hidarimae.setProcessValue(x_pulse1);
+ left_hidariusiro.setProcessValue(x_pulse2);
//制御量(計算結果)
- migimae_data[0] = migimae.compute();
- migiusiro_data[0] = migiusiro.compute();
- hidarimae_data[0] = hidarimae.compute();
- hidariusiro_data[0] = 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((abs(x_pulse1) < target) && (abs(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((abs(x_pulse1) > target) && (abs(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] = 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];
+ 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;
@@ -518,65 +616,81 @@
void turn_right_PID(unsigned int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
- migimae.setInputLimits(-2147483648, 2147483647);
- migiusiro.setInputLimits(-2147483648, 2147483647);
- hidarimae.setInputLimits(-2147483648, 2147483647);
- hidariusiro.setInputLimits(-2147483648, 2147483647);
+ 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) && (y_pulse1 < target) && (y_pulse2 < target)) {
- migimae.setOutputLimits(0x00, 0x7B);
- migiusiro.setOutputLimits(0x00, 0x7B);
- hidarimae.setOutputLimits(0x84, 0xFF);
- hidariusiro.setOutputLimits(0x84, 0xFF);
+ //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);
}
//左旋回(目標より行き過ぎ)
- else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) {
- migimae.setOutputLimits(0x84, 0xFF);
- migiusiro.setOutputLimits(0x84, 0xFF);
- hidarimae.setOutputLimits(0x00, 0x7B);
- hidariusiro.setOutputLimits(0x00, 0x7B);
+ //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);
}
//よくわからんやつ
- migimae.setMode(AUTO_MODE);
- migiusiro.setMode(AUTO_MODE);
- hidarimae.setMode(AUTO_MODE);
- 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);
//目標値
- migimae.setSetPoint(target);
- migiusiro.setSetPoint(target);
- hidarimae.setSetPoint(target);
- hidariusiro.setSetPoint(target);
+ turn_right_migimae.setSetPoint(target);
+ turn_right_migiusiro.setSetPoint(target);
+ turn_right_hidarimae.setSetPoint(target);
+ turn_right_hidariusiro.setSetPoint(target);
//センサ出力
- migimae.setProcessValue(x_pulse1);
- migiusiro.setProcessValue(x_pulse2);
- hidarimae.setProcessValue(y_pulse1);
- hidariusiro.setProcessValue(y_pulse2);
+ 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));
//制御量(計算結果)
- migimae_data[0] = migimae.compute();
- migiusiro_data[0] = migiusiro.compute();
- hidarimae_data[0] = hidarimae.compute();
- hidariusiro_data[0] = hidariusiro.compute();
+ 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_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse1 < target)) {
+ //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) {
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_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_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];
+ //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;
@@ -588,61 +702,69 @@
void turn_left_PID(unsigned int target) {
//センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
- migimae.setInputLimits(-2147483648, 2147483647);
- migiusiro.setInputLimits(-2147483648, 2147483647);
- hidarimae.setInputLimits(-2147483648, 2147483647);
- hidariusiro.setInputLimits(-2147483648, 2147483647);
+ 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((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < target)) {
- migimae.setOutputLimits(0x84, 0xFF);
- migiusiro.setOutputLimits(0x84, 0xFF);
- hidarimae.setOutputLimits(0x00, 0x7B);
- hidariusiro.setOutputLimits(0x00, 0x7B);
+ //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);
}
//左旋回(目標より行き過ぎ)
- else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) {
- migimae.setOutputLimits(0x00, 0x7B);
- migiusiro.setOutputLimits(0x00, 0x7B);
- hidarimae.setOutputLimits(0x84, 0xFF);
- hidariusiro.setOutputLimits(0x84, 0xFF);
+ //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);
}
//よくわからんやつ
- migimae.setMode(AUTO_MODE);
- migiusiro.setMode(AUTO_MODE);
- hidarimae.setMode(AUTO_MODE);
- 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);
//目標値
- migimae.setSetPoint(target);
- migiusiro.setSetPoint(target);
- hidarimae.setSetPoint(target);
- hidariusiro.setSetPoint(target);
+ turn_left_migimae.setSetPoint(target);
+ turn_left_migiusiro.setSetPoint(target);
+ turn_left_hidarimae.setSetPoint(target);
+ turn_left_hidariusiro.setSetPoint(target);
//センサ出力
- migimae.setProcessValue(abs(x_pulse1));
- migiusiro.setProcessValue(abs(y_pulse1));
- hidarimae.setProcessValue(abs(y_pulse2));
- hidariusiro.setProcessValue(abs(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] = migimae.compute();
- migiusiro_data[0] = migiusiro.compute();
- hidarimae_data[0] = hidarimae.compute();
- hidariusiro_data[0] = 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((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < target)) {
+ //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];
true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
//左旋回(目標より行き過ぎ)
- else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) {
+ //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];
@@ -674,7 +796,7 @@
wait(0.05);
}
//だんだん加速(逆転)
- for(init_send_data[0] = 0x7B; init_send_data[0] <= 0x00; init_send_data[0]--){
+ 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);