NITICRobotClub 2018Team-B
Bteam tactics plan A _ move table ver
Dependencies: HCSR04 PID QEI mbed
Fork of
UNKO_FINAL
by 
NITICRobotClub 2018Team-B
main.cpp
- Committer:
- yuron
- Date:
- 2018-10-10
- Revision:
- 13:93321c73df60
- Parent:
- 12:1a22b9797004
- Child:
- 14:677e9f0785b8
File content as of revision 13:93321c73df60:
/* ------------------------------------------------------------------- */
/* 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"
//Pi
#define PI 3.14159265359
//PIDGain of wheels(fast_mode)
#define Kp 20.0
#define Ki 0.02
#define Kd 0.0
//PIDGain of rollers
#define front_roller_Kp 1.3
#define front_roller_Ki 0.19
#define front_roller_Kd 0.0
#define back_roller_Kp 1.3
#define back_roller_Ki 0.1
#define back_roller_Kd 0.0
//PIDGain of wheels(slow_mode)
#define Kp_slow 0.6
#define Ki_slow 0.03
#define Kd_slow 0.0
//搭載ボトル数
//1弾倉当たり9発、残り3発になったら弾倉切り替えしよう
#define bottles 14
//停止
#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 right_fast 13
#define left_fast 14
//2段低
#define low_grade 1
//2段高
#define high_grade 2
//移動低
#define low_table 3
//移動中
#define medium_table 4
//移動高
#define high_table 5
//2段低最小距離
#define low_grade_minimum_distance 4
//2段低最大距離
#define low_grade_maximum_distance 16
//2段高最小距離
//#define high_grade_minimum_distance 1
//2段高最大距離
//#define high_grade_maximum_distance 1
//移動低最小距離
#define low_table_minimum_distance 10
//移動低最大距離
#define low_table_maximum_distance 15
//移動中最小距離
#define medium_table_minimum_distance 20
//移動中最大距離
#define medium_table_maximum_distance 30
//移動高最小距離
#define high_table_minimum_distance 30
//移動高最大距離
#define high_table_maximum_distance 40
//赤ゾーン
#define red 1
//青ゾーン
#define blue 0
//右前オムニ
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 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(front_roller_Kp, front_roller_Ki, front_roller_Kd, 0.001);
//後ローラー
PID back_roller (back_roller_Kp, back_roller_Ki, back_roller_Kd, 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);
//フェンス検知用リミットスイッチ
DigitalIn limit(PH_1);
//スイッチ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(PC_13);
//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 measure_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_rpm;
Timer timer;
Timer back_timer1;
Timer back_timer2;
Timer back_timer3;
Timer start_timer;
bool roller_flag = 0;
bool start_flag = 0;
bool trace_flag = 1;
//2段下段乗ったかなフラグ
bool low_grade_flag = 0;
//2段上段乗ったかなフラグ
bool high_grade_flag = 0;
//移動(低)乗ったかなフラグ
bool low_table_flag = 0;
//移動(中)乗ったかなフラグ
bool medium_table_flag = 0;
//移動(高)乗ったかなフラグ
bool high_table_flag = 0;
int table_fire = 0;
/*
//2段下段撃ったかなフラグ
bool low_grade_fire_flag = 0;
//2段上段撃ったかなフラグ
bool high_grade_fire_flag = 0;
//移動(低)撃ったかなフラグ
bool low_table_fire_flag = 0;
//移動(中)撃ったかなフラグ
bool medium_table_fire_flag = 0;
//移動(高)撃ったかなフラグ
bool high_table_fire_flag = 0;
*/
static int mouted_bottles = 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 lateral_frag = 0;
/*
int bottles_flag[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
*/
int firing_minimum_distamce = 0;
int firing_maximum_distance = 0;
int migimae_rpm;
int migiusiro_rpm;
int hidarimae_rpm;
int hidariusiro_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 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 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 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];
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] = {0x80};
char true_migimae_data[1];
char true_migiusiro_data[1];
char true_hidarimae_data[1];
char true_hidariusiro_data[1];
//各関数のプロトタイプ宣言
//回転数取得関数
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() {
//前回の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];
//配列の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();
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値に変換
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値に変換
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値に変換
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(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値に変換
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値に変換
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値に変換
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値に変換
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);
}
int main(void) {
//こんにちは世界
pc.printf("HelloWorld\n");
//緊急停止用信号ピンをLow
emergency = 0;
//スタートボタンのLEDをHigh
start_LED = 1;
//回転数取得関数のタイマ割り込み(40ms)
get_rpm.attach_us(&flip, 40000);
//フォトセンサ制御基板との通信ボーレートの設定(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);
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);
myled1 = 0;
myled2 = 0;
myled3 = 0;
myled4 = 0;
myled5 = 0;
while(1) {
//超音波取得関数の呼び出し
ultrasonic();
//赤ゾーン選択
if(side == red){
red_side = 1;
blue_side = 0;
//青ゾーン選択
} else {
red_side = 0;
blue_side = 1;
}
//スタートボタン押したらエアシリ伸びる
if(start_button == 0){
start_flag = 1;
}
//2段下段スイッチでフラグ立てる
if(user_switch1 == 0) {
low_grade_flag = 1;
myled1 = 1;
}
//2段上段スイッチでフラグ立てる
if(user_switch2 == 0) {
high_grade_flag = 1;
myled2 = 1;
}
//移動(低)スイッチでフラグ立てる
if(user_switch3 == 0) {
low_table_flag = 1;
myled3 = 1;
}
//移動(中)スイッチでフラグ立てる
/*
if(user_switch4 == 0) {
medium_table_flag = 1;
myled4 = 1;
}
*/
//移動(高)スイッチでフラグ立てる
if(user_switch5 == 0) {
high_table_flag = 1;
myled5 = 1;
}
/*
//2段高ローラー回転
if(high_grade_fire_flag == 0) {
//ローラー回転開始
roller_PID(1006, 928);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
}
*/
//フォトインタラプタ検知でLチカ
if(photo_interrupter == 1) {
myled4 = 1;
} else {
myled4 = 0;
}
//6発発射したら弾倉切り替え
if(mouted_bottles == 10) {
if(photo_interrupter == 0) {
loading_data[0] = 0x0C;
i2c.write(0x30, loading_data, 1, false);
} else {
loading_data[0] = 0x80;
i2c.write(0x30, loading_data, 1, false);
}
}
//pc.printf("%3d %3d %3d %3d %3d %3d\n\r", front_line_state, back_line_state, line_state_pettern, abs(measure_pulse), distance, mouted_bottles);
//pc.printf("%d %3lf\n\r", mouted_bottles, timer.read());
pc.printf("%3d %3d %3d %3lf\n\r", trace_flag, line_state_pettern, distance, timer.read());
//スタートボタンで動作開始
if(start_flag == 1) {
//2段下段
if(low_grade_flag == 0) {
//パルスがnを満たすまで無条件で横移動
//if(abs(measure_pulse) < 100) {
/*
start_timer.start();
if(start_timer.read() <= 1.0f) {
//青ゾーンの時指定のパルス値まで無条件高速右移動
if(side == blue) {
line_state_pettern = right_fast;
//赤ゾーンの時指定のパルス値まで無条件高速左移動
}
else if(side == red) {
line_state_pettern = left_fast;
}
} else {
//else if(start_timer.read() > 2.0f) {
}
*/
//一定の距離を超えたらゆっくり移動してライン見つけたら(ry
//} else {
//距離が11cm~29cmだったらトレースせずに停止
if(distance >= firing_minimum_distamce && distance <= firing_maximum_distance && distance != 0) {
//ライントレースの停止
trace_flag = 0;
//タイマスタート
timer.start();
//発射距離に到達して1秒待って発射
if(timer.read() > 1.0f && timer.read() <= 2.5f) {
cylinder_data[0] = 5;
mouted_bottles = 5;
i2c.write(0x40, cylinder_data, 1);
}
else if(timer.read() > 12.5f && timer.read() <= 15.0f) {
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
}
else if(timer.read() > 15.0f && timer.read() <= 15.5f) {
line_state_pettern = right_slow;
}
else if(timer.read() > 15.5f && timer.read() <= 17.5f) {
line_state_pettern = stop;
cylinder_data[0] = 5;
mouted_bottles = 10;
i2c.write(0x40, cylinder_data, 1);
}
else if(timer.read() > 28.0f && timer.read() <= 30.0f) {
line_state_pettern = stop;
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
}
else if(timer.read() > 30.0f && timer.read() <= 31.0f) {
line_state_pettern = left_slow;
}
else if(timer.read() > 31.0f && timer.read() <= 33.0f) {
line_state_pettern = stop;
cylinder_data[0] = 5;
mouted_bottles = 15;
i2c.write(0x40, cylinder_data, 1);
}
else if(timer.read() > 33.0f && timer.read() <= 43.0f) {
line_state_pettern = stop;
}
else if(timer.read() > 43.0f && timer.read() <= 45.0f) {
line_state_pettern = back_trace;
//ライントレースの復帰
//trace_flag = 1;
//トレース方向の反転
//trace_direction = 1;
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
}
else if(timer.read() > 45.0f) {
line_state_pettern = right_slow;
//ライントレースの復帰
trace_flag = 1;
//トレース方向の反転
trace_direction = 1;
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
} else {
line_state_pettern = stop;
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
}
//上記以外の距離でライントレースするよん
} else {
if(trace_flag == 1) {
//タイマリセット
timer.reset();
//ライン検知するまで右移動
if(back_line_state == 0) {
//青ゾーンの時ライン検知するまで右移動
if(side == blue) {
line_state_pettern = right_slow;
//赤ゾーンの時ライン検知するまで左移動
} else {
line_state_pettern = left_slow;
}
}
//ライン検知するまで右移動
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)) {
if(side == red) {
line_state_pettern = left_super_slow;
}
else if(side == blue) {
line_state_pettern = right_super_slow;
}
}
//前はライン検知できないけど後ろは検知してる時左移動
else if((front_line_state == 0) && (back_line_state >= 1)) {
if(side == red) {
line_state_pettern = left_super_slow;
}
else if(side == blue) {
line_state_pettern = right_super_slow;
}
}
//前が右寄りの時
else if((front_line_state <= 5) && (front_line_state != 0)) {
//前も後ろも右寄りの時右移動
if((back_line_state >= 13) && (back_line_state <= 17)) {
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 <= 5) && (back_line_state != 0)) {
line_state_pettern = turn_left;
}
}
//前が真ん中寄りの時
else if((front_line_state >= 6) && (front_line_state <= 12)) {
//前が真ん中で後ろが右寄りの時前前進後ろ右旋回
if((back_line_state >= 13) && (back_line_state <= 17)) {
line_state_pettern = front_front_back_right;
}
//前も後ろも真ん中の時前進
else if((back_line_state >= 6) && (back_line_state <= 12)) {
if(trace_direction == 0) {
//20cm未満で後進
if(distance < firing_minimum_distamce && distance != 0) {
line_state_pettern = back_trace;
}
//25cmより遠くて前進
else if(distance > firing_maximum_distance) {
line_state_pettern = front_trace;
} else {
line_state_pettern = front_trace;
}
}
else if(trace_direction == 1) {
line_state_pettern = back_trace;
}
}
//前が真ん中で後ろが左寄りの時前前進後ろ左旋回
else if((back_line_state <= 5) && (back_line_state != 0)) {
line_state_pettern = front_front_back_left;
}
}
//前が左寄りの時
else if((front_line_state >= 13) && (front_line_state <= 17)) {
//前が左寄りで後ろが右寄りの時右旋回
if((back_line_state >= 13) && (back_line_state <= 17)) {
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 <= 5) && (back_line_state != 0)) {
line_state_pettern = left_super_slow;
}
//前で白線の長いの検知したら無視するよ～ん
else if((front_line_state == 40) || (front_line_state == 80)) {
if(trace_direction == 0) {
line_state_pettern = front_trace;
}
else if(trace_direction == 1) {
line_state_pettern = back_trace;
}
}
//後で白線の長いの検知したら無視するよ～ん
else if((back_line_state == 40) || (back_line_state == 80)) {
if(trace_direction == 0) {
line_state_pettern = front_trace;
}
else if(trace_direction == 1) {
line_state_pettern = back_trace;
}
//それ以外
} else {
line_state_pettern = stop;
}
}
}
}
//}
} else {
line_state_pettern = stop;
}
//lateral_fragが0(初期状態)の時
if(lateral_frag == 0) {
//ローラー回転2段高
table_fire = low_grade;
firing_minimum_distamce = low_grade_minimum_distance;
firing_maximum_distance = low_grade_maximum_distance;
//リミットONでlateral_frag = 1
if(limit.read() == 0) {
lateral_frag = 1;
}
}
//リミットがONの時(1回目)
if(lateral_frag == 1) {
//ライントレース有効果
trace_flag = 1;
//ローラー回転移動中
table_fire = low_table;
firing_minimum_distamce = low_table_minimum_distance;
firing_maximum_distance = low_table_maximum_distance;
//トレース方向の反転(前進)
trace_direction = 0;
back_timer1.start();
if(back_timer1.read() <= 1.5f && back_timer1.read() != 0.0f) {
if(side == blue) {
line_state_pettern = right_slow;
}
else if(side == red) {
line_state_pettern = left_slow;
}
}
else if(back_timer1.read() > 1.5f) {
back_timer1.reset();
lateral_frag = 2;
}
}
else if(lateral_frag == 2) {
if(limit.read() == 0) {
lateral_frag = 3;
}
}
//リミットがONの時(2回目)
else if(lateral_frag == 3) {
//ローラー回転停止
table_fire = 0;
//スタートゾーンに近づいたら減速
if(abs(measure_pulse) < 1200) {
if(side == blue) {
line_state_pettern = left_super_slow;
}
else if(side == red) {
line_state_pettern = right_super_slow;
}
}
else if(abs(measure_pulse) < 500) {
line_state_pettern = stop;
} else {
if(side == blue) {
line_state_pettern = left_fast;
}
else if(side == red) {
line_state_pettern = right_fast;
}
}
/*
//ローラー回転移動中
table_fire = medium_table;
firing_minimum_distamce = medium_table_minimum_distance;
firing_maximum_distance = medium_table_maximum_distance;
trace_direction = 0;
back_timer2.start();
if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
if(side == blue) {
line_state_pettern = right_slow;
}
else if(side == red) {
line_state_pettern = left_slow;
}
}
else if(back_timer2.read() > 0.8f) {
back_timer2.reset();
lateral_frag = 4;
}*/
}
/*
else if(lateral_frag == 4) {
if(limit.read() == 0) {
//上でback_timer3を使用しているためここでリセットをかける
//back_timer3.reset();
lateral_frag = 5;
}
}
//リミットがONの時(3回目)
else if(lateral_frag == 5) {
//ローラー回転移動高
table_fire = high_table;
firing_minimum_distamce = high_table_minimum_distance;
firing_maximum_distance = high_table_maximum_distance;
trace_direction = 0;
back_timer3.start();
if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
if(side == blue) {
line_state_pettern = right_slow;
}
else if(side == red) {
line_state_pettern = left_slow;
}
}
else if(back_timer3.read() > 0.8f) {
back_timer3.reset();
lateral_frag = 6;
}
}
else if(lateral_frag == 6) {
if(limit.read() == 0) {
lateral_frag = 7;
}
}
//リミットがONの時(4回目)
else if(lateral_frag == 7) {
//ローラー回転停止
table_fire = 0;
//スタートゾーンに近づいたら減速
if(abs(measure_pulse) < 1200) {
if(side == blue) {
line_state_pettern = left_super_slow;
}
else if(side == red) {
line_state_pettern = right_super_slow;
}
}
else if(abs(measure_pulse) < 500) {
line_state_pettern = stop;
} else {
if(side == blue) {
line_state_pettern = left_fast;
}
else if(side == red) {
line_state_pettern = right_fast;
}
}
}*/
}
//テーブルごとに目標値を変えてローラー回転
switch(table_fire) {
//2段低
case low_grade:
//ローラー回転開始
//roller_PID(810, 683);
roller_PID(815, 688);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
break;
//2段高
case high_grade:
//ローラー回転開始
roller_PID(400, 400);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
break;
//移動低
case low_table:
//ローラー回転開始
roller_PID(820, 690);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
break;
//移動中
case medium_table:
//ローラー回転開始
roller_PID(800, 800);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
break;
//移動高
case high_table:
//ローラー回転開始
roller_PID(1000, 1000);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
break;
//それ以外ショートブレーキ
default:
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);
wait_us(20);
break;
}
switch(line_state_pettern) {
//青ゾーンでライン検知しないと低速右移動
case right_slow:
//右前、右後ろ、左前、左後ろ
right_PID_slow(50, 55, 50, 55);
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, 55, 50, 55);
//left_PID_slow(100, 110, 100, 110);
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(5, 5, 5, 5);
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(5, 5, 5, 5);
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(5, 5, 5, 5);
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(5, 5, 5, 5);
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(33, 31, 35, 35);
//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(5, 5, 5, 5);
//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(5, 5, 5, 5);
//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(5, 5, 5, 5);
//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(5, 5, 5, 5);
//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;
case right_fast:
//right_PID(300, 255, 300, 255);
right_PID_slow(100, 107, 100, 107);
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_fast:
//left_PID(255, 300, 255, 300);
left_PID_slow(107, 100, 107, 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);
break;
case stop:
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;
//それ以外ショートブレーキ
default:
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(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(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, 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_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_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);
//斜め左後
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);
//右旋回
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);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data,1, false);
wait_us(20);
//前前進後ろ左旋回
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);
//前右旋回後ろ前進
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);
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data,1, false);
wait_us(20);
*/
/*
//ローラーぐるぐる(max930rpm)
pc.printf("%3d %3d %3d\n\r", abs(front_roller_rpm), abs(back_roller_rpm), distance);
//このパラメータ(距離10cm, 移動1個め)で5~8割の確率で勃つ
//roller_PID(814, 696);
roller_PID(1006, 928);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
*/
/*
//どんどん加速(逆転)
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] = 0x0C; send_data[0] > 0x7C; send_data[0]--){
//ice(0x88, send_data);
//ice(0xA2, send_data);
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);
}
*/
}
}