INCTRC Information Sharing Test
2018/09/05現在のBチーム自動機の制御プログラム(バックアップも兼ねて)
Dependencies: HCSR04 PID QEI mbed
Fork of
Lets_move_Seki2018
by 
INCTRC Information Sharing Test
Diff: main.cpp
- Revision:
- 4:df334779a69e
- Parent:
- 3:1223cab367d9
--- a/main.cpp Mon Aug 27 08:15:36 2018 +0000
+++ b/main.cpp Wed Sep 05 15:07:15 2018 +0000
@@ -1,293 +1,934 @@
-/* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
-/* 吉田啓人氏が頑張って改善したMDプログラム用に改善しました */
-/* 具体的には取得したパルス数を分解能で割り算→掛け算でRPSへ変換から */
-/* 取得したパルス数を掛け算→分解能で割ってRPSへ変換としている。 */
-/* ロリコンにはTIM2~TIM5がいいらしい(Nucleo_F401re) */
-/* TIM2(CH1: NC, CH2: D3(PB_3), CH3: D6(PB_10), CH4: NC) */
-/* TIM3(CH1: D5(PB_4), CH2: D9(PC_7), CH3: NC, CH4: NC) */
-/* TIM4(CH1: D10(PB_6), CH2: NC, CH3: NC, CH4: NC) */
-/* TIM5(CH1: NC, CH2: NC, CH3: NC, CH4: NC) */
-/* 2018/6/9追記 */
-/* ついに回転数の！PI制御を実現できました。 */
-/* タイマ割込みで角速度を取得してみようと思います。 */
-
+/* ------------------------------------------------------------------- */
+/* NHKロボコン2018-Bチーム自動ロボット(設計者: 4S 関) */
+/* ------------------------------------------------------------------- */
+/* このプログラムは上記のロボット専用の制御プログラムである。 */
+/* ------------------------------------------------------------------- */
+/* 対応機種: NUCLEO-F446RE */
+/* ------------------------------------------------------------------- */
+/* 製作者: 4D 高久 雄飛, mail: rab1sy23@gmail.com */
+/* ------------------------------------------------------------------- */
+/* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */
+/* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */
+/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
+#include "hcsr04.h"
#define PI 3.14159265359
+#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
+
+//右前オムニ
+PID migimae(Kp, Ki, Kd, 0.001);
+//右後オムニ
+PID migiusiro(Kp, Ki, Kd, 0.001);
+//左前オムニ
+PID hidarimae(Kp, Ki, Kd, 0.001);
+//左後オムニ
+PID hidariusiro(Kp, Ki, Kd, 0.001);
+//前ローラー
+PID front_roller(roller_Kp, roller_Ki, 0.0, 0.001);
+//後ローラー
+PID back_roller(roller_Kp, roller_Ki, 0.0, 0.001);
-PID controller(0.7, 0.7, 0.0, 0.001);
-PID RF(0.5, 0.4, 0.0, 0.001);
-PID RB(0.5, 0.3, 0.0, 0.001);
-PID LF(0.5, 0.3, 0.0, 0.001);
-PID LB(0.5, 0.3, 0.0, 0.001);
-I2C i2c(PB_3, PB_10); //SDA, SCL
-Serial pc(USBTX, USBRX);
-Serial photo(D8, D2);
+//MDとの通信ポート
+I2C i2c(PB_9, PB_8); //SDA, SCL
+//PCとの通信ポート
+Serial pc(USBTX, USBRX); //TX, RX
+//フォトインタラプタ制御基板からの受信ポート
+Serial photo(NC, PC_11); //TX, RX
+//TWE-Liteからの受信ポート
+Serial twe(PC_12, PD_2); //TX, RX
+
+//超音波センサ1
+HCSR04 sonic(PB_3, PB_10); //Trig, Echo
+//超音波センサ2
+//HCSR04 sonic2(PC_13, PA_15); //Trig, Echo
+//超音波センサ3
+//HCSR04 sonic3(PC_15, PC_14); //Trig, Echo
+//超音波センサ4
+//HCSR04 sonic4(PH_1 , PH_0 ); //Trig, Echo
+
+//赤、青ゾーン切り替え用スイッチ
+DigitalIn side(PC_1);
+//スタートボタン
+DigitalIn start_button(PC_4);
+//スイッチ1
+DigitalIn user_switch1(PB_0);
+//スイッチ2
+DigitalIn user_switch2(PA_4);
+//スイッチ3
+DigitalIn user_switch3(PA_3);
+//スイッチ4
+//以下の文を入れるとロリコンが読めなくなる
+//DigitalIn user_switch4(PA_2);
+//スイッチ5
+DigitalIn user_switch5(PA_10);
+
+//フォトインタラプタ
+DigitalIn photo_interrupter(PA_15);
+
+//12V停止信号ピン
DigitalOut emergency(PA_13);
-DigitalOut myled(D13);
+
+//赤ゾーンLED
+DigitalOut blue_side(PC_0);
+//青ゾーンLED
+DigitalOut red_side(PC_3);
+//スタートLED
+DigitalOut start_LED(PA_8);
+//LED1
+DigitalOut myled1(PC_6);
+//LED2
+DigitalOut myled2(PC_5);
+//LED3
+DigitalOut myled3(PA_12);
+//LED4
+DigitalOut myled4(PB_1);
+//LED5
+DigitalOut myled5(PA_5);
-/* 以下446基板用 */
-QEI wheel_LB(PA_1, PA_0, NC, 624);
-QEI wheel_RB(PB_6, PB_7, NC, 624);
-QEI wheel_LF(PB_4, PB_5, NC, 624);
-QEI wheel_RF(PC_8, PC_9, NC, 624);
+//前ローラー
+QEI front_roller_wheel(PA_0, PA_1, NC, 624);
+//後ローラー
+QEI back_roller_wheel(PB_5, PB_4, NC, 624);
+//計測オムニ1
+//QEI measure1_wheel(PC_9, PC_8, NC, 624);
+//計測オムニ2(使用不可)
+//QEI measure2_wheel(PB_3, PB_10, NC, 624);
+//右前オムニ
+QEI migimae_wheel(PB_6 , PA_7 , NC, 624);
+//右後オムニ
+QEI migiusiro_wheel(PA_11, PB_12, NC, 624);
+//左前オムニ
+QEI hidarimae_wheel(PB_2, PB_15, NC, 624);
+//左後オムニ
+QEI hidariusiro_wheel(PB_14, PB_13, NC, 624);
-Ticker get_RPS;
-Ticker print_RPS;
+Ticker get_rps;
+Ticker get_distance;
Timer timer;
-int rps_RF;
-int rps_RB;
-int rps_LF;
-int rps_LB;
+int roller_flag = 0;
+int loading_state = 0;
-int rpm_RF;
-int rpm_RB;
-int rpm_LF;
-int rpm_LB;
+int migimae_rpm;
+int migiusiro_rpm;
+int hidarimae_rpm;
+int hidariusiro_rpm;
+int measure1_rpm;
+//int measure2_rpm;
+int front_roller_rpm;
+int back_roller_rpm;
-int pulse_RF;
-int pulse_RB;
-int pulse_LF;
-int pulse_LB;
+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 counter_RF;
-int counter_RB;
-int counter_LF;
-int counter_LB;
+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];
-double a_v; /* 角速度 */
-double n_v; /* 速度(秒速) */
-double h_v; /* 速度(時速) */
-double n_a; /* 加速度 */
+
+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;
char send_data[1];
char true_send_data[1];
-char RF_data[1];
-char RB_data[1];
-char LF_data[1];
-char LB_data[1];
-char true_RF_data[1];
-char true_RB_data[1];
-char true_LF_data[1];
-char true_LB_data[1];
+char migimae_data[1];
+char migiusiro_data[1];
+char hidarimae_data[1];
+char hidariusiro_data[1];
+char front_roller_data[1];
+char back_roller_data[1];
+char loading_data[1];
+char cylinder_data[1];
+
+char true_migimae_data[1];
+char true_migiusiro_data[1];
+char true_hidarimae_data[1];
+char true_hidariusiro_data[1];
+
+int line_state = 0;
+
+unsigned int distance;
/* ロリコン処理関数 */
void flip();
-/* RPS表示関数 */
-void flip2();
+int front_PID(int RF, int RB, int LF, int LB);
+int back_PID(int RF, int RB, int LF, int LB);
+int rihgt_PID(int RF, int RB, int LF, int LB);
+int left_PID(int RF, int RB, int LF, int LB);
+int right_front_PID(int RB, int LF);
+int right_back_PID(int RF, int LB);
+int left_front_PID(int RF, int LB);
+int left_back_PID(int RB, int RF);
+int turn_right_PID(int RF, int RB, int LF, int LB);
+int turn_left_PID(int RF, int RB, int LF, int LB);
+int roller_PID(int front, int back);
+void linetrace();
+void ultrasonic();
-void flip(){
+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();
+
+ //((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;
- pulse_RF = wheel_RF.getPulses();
- pulse_RB = wheel_RB.getPulses();
- pulse_LF = wheel_LF.getPulses();
- pulse_LB = wheel_LB.getPulses();
+ //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);
+
+ 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();
+}
+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);
- /* *rps変換 */
- /*10ms*100 = 1s
- counter_RB = pulse_RB * 100;
- counter_LB = pulse_LB * 100;
- */
-
- //40ms*25 = 1s
- counter_RF = pulse_RF * 25;
- counter_RB = pulse_RB * 25;
- counter_LF = pulse_LF * 25;
- counter_LB = pulse_LB * 25;
-
- /*
- //100ms*10 = 1s
- counter_RB = pulse_RB * 10;
- counter_LB = pulse_LB * 10;
- */
+ // 制御量の最小、最大
+ 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);
- /* /分解能 */
- rps_RF = counter_RF / 100;
- rps_RB = counter_RB / 100;
- rps_LF = counter_LF / 100;
- rps_LB = counter_LB / 100;
-
- rpm_RF = pulse_RF * 25 * 60 / 100;
- rpm_RB = pulse_RB * 25 * 60 / 100;
- rpm_LF = pulse_LF * 25 * 60 / 100;
- rpm_LB = pulse_LB * 25 * 60 / 100;
-
-
- /* RPMから角速度へ変換 */
- a_v = rpm_LB * PI / 30;
-
- /* RPMから速度(秒速)へ変換 */
- /* タイヤ半径を0.05[m]とする */
- n_v = 0.05 * 2 * PI * rpm_LB / 60;
-
- /* 速度(秒速)から速度(時速)へ変換 */
- h_v = n_v * 3600 / 1000;
-
- //pc.printf("RF: %d RB: %d LF: %d LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
- pc.printf("前: %d, 後: %d\n", rpm_LB, rpm_LF);
- //pc.printf("%d\n", rpm_LB);
+ // 目標値
+ 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();
- wheel_RF.reset();
- wheel_RB.reset();
- wheel_LF.reset();
- wheel_LB.reset();
+ // 制御量を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;
}
-int front_PID(int setpoint)
-{
+int back_PID(int RF, int RB, int LF, int LB) {
// センサ出力値の最小、最大
- RF.setInputLimits(-2000, 2000);
- RB.setInputLimits(-2000, 2000);
- LF.setInputLimits(-2000, 2000);
- LB.setInputLimits(-2000, 2000);
+ 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;
+}
+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);
// 制御量の最小、最大
- RF.setOutputLimits(0x0C, 0x7C);
- RB.setOutputLimits(0x0C, 0x7C);
- LF.setOutputLimits(0x0C, 0x7C);
- LB.setOutputLimits(0x0C, 0x7C);
+ 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値に変換
+ 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;
+}
+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);
// よくわからんやつ
- RF.setMode(AUTO_MODE);
- RB.setMode(AUTO_MODE);
- LF.setMode(AUTO_MODE);
- LB.setMode(AUTO_MODE);
+ 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値に変換
+ 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;
+}
+//斜め右前
+int right_front_PID(int RB, int LF) {
+ // センサ出力値の最小、最大
+ migiusiro.setInputLimits(-400, 400);
+ hidarimae.setInputLimits(-400, 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値に変換
+ true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
+
+ return 0;
+}
+//斜め右後
+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);
// 目標値
- RF.setSetPoint(setpoint);
- RB.setSetPoint(setpoint);
- LF.setSetPoint(setpoint);
- LB.setSetPoint(setpoint);
+ migimae.setSetPoint(RF);
+ hidariusiro.setSetPoint(LB);
// センサ出力
- RF.setProcessValue(rpm_RF);
- RB.setProcessValue(rpm_RB);
- LF.setProcessValue(rpm_LF);
- LB.setProcessValue(rpm_LB);
+ //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
+ migimae.setProcessValue(abs(migimae_rpm));
+ hidariusiro.setProcessValue(abs(hidariusiro_rpm));
// 制御量(計算結果)
- RF_data[0] = RF.compute();
- RB_data[0] = RB.compute();
- LF_data[0] = LF.compute();
- LB_data[0] = LB.compute();
+ migimae_data[0] = migimae.compute();
+ hidariusiro_data[0] = hidariusiro.compute();
+
+ true_migimae_data[0] = migimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+ return 0;
+}
+//斜め左前
+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値に変換
- true_RF_data[0] = 0x7D - RF_data[0];
- true_RB_data[0] = 0x7D - RB_data[0];
- true_LF_data[0] = 0x7D - LF_data[0];
- true_LB_data[0] = 0x7D - LB_data[0];
+ true_migimae_data[0] = 0x7D - migimae_data[0];
+ true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
return 0;
}
-int back_PID(int setpoint1, int setpoint2)
-{
+//斜め左後
+int left_back_PID(int RB, int LF) {
// センサ出力値の最小、最大
- RF.setInputLimits(0, 2000);
- RB.setInputLimits(0, 2000);
- LF.setInputLimits(0, 2000);
- LB.setInputLimits(0, 2000);
+ migiusiro.setInputLimits(-400, 400);
+ hidarimae.setInputLimits(-400, 400);
// 制御量の最小、最大
- RF.setOutputLimits(0x84, 0xFF);
- RB.setOutputLimits(0x84, 0xFF);
- LF.setOutputLimits(0x84, 0xFF);
- LB.setOutputLimits(0x84, 0xFF);
-
+ migiusiro.setOutputLimits(0x84, 0xFF);
+ hidarimae.setOutputLimits(0x84, 0xFF);
+
// よくわからんやつ
- RF.setMode(AUTO_MODE);
- RB.setMode(AUTO_MODE);
- LF.setMode(AUTO_MODE);
- LB.setMode(AUTO_MODE);
+ migiusiro.setMode(AUTO_MODE);
+ hidarimae.setMode(AUTO_MODE);
// 目標値
- RF.setSetPoint(setpoint1);
- RB.setSetPoint(setpoint1);
- LF.setSetPoint(setpoint1);
- LB.setSetPoint(setpoint2);
+ migiusiro.setSetPoint(RB);
+ hidarimae.setSetPoint(LF);
// センサ出力
//目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
- RF.setProcessValue(abs(rpm_RF));
- RB.setProcessValue(abs(rpm_RB));
- LF.setProcessValue(abs(rpm_LF));
- LB.setProcessValue(abs(rpm_LB));
+ migiusiro.setProcessValue(abs(migiusiro_rpm));
+ hidarimae.setProcessValue(abs(hidarimae_rpm));
// 制御量(計算結果)
- RF_data[0] = RF.compute();
- RB_data[0] = RB.compute();
- LF_data[0] = LF.compute();
- LB_data[0] = LB.compute();
+ migiusiro_data[0] = migiusiro.compute();
+ hidarimae_data[0] = hidarimae.compute();
+
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
return 0;
}
-int main(void)
-{
+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値に変換
+ 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;
+}
+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値に変換
+ 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;
+}
+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();
+}
+void ultrasonic() {
+ sonic.start();
+ wait(0.01);
+ //get_dist_cm関数は、計測から得られた距離(cm)を返します。
+ distance = sonic.get_dist_cm();
+ //pc.printf("%d[cm]\r\n", distance);
+}
+int main(void) {
+ pc.printf("HelloWorld\n");
emergency = 0;
+ /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
+ get_rps.attach_us(&flip, 40000);
+ //get_distance.attach_us(&ultrasonic, 100000);
+ photo.baud(115200);
+ photo.attach(linetrace, Serial::RxIrq);
+ side.mode(PullDown);
+
+ // setTemp関数は、HCSR04のメンバ変数temperature(気温を保持する変数)を引数として受け取った値に変更します。
+ sonic.setTemp(25);
+
send_data[0] = 0x80;
- //i2c.write(0x10, send_data, 1);
- //i2c.write(0x12, send_data, 1);
- //i2c.write(0x14, send_data, 1);
- //i2c.write(0x16, 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);
i2c.write(0x20, send_data, 1);
i2c.write(0x22, send_data, 1);
+ i2c.write(0x30, send_data, 1);
+ i2c.write(0x40, send_data, 1);
wait(0.1);
- /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
- get_RPS.attach_us(&flip, 40000);
- //get_RPS.attach_us(&flip, 100000);
-
- while(1)
- {
+ while(1) {
+ ultrasonic();
+ //pc.printf("%d[cm]\r\n", distance);
+ if(side == 1){
+ 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);
+ */
+ myled1 = 1;
+ cylinder_data[0] = 0x0F;
+ i2c.write(0x40, cylinder_data, 1);
+ }
+ if(user_switch2 == 0 && user_switch3 == 1){
+ loading_data[0] = 0x0C;
+ myled2 = 1;
+ i2c.write(0x30, loading_data, 1);
+ }
+ else if(user_switch3 == 0 && user_switch2 == 1){
+ loading_data[0] = 0xFF;
+ myled3 = 1;
+ i2c.write(0x30, loading_data, 1);
+ } else {
+ loading_data[0] = 0x80;
+ myled2 = 0;
+ myled3 = 0;
+ i2c.write(0x30, loading_data, 1);
+ }
+
/*
- front_PID(300);
- i2c.write(0x20, true_LB_data, 1, false);
- i2c.write(0x22, true_LF_data, 1, false);
- wait_us(20);
+ 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);
+ }
*/
+ /*
+ if(user_switch5 == 0){
+ roller_flag = 1;
+ }
+ 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);
+ }
+ 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);
+ }
+ */
- //back_PID(430, 730);
- //back_PID(300, 500);
- back_PID(325, 650);
- //back_PID(250, 900);
- //LB_data[0] = 0x0C;
- //LF_data[0] = 0x0C;
- i2c.write(0x10, LB_data, 1, false);
- i2c.write(0x22, LF_data, 1, false);
- wait_us(20);
+ /*
+ 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);
+ }
+ */
+
+ //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);
+ }
+ 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);
+ }
+ 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);
+ }
/*
- timer.start();
- while(timer.read() <= 5.0f) {
- myled = 1;
- front_PID(200);
- i2c.write(0x20, true_LF_data, 1, false);
- wait_us(20);
- }
- LF.reset();
- while((timer.read() > 5.0f) && (timer.read() <= 10.0f)) {
- myled = 0;
- true_LF_data[0] = 0x80;
- i2c.write(0x20, true_LF_data, 1, false);
- wait_us(20);
- }
- while((timer.read() > 10.0f) && (timer.read() <= 15.0f)) {
- myled = 1;
- back_PID(200);
- i2c.write(0x20, LF_data, 1, false);
- wait_us(20);
- }
- LF.reset();
- while((timer.read() > 15.0f) && (timer.read() <= 20.0f)) {
- myled = 0;
- true_LF_data[0] = 0x80;
- i2c.write(0x20, true_LF_data, 1, false);
- wait_us(20);
- }
- timer.reset();
+ 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);
+
+ back_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);
+
+ right_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);
+
+ 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);
+
+ //斜め右前
+ right_front_PID(270, 270);
+ true_migiusiro_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);
+
+ //斜め右後
+ right_back_PID(270, 270);
+ 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_front_PID(270, 270);
+ 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);
+ true_migiusiro_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);
+
+ 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);
+ i2c.write(0x20, front_roller_data, 1, false);
+ i2c.write(0x22, back_roller_data, 1, false);
+ wait_us(20);
*/
/*