2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
Diff: main.cpp
- Revision:
- 5:167327a82430
- Parent:
- 4:df334779a69e
- Child:
- 6:5a051a378e42
--- a/main.cpp Wed Sep 05 15:07:15 2018 +0000
+++ b/main.cpp Sun Sep 23 17:02:06 2018 +0000
@@ -10,19 +10,59 @@
/* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */
/* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */
/* ------------------------------------------------------------------- */
+
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
#include "hcsr04.h"
+
+//Pi
#define PI 3.14159265359
+//PIDGain of wheels(fast_mode)
#define Kp 20.0
#define Ki 0.02
#define Kd 0.0
-//#define roller_Kp 0.5
-//#define roller_Ki 0.3
-#define roller_Kp 2.5
-#define roller_Ki 0.01
+//PIDGain of rollers
+#define roller_Kp 4.0
+#define roller_Ki 0.04
+#define roller_Kd 0.0
+//PIDGain of wheels(slow_mode)
+#define Kp_slow 0.6
+#define Ki_slow 0.03
+#define Kd_slow 0.0
+
+//停止
+#define stop 0
+//低速右移動
+#define right_slow 1
+//低速左移動
+#define left_slow 2
+//超低速右移動
+#define right_super_slow 3
+//超低速左移動
+#define left_super_slow 4
+//右旋回
+#define turn_right 5
+//左旋回
+#define turn_left 6
+//前進
+#define front_trace 7
+//後進
+#define back_trace 8
+//前前進後ろ右旋回
+#define front_front_back_right 9
+//前前進後ろ左旋回
+#define front_front_back_left 10
+//前右旋回後ろ前進
+#define front_right_back_front 11
+//前左旋回後ろ前進
+#define front_left_back_front 12
+
+//赤ゾーン
+#define red 1
+//青ゾーン
+#define blue 0
//右前オムニ
PID migimae(Kp, Ki, Kd, 0.001);
@@ -32,10 +72,20 @@
PID hidarimae(Kp, Ki, Kd, 0.001);
//左後オムニ
PID hidariusiro(Kp, Ki, Kd, 0.001);
+
+//右前オムニ(スローモード)
+PID migimae_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
+//右後オムニ(スローモード)
+PID migiusiro_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
+//左前オムニ(スローモード)
+PID hidarimae_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
+//左後オムニ(スローモード)
+PID hidariusiro_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
+
//前ローラー
-PID front_roller(roller_Kp, roller_Ki, 0.0, 0.001);
+PID front_roller(roller_Kp, roller_Ki, roller_Kd, 0.001);
//後ローラー
-PID back_roller(roller_Kp, roller_Ki, 0.0, 0.001);
+PID back_roller (roller_Kp, roller_Ki, roller_Kd, 0.001);
//MDとの通信ポート
I2C i2c(PB_9, PB_8); //SDA, SCL
@@ -59,6 +109,8 @@
DigitalIn side(PC_1);
//スタートボタン
DigitalIn start_button(PC_4);
+//フェンス検知用リミットスイッチ
+DigitalIn limit(PH_1);
//スイッチ1
DigitalIn user_switch1(PB_0);
//スイッチ2
@@ -99,7 +151,7 @@
//後ローラー
QEI back_roller_wheel(PB_5, PB_4, NC, 624);
//計測オムニ1
-//QEI measure1_wheel(PC_9, PC_8, NC, 624);
+QEI measure_wheel(PC_9, PC_8, NC, 624);
//計測オムニ2(使用不可)
//QEI measure2_wheel(PB_3, PB_10, NC, 624);
//右前オムニ
@@ -114,46 +166,62 @@
Ticker get_rps;
Ticker get_distance;
Timer timer;
+Timer back_timer1;
+Timer back_timer2;
+Timer back_timer3;
-int roller_flag = 0;
-int loading_state = 0;
+bool roller_flag = 0;
+bool start_flag = 0;
+int loading_state = 0;
+int line_state = 0;
+int front_line_state = 0;
+int back_line_state = 0;
+int line_state_pettern = 0;
+unsigned int distance;
+int trace_direction = 0;
+static int i = 0;
+int right_flag = 0;
+
int migimae_rpm;
int migiusiro_rpm;
int hidarimae_rpm;
int hidariusiro_rpm;
-int measure1_rpm;
-//int measure2_rpm;
+//int measure_rpm;
int front_roller_rpm;
int back_roller_rpm;
-int migimae_pulse;
-int migiusiro_pulse;
-int hidarimae_pulse;
-int hidariusiro_pulse;
-int measure1_pulse;
-//int measure2_pulse;
-int front_roller_pulse;
-int back_roller_pulse;
+//int migimae_pulse;
+//int migiusiro_pulse;
+//int hidarimae_pulse;
+//int hidariusiro_pulse;
+int measure_pulse;
+//int front_roller_pulse;
+//int back_roller_pulse;
+
+int migimae_pulse[10];
+int migiusiro_pulse[10];
+int hidarimae_pulse[10];
+int hidariusiro_pulse[10];
+//int measure_pulse[10];
+int front_roller_pulse[10];
+int back_roller_pulse[10];
-int ave_migimae_pulse[10];
-int ave_migiusiro_pulse[10];
-int ave_hidarimae_pulse[10];
-int ave_hidariusiro_pulse[10];
-int ave_measure1_pulse[10];
-//int ave_measure2_pulse[10];
-int ave_front_roller_pulse[10];
-int ave_back_roller_pulse[10];
+int sum_migimae_pulse;
+int sum_migiusiro_pulse;
+int sum_hidarimae_pulse;
+int sum_hidariusiro_pulse;
+//int sum_measure_pulse;
+int sum_front_roller_pulse;
+int sum_back_roller_pulse;
-
-int migimae_counter;
-int migiusiro_counter;
-int hidarimae_counter;
-int hidariusiro_counter;
-int measure1_counter;
-//int measure2_counter;
-int front_roller_counter;
-int back_roller_counter;
+int ave_migimae_pulse;
+int ave_migiusiro_pulse;
+int ave_hidarimae_pulse;
+int ave_hidariusiro_pulse;
+//int ave_measure_pulse;
+int ave_front_roller_pulse;
+int ave_back_roller_pulse;
char send_data[1];
char true_send_data[1];
@@ -172,502 +240,1060 @@
char true_hidarimae_data[1];
char true_hidariusiro_data[1];
-int line_state = 0;
-
-unsigned int distance;
-
-/* ロリコン処理関数 */
+//各関数のプロトタイプ宣言
+//回転数取得関数
void flip();
+//前進PID(fast_mode)
int front_PID(int RF, int RB, int LF, int LB);
+//前進PID(slow_mode)
+int front_PID_slow(int RF, int RB, int LF, int LB);
+//前進PID(fast_mode)
int back_PID(int RF, int RB, int LF, int LB);
+//後進PID(slow_mode)
+int back_PID_slow(int RF, int RB, int LF, int LB);
+//右進PID(fast_mode)
int rihgt_PID(int RF, int RB, int LF, int LB);
+//左進PID(fast_mode)
int left_PID(int RF, int RB, int LF, int LB);
+//右前PID(slow_mode)
int right_front_PID(int RB, int LF);
+//右後PID(slow_mode)
int right_back_PID(int RF, int LB);
+//左前PID(slow_mode)
int left_front_PID(int RF, int LB);
+//左後PID(slow_mode)
int left_back_PID(int RB, int RF);
+//右旋回PID(fast mode)
int turn_right_PID(int RF, int RB, int LF, int LB);
+//右旋回PID(slow mode)
+int turn_right_PID_slow(int RF, int RB, int LF, int LB);
+//左旋回PID(fast mode)
int turn_left_PID(int RF, int RB, int LF, int LB);
+//左旋回PID(sow mode)
+int turn_left_PID_slow(int RF, int RB, int LF, int LB);
+//前前進後右旋回(slow_mode)
+int front_front_back_right_PID(int RF, int RB, int LF, int LB);
+//前前進後左旋回(slow_mode)
+int front_front_back_left_PID(int RF, int RB, int LF, int LB);
+//前右旋回後前進(slow_mode)
+int front_right_back_front_PID(int RF, int RB, int LF, int LB);
+//前左旋回後前進(slow_mode)
+int front_left_back_front_PID(int RF, int RB, int LF, int LB);
+//ローラーPID
int roller_PID(int front, int back);
+
+//ラインセンサ制御基板との通信用関数
void linetrace();
+//超音波センサ用関数
void ultrasonic();
+//移動平均をPID
void flip() {
- migimae_pulse = migimae_wheel.getPulses();
- migiusiro_pulse = migiusiro_wheel.getPulses();
- hidarimae_pulse = hidarimae_wheel.getPulses();
- hidariusiro_pulse = hidariusiro_wheel.getPulses();
- //measure1_pulse = measure1_wheel.getPulses();
- //measure2_pulse = measure2_wheel.getPulses();
- front_roller_pulse = front_roller_wheel.getPulses();
- back_roller_pulse = back_roller_wheel.getPulses();
+
+ //前回のi番目のデータを消して新たにそこにデータを書き込む(キューのような考え?)
+ sum_migimae_pulse -= migimae_pulse[i];
+ sum_migiusiro_pulse -= migiusiro_pulse[i];
+ sum_hidarimae_pulse -= hidarimae_pulse[i];
+ sum_hidariusiro_pulse -= hidariusiro_pulse[i];
+ sum_front_roller_pulse -= front_roller_pulse[i];
+ sum_back_roller_pulse -= back_roller_pulse[i];
- //((40ms*25 = 1s)* 60 = 1min) / 分解能
- migimae_rpm = migimae_pulse * 25 * 60 / 1200;
- migiusiro_rpm = migiusiro_pulse * 25 * 60 / 1200;
- hidarimae_rpm = hidarimae_pulse * 25 * 60 / 1200;
- hidariusiro_rpm = hidariusiro_pulse * 25 * 60 / 1200;
- //measure1_rpm = measure1_pulse * 25 * 60 / 1200;
- //measure2_rpm = measure2_pulse * 25 * 60 / 1200;
- front_roller_rpm = front_roller_pulse * 25 * 60 / 1200;
- back_roller_rpm = back_roller_pulse * 25 * 60 / 1200;
-
- //pc.printf("RF: %d RB: %d LF: %d LB: %d\n", migimae_rpm, migiusiro_rpm, hidarimae_rpm, hidariusiro_rpm);
- //pc.printf("前: %d, 後: %d, %d\n", abs(front_roller_rpm), abs(back_roller_rpm), distance);
- //pc.printf("%d\n", abs(back_roller_rpm));
- //pc.printf("RF: %d, RB: %d\n", migimae_rpm, migiusiro_rpm);
+ //配列のi番目の箱に取得パルスを代入
+ migimae_pulse[i] = migimae_wheel.getPulses();
+ migiusiro_pulse[i] = migiusiro_wheel.getPulses();
+ hidarimae_pulse[i] = hidarimae_wheel.getPulses();
+ hidariusiro_pulse[i] = hidariusiro_wheel.getPulses();
+ measure_pulse = measure_wheel.getPulses();
+ front_roller_pulse[i] = front_roller_wheel.getPulses();
+ back_roller_pulse[i] = back_roller_wheel.getPulses();
+ //migimae_pulse = migimae_wheel.getPulses();
+ //migiusiro_pulse = migiusiro_wheel.getPulses();
+ //hidarimae_pulse = hidarimae_wheel.getPulses();
+ //hidariusiro_pulse = hidariusiro_wheel.getPulses();
migimae_wheel.reset();
migiusiro_wheel.reset();
hidarimae_wheel.reset();
hidariusiro_wheel.reset();
- //measure1_wheel.reset();
- //measure2_wheel.reset();
front_roller_wheel.reset();
back_roller_wheel.reset();
+
+ //i[0]~i[9]までの合計値を代入
+ sum_migimae_pulse += migimae_pulse[i];
+ sum_migiusiro_pulse += migiusiro_pulse[i];
+ sum_hidarimae_pulse += hidarimae_pulse[i];
+ sum_hidariusiro_pulse += hidariusiro_pulse[i];
+ sum_front_roller_pulse += front_roller_pulse[i];
+ sum_back_roller_pulse += back_roller_pulse[i];
+
+ //インクリメント
+ i++;
+
+ //iが最大値(9)になったらリセット
+ if(i > 9) {
+ i = 0;
+ }
+
+ //10回分の合計値を10で割り、取得パルスの平均を出す
+ ave_migimae_pulse = sum_migimae_pulse / 10;
+ ave_migiusiro_pulse = sum_migiusiro_pulse / 10;
+ ave_hidarimae_pulse = sum_hidarimae_pulse / 10;
+ ave_hidariusiro_pulse = sum_hidariusiro_pulse / 10;
+ ave_front_roller_pulse = sum_front_roller_pulse / 10;
+ ave_back_roller_pulse = sum_back_roller_pulse / 10;
+
+ //平均値をRPMへ変換
+ migimae_rpm = ave_migimae_pulse * 25 * 60 / 1200;
+ migiusiro_rpm = ave_migiusiro_pulse * 25 * 60 / 1200;
+ hidarimae_rpm = ave_hidarimae_pulse * 25 * 60 / 1200;
+ hidariusiro_rpm = ave_hidariusiro_pulse * 25 * 60 / 1200;
+ front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 1200;
+ back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
+
+ //pc.printf("左後 %d\n", abs(hidariusiro_rpm));
+ //pc.printf("%d\n\r", measure_pulse);
+ //pc.printf("%d %d\n", abs(front_roller_rpm), abs(back_roller_rpm));
+ //pc.printf("前 %d 後 %d %d %d\n", abs(front_roller_rpm), abs(back_roller_rpm), distance, line_state);
}
+//前進(fast mode)
int front_PID(int RF, int RB, int LF, int LB) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migimae.setInputLimits(-400, 400);
migiusiro.setInputLimits(-400, 400);
hidarimae.setInputLimits(-400, 400);
hidariusiro.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migimae.setOutputLimits(0x0C, 0x7C);
migiusiro.setOutputLimits(0x0C, 0x7C);
hidarimae.setOutputLimits(0x0C, 0x7C);
hidariusiro.setOutputLimits(0x0C, 0x7C);
- // よくわからんやつ
+ // よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
- // 目標値
+ // 目標値
migimae.setSetPoint(RF);
migiusiro.setSetPoint(RB);
hidarimae.setSetPoint(LF);
hidariusiro.setSetPoint(LB);
-
- // センサ出力
+
+ // センサ出力
migimae.setProcessValue(abs(migimae_rpm));
migiusiro.setProcessValue(abs(migiusiro_rpm));
hidarimae.setProcessValue(abs(hidarimae_rpm));
hidariusiro.setProcessValue(abs(hidariusiro_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
- // 制御量をPWM値に変換
+ // 制御量をPWM値に変換
true_migimae_data[0] = 0x7D - migimae_data[0];
true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
-
+
return 0;
}
+//前進(slow mode)
+int front_PID_slow(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x0C, 0x7C);
+ migiusiro_slow.setOutputLimits(0x0C, 0x7C);
+ hidarimae_slow.setOutputLimits(0x0C, 0x7C);
+ hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = 0x7D - migimae_data[0];
+ true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
+
+ return 0;
+}
+//後進(fast mode)
int back_PID(int RF, int RB, int LF, int LB) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migimae.setInputLimits(-400, 400);
migiusiro.setInputLimits(-400, 400);
hidarimae.setInputLimits(-400, 400);
hidariusiro.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x84, 0xFF);
-
- // よくわからんやつ
+
+ // よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
-
- // 目標値
+
+ // 目標値
migimae.setSetPoint(RF);
migiusiro.setSetPoint(RB);
hidarimae.setSetPoint(LF);
hidariusiro.setSetPoint(LB);
-
- // センサ出力
+
+ // センサ出力
//目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
migimae.setProcessValue(abs(migimae_rpm));
migiusiro.setProcessValue(abs(migiusiro_rpm));
hidarimae.setProcessValue(abs(hidarimae_rpm));
hidariusiro.setProcessValue(abs(hidariusiro_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
-
+
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];
-
+
return 0;
}
+//後進(slow mode)
+int back_PID_slow(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x84, 0xFF);
+ migiusiro_slow.setOutputLimits(0x84, 0xFF);
+ hidarimae_slow.setOutputLimits(0x84, 0xFF);
+ hidariusiro_slow.setOutputLimits(0x84, 0xFF);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ 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];
+
+ return 0;
+}
+//右進(fast mode)
int right_PID(int RF, int RB, int LF, int LB) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migimae.setInputLimits(-400, 400);
migiusiro.setInputLimits(-400, 400);
hidarimae.setInputLimits(-400, 400);
hidariusiro.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x0C, 0x7C);
hidarimae.setOutputLimits(0x0C, 0x7C);
hidariusiro.setOutputLimits(0x84, 0xFF);
- // よくわからんやつ
+ // よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
- // 目標値
+ // 目標値
migimae.setSetPoint(RF);
migiusiro.setSetPoint(RB);
hidarimae.setSetPoint(LF);
hidariusiro.setSetPoint(LB);
-
- // センサ出力
+
+ // センサ出力
migimae.setProcessValue(abs(migimae_rpm));
migiusiro.setProcessValue(abs(migiusiro_rpm));
hidarimae.setProcessValue(abs(hidarimae_rpm));
hidariusiro.setProcessValue(abs(hidariusiro_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
- // 制御量をPWM値に変換
+ // 制御量をPWM値に変換
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
-
+
return 0;
}
+//右進(slow mode)
+int right_PID_slow(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x84, 0xFF);
+ migiusiro_slow.setOutputLimits(0x0C, 0x7C);
+ hidarimae_slow.setOutputLimits(0x0C, 0x7C);
+ hidariusiro_slow.setOutputLimits(0x84, 0xFF);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+
+ return 0;
+}
+//左進(fast mode)
int left_PID(int RF, int RB, int LF, int LB) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migimae.setInputLimits(-400, 400);
migiusiro.setInputLimits(-400, 400);
hidarimae.setInputLimits(-400, 400);
hidariusiro.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migimae.setOutputLimits(0x0C, 0x7C);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x0C, 0x7C);
- // よくわからんやつ
+ // よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
- // 目標値
+ // 目標値
migimae.setSetPoint(RF);
migiusiro.setSetPoint(RB);
hidarimae.setSetPoint(LF);
hidariusiro.setSetPoint(LB);
-
- // センサ出力
+
+ // センサ出力
migimae.setProcessValue(abs(migimae_rpm));
migiusiro.setProcessValue(abs(migiusiro_rpm));
hidarimae.setProcessValue(abs(hidarimae_rpm));
hidariusiro.setProcessValue(abs(hidariusiro_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
- // 制御量をPWM値に変換
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = 0x7D - migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
+
+ return 0;
+}
+//左進(slow mode)
+int left_PID_slow(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x0C, 0x7C);
+ migiusiro_slow.setOutputLimits(0x84, 0xFF);
+ hidarimae_slow.setOutputLimits(0x84, 0xFF);
+ hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
true_migimae_data[0] = 0x7D - migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
-
+
return 0;
}
-//斜め右前
+//斜め右前(fast mode)
int right_front_PID(int RB, int LF) {
- // センサ出力値の最小、最大
- migiusiro.setInputLimits(-400, 400);
- hidarimae.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+ // センサ出力値の最小、最大
+ migiusiro.setInputLimits(0, 400);
+ hidarimae.setInputLimits(0, 400);
+
+ // 制御量の最小、最大
migiusiro.setOutputLimits(0x0C, 0x7C);
hidarimae.setOutputLimits(0x0C, 0x7C);
- // よくわからんやつ
+ // よくわからんやつ
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
- // 目標値
+ // 目標値
migiusiro.setSetPoint(RB);
hidarimae.setSetPoint(LF);
-
- // センサ出力
+
+ // センサ出力
migiusiro.setProcessValue(abs(migiusiro_rpm));
hidarimae.setProcessValue(abs(hidarimae_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
- // 制御量をPWM値に変換
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
-
+ true_hidariusiro_data[0] = 0x80;
+
return 0;
}
-//斜め右後
+//斜め右後(fast mode)
int right_back_PID(int RF, int LB) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migimae.setInputLimits(-400, 400);
hidariusiro.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x84, 0xFF);
-
- // よくわからんやつ
+
+ // よくわからんやつ
migimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
-
- // 目標値
+
+ // 目標値
migimae.setSetPoint(RF);
hidariusiro.setSetPoint(LB);
-
- // センサ出力
+
+ // センサ出力
//目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
migimae.setProcessValue(abs(migimae_rpm));
hidariusiro.setProcessValue(abs(hidariusiro_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migimae_data[0] = migimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
-
+
true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = hidariusiro_data[0];
-
+
return 0;
}
-//斜め左前
+//斜め左前(fast mode)
int left_front_PID(int RF, int LB) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migimae.setInputLimits(-400, 400);
hidariusiro.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migimae.setOutputLimits(0x0C, 0x7C);
hidariusiro.setOutputLimits(0x0C, 0x7C);
- // よくわからんやつ
+ // よくわからんやつ
migimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
- // 目標値
+ // 目標値
migimae.setSetPoint(RF);
hidariusiro.setSetPoint(LB);
-
- // センサ出力
+
+ // センサ出力
migimae.setProcessValue(abs(migimae_rpm));
hidariusiro.setProcessValue(abs(hidariusiro_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migimae_data[0] = migimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
- // 制御量をPWM値に変換
+ // 制御量をPWM値に変換
true_migimae_data[0] = 0x7D - migimae_data[0];
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
-
+
return 0;
}
-//斜め左後
+//斜め左後(fast mode)
int left_back_PID(int RB, int LF) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migiusiro.setInputLimits(-400, 400);
hidarimae.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x84, 0xFF);
-
- // よくわからんやつ
+
+ // よくわからんやつ
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
-
- // 目標値
+
+ // 目標値
migiusiro.setSetPoint(RB);
hidarimae.setSetPoint(LF);
-
- // センサ出力
+
+ // センサ出力
//目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
migiusiro.setProcessValue(abs(migiusiro_rpm));
hidarimae.setProcessValue(abs(hidarimae_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
-
+
+ true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
-
+ true_hidariusiro_data[0] = 0x80;
+
return 0;
}
+//右旋回(fast mode)
int turn_right_PID(int RF, int RB, int LF, int LB) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migimae.setInputLimits(-400, 400);
migiusiro.setInputLimits(-400, 400);
hidarimae.setInputLimits(-400, 400);
hidariusiro.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migimae.setOutputLimits(0x84, 0xFF);
migiusiro.setOutputLimits(0x84, 0xFF);
hidarimae.setOutputLimits(0x0C, 0x7C);
hidariusiro.setOutputLimits(0x0C, 0x7C);
- // よくわからんやつ
+ // よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
- // 目標値
+ // 目標値
migimae.setSetPoint(RF);
migiusiro.setSetPoint(RB);
hidarimae.setSetPoint(LF);
hidariusiro.setSetPoint(LB);
-
- // センサ出力
+
+ // センサ出力
migimae.setProcessValue(abs(migimae_rpm));
migiusiro.setProcessValue(abs(migiusiro_rpm));
hidarimae.setProcessValue(abs(hidarimae_rpm));
hidariusiro.setProcessValue(abs(hidariusiro_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
- // 制御量をPWM値に変換
+ // 制御量をPWM値に変換
true_migimae_data[0] = migimae_data[0];
true_migiusiro_data[0] = migiusiro_data[0];
true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
-
+
return 0;
}
+//右旋回(slow mode)
+int turn_right_PID_slow(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x84, 0xFF);
+ migiusiro_slow.setOutputLimits(0x84, 0xFF);
+ hidarimae_slow.setOutputLimits(0x0C, 0x7C);
+ hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
+
+ return 0;
+}
+//左旋回(fast mode)
int turn_left_PID(int RF, int RB, int LF, int LB) {
- // センサ出力値の最小、最大
+ // センサ出力値の最小、最大
migimae.setInputLimits(-400, 400);
migiusiro.setInputLimits(-400, 400);
hidarimae.setInputLimits(-400, 400);
hidariusiro.setInputLimits(-400, 400);
-
- // 制御量の最小、最大
+
+ // 制御量の最小、最大
migimae.setOutputLimits(0x0C, 0x7C);
migiusiro.setOutputLimits(0x0C, 0x7C);
hidarimae.setOutputLimits(0x84, 0xFF);
hidariusiro.setOutputLimits(0x84, 0xFF);
- // よくわからんやつ
+ // よくわからんやつ
migimae.setMode(AUTO_MODE);
migiusiro.setMode(AUTO_MODE);
hidarimae.setMode(AUTO_MODE);
hidariusiro.setMode(AUTO_MODE);
- // 目標値
+ // 目標値
migimae.setSetPoint(RF);
migiusiro.setSetPoint(RB);
hidarimae.setSetPoint(LF);
hidariusiro.setSetPoint(LB);
-
- // センサ出力
+
+ // センサ出力
migimae.setProcessValue(abs(migimae_rpm));
migiusiro.setProcessValue(abs(migiusiro_rpm));
hidarimae.setProcessValue(abs(hidarimae_rpm));
hidariusiro.setProcessValue(abs(hidariusiro_rpm));
-
- // 制御量(計算結果)
+
+ // 制御量(計算結果)
migimae_data[0] = migimae.compute();
migiusiro_data[0] = migiusiro.compute();
hidarimae_data[0] = hidarimae.compute();
hidariusiro_data[0] = hidariusiro.compute();
- // 制御量をPWM値に変換
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = 0x7D - migimae_data[0];
+ true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+
+ return 0;
+}
+//左旋回(slow mode)
+int turn_left_PID_slow(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x0C, 0x7C);
+ migiusiro_slow.setOutputLimits(0x0C, 0x7C);
+ hidarimae_slow.setOutputLimits(0x84, 0xFF);
+ hidariusiro_slow.setOutputLimits(0x84, 0xFF);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
true_migimae_data[0] = 0x7D - migimae_data[0];
true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
true_hidarimae_data[0] = hidarimae_data[0];
true_hidariusiro_data[0] = hidariusiro_data[0];
-
+
+ return 0;
+}
+//前前進後右旋回(slow_mode)
+int front_front_back_right_PID(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x0C, 0x7C);
+ migiusiro_slow.setOutputLimits(0x0C, 0x7C);
+ hidarimae_slow.setOutputLimits(0x0C, 0x7C);
+ hidariusiro_slow.setOutputLimits(0x84, 0xFF);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = 0x7D - migimae_data[0];
+ true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+
return 0;
}
+//前前進後左旋回(slow_mode)
+int front_front_back_left_PID(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x0C, 0x7C);
+ migiusiro_slow.setOutputLimits(0x84, 0xFF);
+ hidarimae_slow.setOutputLimits(0x0C, 0x7C);
+ hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = 0x7D - migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
+
+ return 0;
+}
+//前右旋回後前進(slow_mode)
+int front_right_back_front_PID(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x84, 0xFF);
+ migiusiro_slow.setOutputLimits(0x0C, 0x7C);
+ hidarimae_slow.setOutputLimits(0x0C, 0x7C);
+ hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
+
+ return 0;
+}
+//前左旋回後前進(slow_mode)
+int front_left_back_front_PID(int RF, int RB, int LF, int LB) {
+ // センサ出力値の最小、最大
+ migimae_slow.setInputLimits(-400, 400);
+ migiusiro_slow.setInputLimits(-400, 400);
+ hidarimae_slow.setInputLimits(-400, 400);
+ hidariusiro_slow.setInputLimits(-400, 400);
+
+ // 制御量の最小、最大
+ migimae_slow.setOutputLimits(0x0C, 0x7C);
+ migiusiro_slow.setOutputLimits(0x0C, 0x7C);
+ hidarimae_slow.setOutputLimits(0x84, 0xFF);
+ hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
+
+ // よくわからんやつ
+ migimae_slow.setMode(AUTO_MODE);
+ migiusiro_slow.setMode(AUTO_MODE);
+ hidarimae_slow.setMode(AUTO_MODE);
+ hidariusiro_slow.setMode(AUTO_MODE);
+
+ // 目標値
+ migimae_slow.setSetPoint(RF);
+ migiusiro_slow.setSetPoint(RB);
+ hidarimae_slow.setSetPoint(LF);
+ hidariusiro_slow.setSetPoint(LB);
+
+ // センサ出力
+ migimae_slow.setProcessValue(abs(migimae_rpm));
+ migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
+ hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
+ hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
+
+ // 制御量(計算結果)
+ migimae_data[0] = migimae_slow.compute();
+ migiusiro_data[0] = migiusiro_slow.compute();
+ hidarimae_data[0] = hidarimae_slow.compute();
+ hidariusiro_data[0] = hidariusiro_slow.compute();
+
+ // 制御量をPWM値に変換
+ true_migimae_data[0] = 0x7D - migimae_data[0];
+ true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
+
+ return 0;
+}
+//ローラー
int roller_PID(int front, int back) {
front_roller.setInputLimits(-2000, 2000);
back_roller.setInputLimits(-2000, 2000);
-
+
front_roller.setOutputLimits(0x84, 0xFF);
back_roller.setOutputLimits(0x84, 0xFF);
-
+
front_roller.setMode(AUTO_MODE);
back_roller.setMode(AUTO_MODE);
-
+
front_roller.setSetPoint(front);
back_roller.setSetPoint(back);
-
+
front_roller.setProcessValue(abs(front_roller_rpm));
back_roller.setProcessValue(abs(back_roller_rpm));
-
+
front_roller_data[0] = front_roller.compute();
back_roller_data[0] = back_roller.compute();
-
+
return 0;
}
+//ラインセンサ基板との通信用関数
void linetrace() {
line_state = photo.getc();
+ //7bit目が1だったら7bit目を0に戻す
+ if((0b10000000 & line_state) == 0b10000000) {
+ back_line_state = 0b01111111 & line_state;
+ }
+ else {
+ front_line_state = line_state;
+ }
+ //4byte以上は出力できないよ
+ //pc.printf("%d\n\r", line_state);
+ //pc.printf("NAMA %d, front: %d, back: %d\n\r", line_state, front_line_state, back_line_state);
}
+//超音波センサ用関数
void ultrasonic() {
+ //超音波発射
sonic.start();
+ //10ms待機
wait(0.01);
//get_dist_cm関数は、計測から得られた距離(cm)を返します。
distance = sonic.get_dist_cm();
- //pc.printf("%d[cm]\r\n", distance);
+ //pc.printf("%d[cm]\r\n", distance);
}
int main(void) {
+ //こんにちは世界
pc.printf("HelloWorld\n");
+ //緊急停止用信号ピンをLow
emergency = 0;
- /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
+ //回転数取得関数のタイマ割り込み(40ms)
get_rps.attach_us(&flip, 40000);
- //get_distance.attach_us(&ultrasonic, 100000);
+
+ //フォトセンサ制御基板との通信ボーレートの設定(115200)
photo.baud(115200);
+ pc.baud(460800);
+ //フォトセンサ制御基板との通信関数の受信割り込み
photo.attach(linetrace, Serial::RxIrq);
+ //サイドチェンジボタンをPullDownに設定
side.mode(PullDown);
-
+ limit.mode(PullDown);
+ //超音波センサの温度設定
// setTemp関数は、HCSR04のメンバ変数temperature(気温を保持する変数)を引数として受け取った値に変更します。
sonic.setTemp(25);
-
+
+ //各MD, エアシリ制御基板へ起動後0.1秒間0x80を送りつける(通信切断防止用)
send_data[0] = 0x80;
i2c.write(0x10, send_data, 1);
i2c.write(0x12, send_data, 1);
@@ -678,212 +1304,529 @@
i2c.write(0x30, send_data, 1);
i2c.write(0x40, send_data, 1);
wait(0.1);
-
+
while(1) {
+
+ //超音波取得関数の呼び出し
ultrasonic();
- //pc.printf("%d[cm]\r\n", distance);
- if(side == 1){
+
+ //赤ゾーン選択
+ if(side == red){
red_side = 1;
blue_side = 0;
+ //青ゾーン選択
} else {
red_side = 0;
blue_side = 1;
}
- if(start_button == 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);
- */
- cylinder_data[0] = 0x80;
- myled1 = 0;
- i2c.write(0x40, cylinder_data, 1);
- } else {
- /*
- front_PID(270, 270, 270, 270);
- 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);
- */
+
+ //スタートボタン押したらエアシリ伸びる&start_flag立つ
+ if(start_button == 0){
+ //start_flag = 1;
myled1 = 1;
cylinder_data[0] = 0x0F;
i2c.write(0x40, cylinder_data, 1);
- }
+ } else {
+ myled1 = 0;
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+ }
if(user_switch2 == 0 && user_switch3 == 1){
+ myled2 = 1;
loading_data[0] = 0x0C;
- myled2 = 1;
i2c.write(0x30, loading_data, 1);
- }
+ }
else if(user_switch3 == 0 && user_switch2 == 1){
+ myled3 = 1;
loading_data[0] = 0xFF;
- myled3 = 1;
i2c.write(0x30, loading_data, 1);
} else {
- loading_data[0] = 0x80;
myled2 = 0;
myled3 = 0;
+ loading_data[0] = 0x80;
i2c.write(0x30, loading_data, 1);
- }
-
- /*
- loading_data[0] = 0x0C;
- i2c.write(0x30, loading_data, 1);
- if(photo_interrupter.read() == 1) {
- myled4 = 0;
- loading_state = 1;
- loading_data[0] = 0x80;
- i2c.write(0x30, loading_data, 1);
- }
- */
+ }
+
+ //ここからプロトタイプ移動プログラム
+ //pc.printf("pulse: %d, front: %d, back: %d\n\r", abs(measure_pulse), front_line_state, back_line_state);
+ //pc.printf("FR: %d, BR: %d, FL: %d, BL: %d\n\r", abs(migimae_pulse), abs(migiusiro_pulse), abs(hidarimae_pulse), abs(hidariusiro_pulse));
/*
- if(user_switch5 == 0){
- roller_flag = 1;
+ //パルスが0以上10000未満の間高速右移動
+ if(abs(measure_pulse) < 10000 && abs(measure_pulse) >= 0) {
+ right_PID(255, 300, 255, 300);
+ 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(roller_flag == 1) {
- myled5 = 1;
- timer.reset();
- timer.start();
- roller_PID(325, 650);
- i2c.write(0x20, front_roller_data, 1, false);
- i2c.write(0x22, back_roller_data, 1, false);
- wait_us(20);
-
- if((timer.read() >= 5.0f) && (timer.read() < 5.5f)) {
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- }
- else if((timer.read() >= 5.5f) && (timer.read() < 6.0f)) {
- cylinder_data[0] = 0x80;
- i2c.write(0x40, cylinder_data, 1);
+ //パルスが10000以上になったら低速右移動開始
+ else if(abs(measure_pulse) >= 10000) {
+ right_PID_slow(93, 100, 93, 100);
+ 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);
+ */
+ //距離が11cm~29cmだったらトレースせずに停止
+ if(distance > 10 && distance < 30) {
+ //タイマスタート
+ timer.start();
+ //トレース方向の反転
+ trace_direction = 1;
+ //テーブル検知して1秒たったら後進開始
+ if(timer.read() > 1.0f) {
+ line_state_pettern = back_trace;
+ pc.printf("start_back!\n\r");
+ } else {
+ line_state_pettern = stop;
+ }
+
+ //上記以外の距離でライントレースするよん
+ } else {
+ //タイマリセット
+ timer.reset();
+ /*
+ //リミットスイッチが押されたら
+ if(limit.read() == 0) {
+ //トレース方向の反転
+ trace_direction = 0;
+ //タイマスタート
+ timer.start();
+ //1.5秒間右移動
+ if(timer.read() < 1.5f) {
+ line_state_pettern = right_slow;
+ } else {
+ timer.reset();
+ }
+ */
+
+ //ライン検知するまで右移動
+ if((front_line_state == 0) && (back_line_state == 0)) {
+ //青ゾーンの時ライン検知するまで右移動
+ if(side == blue) {
+ line_state_pettern = right_slow;
+ //赤ゾーンの時ライン検知するまで左移動
+ } else {
+ line_state_pettern = left_slow;
+ }
+ }
+ /*
+ //前はライン検知してるけど後ろは検知できない時右移動
+ else if((front_line_state >= 1) && (back_line_state == 0)) {
+ line_state_pettern = right_super_slow;
+ }
+ //前はライン検知できないけど後ろは検知してる時右移動
+ else if((front_line_state == 0) && (back_line_state >= 1)) {
+ line_state_pettern = right_super_slow;
+ }
+ */
+
+ //前が右寄りの時
+ else if((front_line_state <= 5) && (front_line_state != 0)) {
+
+ //前も後ろも右寄りの時右移動
+ if((back_line_state <= 5) && (back_line_state != 0)) {
+ line_state_pettern = right_super_slow;
+ }
+ //前が右寄りで後ろが真ん中の時前右旋回後ろ前進
+ else if((back_line_state >= 6) && (back_line_state <= 12)) {
+ line_state_pettern = front_right_back_front;
+ }
+ //前が右寄りで後ろが左寄りの時左旋回
+ else if((back_line_state >= 13) && (back_line_state <= 17)) {
+ line_state_pettern = turn_left;
+ }
+
+ }
+ //前が真ん中寄りの時
+ else if((front_line_state >= 6) && (front_line_state <= 12)) {
+
+ //前が真ん中で後ろが右寄りの時前前進後ろ右旋回
+ if((back_line_state <= 5) && (back_line_state != 0)) {
+ line_state_pettern = front_front_back_right;
+ }
+ //前も後ろも真ん中の時前進
+ else if((back_line_state >= 6) && (back_line_state <= 12)) {
+ if(trace_direction == 0) {
+ line_state_pettern = front_trace;
+ }
+ else if(trace_direction == 1) {
+ line_state_pettern = back_trace;
+ /*
+ //リミットスイッチが押されたら
+ if(limit.read() == 0) {
+ //トレース方向の反転
+ trace_direction = 0;
+ right_flag = 1;
+ //timer.start();
+ //1.5秒間右移動
+ //if(timer.read() < 1.5f) {
+ //line_state_pettern = right_slow;
+ //} else {
+ //timer.reset();
+ //}
+ }
+ */
+ }
+ }
+ //前が真ん中で後ろが左寄りの時前前進後ろ左旋回
+ else if((back_line_state >= 13) && (back_line_state <= 17)) {
+ line_state_pettern = front_front_back_left;
+ }
+ }
+ //前が左寄りの時
+ else if((front_line_state >= 13) && (front_line_state <= 17)) {
+
+ //前が左寄りで後ろが右寄りの時右旋回
+ if((back_line_state <= 5) && (back_line_state != 0)) {
+ line_state_pettern = turn_right;
+ }
+ //前が左寄りで後ろが真ん中の時前左旋回後ろ前進
+ else if((back_line_state >= 6) && (back_line_state <= 12)) {
+ line_state_pettern = front_left_back_front;
+ }
+ //前が左よりで後ろも左寄りの時左移動
+ else if((back_line_state >= 13) && (back_line_state <= 17)) {
+ line_state_pettern = left_super_slow;
+ }
+
+ //それ以外
+ } else {
+ line_state_pettern = stop;
+ }
+ //スイッチが押されていないとき(壁から離れたとき)
+ /*
+ } else {
+ //後進し続ける
+ line_state_pettern = back_trace;
+ }*/
}
- else if((timer.read() >= 6.0f) && (timer.read() < 6.5f)) {
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- }
- else if((timer.read() >= 6.5f) && (timer.read() < 7.0f)) {
- cylinder_data[0] = 0x80;
- i2c.write(0x40, cylinder_data, 1);
- }
- else if((timer.read() >= 7.0f) && (timer.read() < 7.5f)) {
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- } else {
- cylinder_data[0] = 0x80;
- i2c.write(0x40, cylinder_data, 1);
- }
- timer.stop();
- } else {
- myled5 = 0;
- front_roller_data[0] = 0x80;
- back_roller_data[0] = 0x80;
- i2c.write(0x20, front_roller_data, 1, false);
- i2c.write(0x22, back_roller_data, 1, false);
- }
- */
+ //}
+
+ pc.printf("F%3d B%3d P%3d %3d %3d\n\r", front_line_state, back_line_state, line_state_pettern, abs(measure_pulse), right_flag);
- /*
- if(distance < 30 && distance != 0) {
- //front_PID(200, 200, 200, 200);
- //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);
- } else {
- //send_data[0] = 0x80;
- //i2c.write(0x10, send_data, 1, false);
- //i2c.write(0x12, send_data, 1, false);
- //i2c.write(0x14, send_data, 1, false);
- //i2c.write(0x16, send_data, 1, false);
+ //right_flagが0(初期状態)の時
+ if(right_flag == 0) {
+ //リミットONでright_flag = 1
+ if(limit.read() == 0) {
+ right_flag = 1;
+ }
}
- */
-
- //pc.printf("%d\n", line_state);
- if(line_state == 0){
- pc.printf("真ん中\n");
- front_PID(70, 70, 70, 70);
- 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);
+ //リミットがONの時(1回目)
+ if(right_flag == 1) {
+ //トレース方向の反転(前進)
+ trace_direction = 0;
+ back_timer1.start();
+ if(back_timer1.read() <= 1.5f && back_timer1.read() != 0.0f) {
+ line_state_pettern = right_slow;
+ }
+ else if(back_timer1.read() > 1.5f) {
+ back_timer1.reset();
+ right_flag = 2;
+ }
+ }
+ else if(right_flag == 2) {
+ if(limit.read() == 0) {
+ right_flag = 3;
+ }
}
- else if(line_state == 1) {
- pc.printf("右\n");
- turn_right_PID(30, 30, 30, 30);
- //right_front_PID(50, 50);
- 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);
+ //リミットがONの時(2回目)
+ else if(right_flag == 3) {
+ trace_direction = 0;
+ back_timer2.start();
+ if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
+ line_state_pettern = right_slow;
+ }
+ else if(back_timer2.read() > 0.8f) {
+ back_timer2.reset();
+ right_flag = 4;
+ }
+ }
+ else if(right_flag == 4) {
+ if(limit.read() == 0) {
+ right_flag = 5;
+ }
}
- else if(line_state == 2) {
- pc.printf("左\n");
- turn_left_PID(30, 30, 30, 30);
- //left_front_PID(50, 50);
- 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);
- }
- else if(line_state == 3) {
- pc.printf("範囲外\n");
- send_data[0] = 0x80;
- i2c.write(0x10, send_data, 1, false);
- i2c.write(0x12, send_data, 1, false);
- i2c.write(0x14, send_data, 1, false);
- i2c.write(0x16, send_data, 1, false);
+ //リミットがONの時(3回目)
+ else if(right_flag == 5) {
+ trace_direction = 0;
+ back_timer3.start();
+ if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
+ line_state_pettern = right_slow;
+ }
+ else if(back_timer3.read() > 0.8f) {
+ back_timer3.reset();
+ right_flag = 6;
+ }
+ }
+ else if(right_flag == 6) {
+ if(limit.read() == 0) {
+ right_flag = 7;
+ }
+ }
+ //リミットがONの時(4回目)
+ else if(right_flag == 7) {
+ //スタートゾーンに近づいたら減速
+ if(abs(measure_pulse) < 1200) {
+ line_state_pettern = left_super_slow;
+ }
+ else if(abs(measure_pulse) < 500) {
+ line_state_pettern = stop;
+ } else {
+ line_state_pettern = left_slow;
+ }
}
/*
- front_PID(270, 270, 270, 270);
+ if(right_flag == 1) {
+ line_state_pettern = front_trace;
+ wait(2);
+ line_state_pettern = right_slow;
+ wait(2);
+ right_flag = 2;
+ }
+ */
+
+ switch(line_state_pettern) {
+
+ //青ゾーンでライン検知しないと低速右移動
+ case right_slow:
+ right_PID_slow(50, 51, 50, 51);
+ 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);
+ break;
+
+ //赤ゾーンでライン検知しないと低速左移動
+ case left_slow:
+ left_PID_slow(50, 50, 50, 50);
+ 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);
+ break;
+
+ //超低速右移動
+ case right_super_slow:
+ right_PID_slow(10, 10, 10, 10);
+ 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);
+ break;
+
+ //超低速左移動
+ case left_super_slow:
+ left_PID_slow(10, 10, 10, 10);
+ 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);
+ break;
+
+ //右旋回
+ case turn_right:
+ turn_right_PID_slow(10, 10, 10, 10);
+ 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);
+ break;
+
+ //左旋回
+ case turn_left:
+ turn_left_PID_slow(10, 10, 10, 10);
+ 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);
+ break;
+
+ //前進
+ case front_trace:
+ front_PID_slow(30, 30, 30, 30);
+ //front_PID_slow(50, 50, 50, 50);
+ 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);
+ break;
+ //後進
+ case back_trace:
+ back_PID_slow(30, 30, 30, 30);
+ //back_PID_slow(50, 50, 50, 50);
+ 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);
+ break;
+
+ //前前進後ろ右旋回
+ case front_front_back_right:
+ front_front_back_right_PID(30, 30, 30, 30);
+ //true_hidarimae_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);
+ break;
+
+ //前前進後ろ左旋回
+ case front_front_back_left:
+ front_front_back_left_PID(30, 30, 30, 30);
+ //true_migimae_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);
+ break;
+
+ //前右旋回後ろ前進
+ case front_right_back_front:
+ front_right_back_front_PID(30, 30, 30, 30);
+ //true_migiusiro_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);
+ break;
+
+ //前左旋回後ろ前進
+ case front_left_back_front:
+ front_left_back_front_PID(30, 30, 30, 30);
+ //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);
+ break;
+
+
+ //それ以外ショートブレーキ
+ default:
+ /*
+ front_PID_slow(30, 30, 30, 30);
+ 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);
+ break;
+ */
+
+ 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);
+ break;
+
+ }
+
+ /*
+ //前進
+ front_PID(300, 300, 300, 300);
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(270, 270, 270, 270);
+
+ //後進
+ back_PID(300, 300, 300, 300);
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(100, 100, 100, 100);
+
+ //右進行
+ //right_PID(255, 300, 255, 300);
+ right_PID_slow(93, 100, 93, 100);
+ 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(300, 255, 300, 255);
+ 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_front_PID(300, 300);
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(100, 100, 100, 100);
+
+ //斜め右後
+ right_back_PID(300, 300);
+ 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_front_PID(300, 300);
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_front_PID(270, 270);
- true_migiusiro_data[0] = 0x80;
- true_hidariusiro_data[0] = 0x80;
+
+ //斜め左後
+ left_back_PID(300, 300);
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_back_PID(270, 270);
- true_migimae_data[0] = 0x80;
+
+ //右旋回
+ turn_right_PID(300, 300, 300, 300);
+ 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);
+
+ //左旋回
+ turn_left_PID(300, 300, 300, 300);
+ 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_front_back_right_PID(30, 30, 30, 30);
true_hidarimae_data[0] = 0x80;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -891,19 +1834,26 @@
i2c.write(0x16, true_hidariusiro_data,1, false);
wait_us(20);
- //斜め左前
- left_front_PID(270, 270);
+ //前前進後ろ左旋回
+ front_front_back_left_PID(30, 30, 30, 30);
true_migimae_data[0] = 0x80;
- true_hidarimae_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);
- //斜め左後
- left_back_PID(270, 270);
+ //前右旋回後ろ前進
+ front_right_back_front_PID(30, 30, 30, 30);
true_migiusiro_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);
+
+ //前左旋回後ろ前進
+ front_left_back_front_PID(30, 30, 30, 30);
true_hidariusiro_data[0] = 0x80;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -911,56 +1861,56 @@
i2c.write(0x16, true_hidariusiro_data,1, false);
wait_us(20);
- turn_right_PID(200, 200, 200, 200);
- 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);
-
- turn_left_PID(100, 100, 100, 100);
- 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);
-
- roller_PID(1000, 1000);
+ //ローラーぐるぐる(max930rpm)
+ roller_PID(440, 400);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
- wait_us(20);
+ wait_us(20);
*/
-
+
+ /*
+ if(front_roller_rpm > 500) {
+ cylinder_data[0] = 0x0F;
+ i2c.write(0x40, cylinder_data, 1);
+ myled5 = 1;
+ wait(0.5);
+
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+ myled5 = 0;
+ wait(0.5);
+ } else {
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+ myled5 = 0;
+ wait(0.5);
+ }
+ */
/*
//どんどん加速(逆転)
- for(send_data[0] = 0x81; send_data[0] < 0xF5; send_data[0]++){
- //ice(0x88, send_data);
- //ice(0xA2, send_data);
-
- i2c.write(0xA0, send_data, 1);
- i2c.write(0xA2, send_data, 1);
- i2c.write(0xA4, send_data, 1);
- i2c.write(0xA6, send_data, 1);
-
- i2c.write(0x20, send_data, 1);
+ for(send_data[0] = 0x84; send_data[0] < 0xFF; send_data[0]++){
+ i2c.write(0x10, send_data, 1);
+ i2c.write(0x12, send_data, 1);
+ i2c.write(0x14, send_data, 1);
+ i2c.write(0x16, send_data, 1);
wait(0.1);
}
//だんだん減速(逆転)
- for(send_data[0] = 0xF4; send_data[0] > 0x80; send_data[0]--){
+ for(send_data[0] = 0x0C; send_data[0] > 0x7C; send_data[0]--){
//ice(0x88, send_data);
//ice(0xA2, send_data);
-
- i2c.write(0xA0, send_data, 1);
- i2c.write(0xA2, send_data, 1);
- i2c.write(0xA4, send_data, 1);
- i2c.write(0xA6, send_data, 1);
-
- i2c.write(0x20, send_data, 1);
+
+ i2c.write(0x10, send_data, 1);
+ i2c.write(0x12, send_data, 1);
+ i2c.write(0x14, send_data, 1);
+ i2c.write(0x16, send_data, 1);
wait(0.1);
}
- send_data[0] = 0x80;
- i2c.write(0x20, send_data, 1);
- wait(5);
*/
}
}
+
+
+
+
+