NITICRobotClub 2018Team-B
自動投射のみです。（手動用にチューニングしました）
Dependencies: HCSR04 PID QEI mbed
Fork of
Lets_move_Seki2018_ver2
by 
INCTRC Information Sharing Test
Diff: main.cpp
- Revision:
- 5:b3bbf96356cf
- Parent:
- 4:df334779a69e
--- a/main.cpp Wed Sep 05 15:07:15 2018 +0000
+++ b/main.cpp Thu Sep 27 09:03:04 2018 +0000
@@ -15,952 +15,155 @@
#include "QEI.h"
#include "PID.h"
#include "hcsr04.h"
+//Pi
#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
+//PIDGain of rollers
+#define roller_Kp 1.5
+#define roller_Ki 0.1
-//右前オムニ
-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);
+InterruptIn shoot(PC_13);
+HCSR04 sonic(PB_3, PB_10);
+
//前ローラー
PID front_roller(roller_Kp, roller_Ki, 0.0, 0.001);
//後ローラー
PID back_roller(roller_Kp, roller_Ki, 0.0, 0.001);
-
//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);
-
-//赤ゾーン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);
-
//前ローラー
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 get_rps;
-Ticker get_distance;
-Timer timer;
-
-int roller_flag = 0;
-int loading_state = 0;
-
-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 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 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_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_front_roller_pulse;
+int ave_back_roller_pulse;
char send_data[1];
-char true_send_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 cylinder_data[0];
-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;
-
-/* ロリコン処理関数 */
+static int i = 0;
+//各関数のプロトタイプ宣言
+void shot();
+void ultrasonic();
+//回転数取得関数
void flip();
-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);
+//ローラー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_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番目の箱に取得パルスを代入
+ front_roller_pulse[i] = front_roller_wheel.getPulses();
+ back_roller_pulse[i] = back_roller_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();
-}
-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値に変換
- 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 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;
-}
-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値に変換
- 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);
-
- // よくわからんやつ
- 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);
+ //i[0]~i[9]までの合計値を代入
+ sum_front_roller_pulse += front_roller_pulse[i];
+ sum_back_roller_pulse += back_roller_pulse[i];
- // 目標値
- 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();
+ //インクリメント
+ i++;
+ //iが最大値(9)になったらリセット
+ if(i > 9) {
+ i = 0;
+ }
+ //10回分の合計値を10で割り、取得パルスの平均を出す
+ ave_front_roller_pulse = sum_front_roller_pulse / 10;
+ ave_back_roller_pulse = sum_back_roller_pulse / 10;
- // 制御量を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);
-
- // 目標値
- 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_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_migimae_data[0] = 0x7D - migimae_data[0];
- true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
-
- return 0;
+ //平均値をRPMへ変換
+ front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 50;
+ back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
+ pc.printf("%d %d\n\r", abs(front_roller_rpm), abs(back_roller_rpm));
}
-//斜め左後
-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_migiusiro_data[0] = migiusiro_data[0];
- true_hidarimae_data[0] = hidarimae_data[0];
-
- return 0;
-}
-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");
+ //緊急停止用信号ピンをLow
emergency = 0;
- /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
+ //回転数取得関数のタイマ割り込み(40ms)
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);
-
+
+ //各MD, エアシリ制御基板へ起動後0.1秒間0x80を送りつける(通信切断防止用)
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(0x20, send_data, 1);
i2c.write(0x22, send_data, 1);
- i2c.write(0x30, send_data, 1);
i2c.write(0x40, send_data, 1);
+
+ shoot.fall(shot);
wait(0.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);
- }
-
- /*
- 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);
- }
- */
-
- /*
- 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);
- }
-
- /*
- 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);
+ //ローラーぐるぐる
+ roller_PID(200, 200);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
- wait_us(20);
- */
-
- /*
- //どんどん加速(逆転)
- 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);
- wait(0.1);
- }
- //だんだん減速(逆転)
- for(send_data[0] = 0xF4; send_data[0] > 0x80; 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);
- wait(0.1);
- }
- send_data[0] = 0x80;
- i2c.write(0x20, send_data, 1);
- wait(5);
- */
+ wait_us(20);
}
}
+
+void shot(){
+ cylinder_data[0] = 0x0F;
+ i2c.write(0x40, cylinder_data, 1);
+ wait(1);
+
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+ wait(0.5);
+
+}
+
+//超音波センサ用関数
+void ultrasonic() {
+ //超音波発射
+ sonic.start();
+ //10ms待機
+ wait(0.01);
+ //get_dist_cm関数は、計測から得られた距離(cm)を返します。
+ int distance = sonic.get_dist_cm();
+ //pc.printf("%d[cm]\r\n", distance);
+}
\ No newline at end of file