2019NHK_teamA
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
Diff: main.cpp
- Revision:
- 14:ab89b6cd9719
- Parent:
- 13:93321c73df60
- Child:
- 15:d022288aec51
--- a/main.cpp Wed Oct 10 16:58:39 2018 +0000
+++ b/main.cpp Mon Aug 26 01:39:35 2019 +0000
@@ -1,2249 +1,692 @@
-/* ------------------------------------------------------------------- */
-/* NHKロボコン2018-Bチーム自動ロボット(設計者: 4S 関) */
-/* ------------------------------------------------------------------- */
-/* このプログラムは上記のロボット専用の制御プログラムである。 */
-/* ------------------------------------------------------------------- */
-/* 対応機種: NUCLEO-F446RE */
-/* ------------------------------------------------------------------- */
-/* 製作者: 4D 高久 雄飛, mail: rab1sy23@gmail.com */
-/* ------------------------------------------------------------------- */
-/* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */
-/* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */
-/* ------------------------------------------------------------------- */
-
+/* ------------------------------------------------------------------- */
+/* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */
+/* Nucleo Type: F446RE */
+/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
+/* Sensor: encorder*4 */
+/* ------------------------------------------------------------------- */
+/* PID関数を各方向毎に追加した */
+/* ------------------------------------------------------------------- */
#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
+//PIDGain of wheels
+#define Kp 4500000.0
+//#define Kp 10000000.0
+#define Ti 0.0
+#define Td 0.0
-//搭載ボトル数
-//1弾倉当たり9発、残り3発になったら弾倉切り替えしよう
-#define bottles 14
+PID migimae(Kp, Ti, Td, 0.001);
+PID migiusiro(Kp, Ti, Td, 0.001);
+PID hidarimae(Kp, Ti, Td, 0.001);
+PID hidariusiro(Kp, Ti, Td, 0.001);
-//停止
-#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
+PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
+PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001);
+PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001);
+PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
+
//後進
-#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
+PID back_migimae(4500000.0, 0.0, 0.0, 0.001);
+PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001);
+PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001);
+PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
-//2段低
-#define low_grade 1
-//2段高
-#define high_grade 2
-//移動低
-#define low_table 3
-//移動中
-#define medium_table 4
-//移動高
-#define high_table 5
+//右進
+PID right_migimae(4500000.0, 0.0, 0.0, 0.001);
+PID right_migiusiro(4500000.0, 0.0, 0.0, 0.001);
+PID right_hidarimae(4500000.0, 0.0, 0.0, 0.001);
+PID right_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
-//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
+//左進
+PID left_migimae(4500000.0, 0.0, 0.0, 0.001);
+PID left_migiusiro(4500000.0, 0.0, 0.0, 0.001);
+PID left_hidarimae(4500000.0, 0.0, 0.0, 0.001);
+PID left_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
-//移動高最小距離
-#define high_table_minimum_distance 30
-//移動高最大距離
-#define high_table_maximum_distance 40
-
-
-//赤ゾーン
-#define red 1
-//青ゾーン
-#define blue 0
+//右旋回
+PID turn_right_migimae(4500000.0, 0.0, 0.0, 0.001);
+PID turn_right_migiusiro(4500000.0, 0.0, 0.0, 0.001);
+PID turn_right_hidarimae(4500000.0, 0.0, 0.0, 0.001);
+PID turn_right_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
-//右前オムニ
-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);
+//左旋回
+PID turn_left_migimae(4500000.0, 0.0, 0.0, 0.001);
+PID turn_left_migiusiro(4500000.0, 0.0, 0.0, 0.001);
+PID turn_left_hidarimae(4500000.0, 0.0, 0.0, 0.001);
+PID turn_left_hidariusiro(4500000.0, 0.0, 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);
-//フェンス検知用リミットスイッチ
-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);
+DigitalOut emergency(D11);
-//赤ゾーンLED
-DigitalOut blue_side(PC_0);
-//青ゾーンLED
-DigitalOut red_side(PC_3);
-//スタートLED
-DigitalOut start_LED(PC_13);
-//DigitalOut start_LED(PA_8);
+DigitalOut USR_LED1(PC_9);
+DigitalOut USR_LED2(PC_8);
+DigitalOut USR_LED3(PC_6);
+DigitalOut USR_LED4(PC_5);
-//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 wheel_x1(PA_8 , PA_6 , NC, 624);
+QEI wheel_x2(PB_14, PB_13, NC, 624);
+QEI wheel_y1(PB_1 , PB_15, NC, 624);
+QEI wheel_y2(PA_12, PA_11, NC, 624);
+//QEI wheel1(D2, D3, NC, 624);
+//QEI wheel2(D5, D4, NC, 624);
+
+//エンコーダ値格納変数
+int x_pulse1, x_pulse2, y_pulse1, y_pulse2;
+
+//操作の段階変数
+unsigned int phase = 0;
-//前ローラー
-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);
+//MD送信データ変数
+char init_send_data[1];
+char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
+char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
-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;
+//関数のプロトタイプ宣言
+void init(void);
+void init_send(void);
+void get_pulses(void);
+void print_pulses(void);
+void front(int target);
+void back(int target);
+void right(int target);
+void left(int target);
+void turn_right(int target);
+void turn_left(int target);
+void dondonkasoku(void);
+void front_PID(int target);
+void back_PID(int target);
+void right_PID(int target);
+void left_PID(int target);
+void turn_right_PID(int target);
+void turn_left_PID(int target);
-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;
-*/
+int main(void) {
+
+ init();
+ init_send();
-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;
+ while(1) {
+
+ get_pulses();
+ print_pulses();
+ front(500);
+ //back(500);
+ //right(500);
+ //left(500);
+ //turn_right(500);
+ //turn_left(500);
+ }
+}
-/*
-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;
+void init(void) {
+
+ //緊急停止用信号ピンをLow
+ emergency = 0;
+ //USR_LED1 = 1; USR_LED2 = 1; USR_LED3 = 1; USR_LED4 = 1;
-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;
+ //通信ボーレートの設定
+ //pc.baud(460800);
+ pc.baud(9600);
+
+ x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0;
+ migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80;
+ true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80;
+}
-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];
+void init_send(void) {
+
+ init_send_data[0] = 0x80;
+ i2c.write(0x10, init_send_data, 1);
+ i2c.write(0x12, init_send_data, 1);
+ i2c.write(0x14, init_send_data, 1);
+ i2c.write(0x16, init_send_data, 1);
+ wait(0.1);
+}
-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];
+void get_pulses(void) {
+
+ x_pulse1 = wheel_x1.getPulses();
+ x_pulse2 = wheel_x2.getPulses();
+ y_pulse1 = wheel_y1.getPulses();
+ y_pulse2 = wheel_y2.getPulses();
+}
-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};
+void print_pulses(void) {
+
+ pc.printf("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2);
+}
-char true_migimae_data[1];
-char true_migiusiro_data[1];
-char true_hidarimae_data[1];
-char true_hidariusiro_data[1];
+void front(int target) {
+
+ front_PID(target);
+ 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);
+}
-//各関数のプロトタイプ宣言
-//回転数取得関数
-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 back(int target) {
+
+ back_PID(target);
+ 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);
+}
-//ラインセンサ制御基板との通信用関数
-void linetrace();
-//超音波センサ用関数
-void ultrasonic();
-
-//移動平均をPID
-void flip() {
+void right(int target) {
+
+ right_PID(target);
+ 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);
+}
- //前回の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];
+void left(int target) {
+
+ left_PID(target);
+ 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);
+}
- //配列の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();
+void turn_right(int target) {
+
+ turn_right_PID(target);
+ 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);
+}
- migimae_wheel.reset();
- migiusiro_wheel.reset();
- hidarimae_wheel.reset();
- hidariusiro_wheel.reset();
- front_roller_wheel.reset();
- back_roller_wheel.reset();
+void turn_left(int target) {
+
+ turn_left_PID(target);
+ 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);
+}
- //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;
- }
+void front_PID(int target) {
- //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;
+ //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
+ migimae.setInputLimits(-2147483648, 2147483647);
+ migiusiro.setInputLimits(-2147483648, 2147483647);
+ hidarimae.setInputLimits(-2147483648, 2147483647);
+ hidariusiro.setInputLimits(-2147483648, 2147483647);
- //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);
+ //制御量の最小、最大
+ //正転(目標に達してない)
+ if((y_pulse1 < target) && (y_pulse2 < target)) {
+ migimae.setOutputLimits(0x84, 0xFF);
+ migiusiro.setOutputLimits(0x84, 0xFF);
+ hidarimae.setOutputLimits(0x84, 0xFF);
+ hidariusiro.setOutputLimits(0x84, 0xFF);
+ }
+ //逆転(目標より行き過ぎ)
+ else if((y_pulse1 > target) && (y_pulse2 > target)) {
+ migimae.setOutputLimits(0x00, 0x7B);
+ migiusiro.setOutputLimits(0x00, 0x7B);
+ hidarimae.setOutputLimits(0x00, 0x7B);
+ hidariusiro.setOutputLimits(0x00, 0x7B);
+ } else {
+ migimae.setOutputLimits(0x7C, 0x83);
+ migiusiro.setOutputLimits(0x7C, 0x83);
+ hidarimae.setOutputLimits(0x7C, 0x83);
+ hidariusiro.setOutputLimits(0x7C, 0x83);
+ }
- // 制御量の最小、最大
- 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];
+ //目標値
+ migimae.setSetPoint(target);
+ migiusiro.setSetPoint(target);
+ hidarimae.setSetPoint(target);
+ hidariusiro.setSetPoint(target);
- 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];
+ //センサ出力
+ migimae.setProcessValue(y_pulse1);
+ migiusiro.setProcessValue(y_pulse1);
+ hidarimae.setProcessValue(y_pulse2);
+ hidariusiro.setProcessValue(y_pulse2);
- 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;
+ //制御量をPWM値に変換
+ //正転(目標に達してない)
+ if((y_pulse1 < target) && (y_pulse2 < target)) {
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+ }
+ //逆転(目標より行き過ぎ)
+ else if((y_pulse1 > target) && (y_pulse2 > target)) {
+ true_migimae_data[0] = 0x7B - migimae_data[0];
+ true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
+ } else {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
}
-//前前進後右旋回(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);
+void back_PID(int target) {
+
+ //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
+ migimae.setInputLimits(-2147483648, 2147483647);
+ migiusiro.setInputLimits(-2147483648, 2147483647);
+ hidarimae.setInputLimits(-2147483648, 2147483647);
+ hidariusiro.setInputLimits(-2147483648, 2147483647);
- 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;
- //青ゾーン選択
+ //制御量の最小、最大
+ //逆転(目標に達してない)
+ if((abs(y_pulse1) < abs(target)) || (abs(y_pulse2) < abs(target))) {
+ migimae.setOutputLimits(0x00, 0x7B);
+ migiusiro.setOutputLimits(0x00, 0x7B);
+ hidarimae.setOutputLimits(0x00, 0x7B);
+ hidariusiro.setOutputLimits(0x0, 0x7B);
+ }
+ //正転(目標より行き過ぎ)
+ else if((abs(y_pulse1) > abs(target)) || (abs(y_pulse2) > abs(target))) {
+ migimae.setOutputLimits(0x84, 0xFF);
+ migiusiro.setOutputLimits(0x84, 0xFF);
+ hidarimae.setOutputLimits(0x84, 0xFF);
+ hidariusiro.setOutputLimits(0x84, 0xFF);
} else {
- red_side = 0;
- blue_side = 1;
+ migimae.setOutputLimits(0x7C, 0x83);
+ migiusiro.setOutputLimits(0x7C, 0x83);
+ hidarimae.setOutputLimits(0x7C, 0x83);
+ hidariusiro.setOutputLimits(0x7C, 0x83);
}
- //スタートボタン押したらエアシリ伸びる
- if(start_button == 0){
- start_flag = 1;
- }
- //2段下段スイッチでフラグ立てる
- if(user_switch1 == 0) {
- low_grade_flag = 1;
- myled1 = 1;
+ //よくわからんやつ
+ migimae.setMode(AUTO_MODE);
+ migiusiro.setMode(AUTO_MODE);
+ hidarimae.setMode(AUTO_MODE);
+ hidariusiro.setMode(AUTO_MODE);
+
+ //目標値
+ migimae.setSetPoint(abs(target));
+ migiusiro.setSetPoint(abs(target));
+ hidarimae.setSetPoint(abs(target));
+ hidariusiro.setSetPoint(abs(target));
+
+ //センサ出力
+ migimae.setProcessValue(abs(y_pulse1));
+ migiusiro.setProcessValue(abs(y_pulse1));
+ hidarimae.setProcessValue(abs(y_pulse2));
+ hidariusiro.setProcessValue(abs(y_pulse2));
+
+ //制御量(計算結果)
+ migimae_data[0] = migimae.compute();
+ migiusiro_data[0] = migiusiro.compute();
+ hidarimae_data[0] = hidarimae.compute();
+ hidariusiro_data[0] = hidariusiro.compute();
+
+ //制御量をPWM値に変換
+ //逆転(目標に達してない)
+ if((abs(y_pulse1) < abs(target)) || (abs(y_pulse2) < abs(target))) {
+ true_migimae_data[0] = 0x7B - migimae_data[0];
+ true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
- //2段上段スイッチでフラグ立てる
- if(user_switch2 == 0) {
- high_grade_flag = 1;
- myled2 = 1;
+ //正転(目標より行き過ぎ)
+ else if((abs(y_pulse1) > abs(target)) || (abs(y_pulse2) > abs(target))) {
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+ } else {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
}
- //移動(低)スイッチでフラグ立てる
- if(user_switch3 == 0) {
- low_table_flag = 1;
- myled3 = 1;
+}
+
+void right_PID(int target) {
+
+ //センサ出力値の最小、最大
+ migimae.setInputLimits(-2147483648, 2147483647);
+ migiusiro.setInputLimits(-2147483648, 2147483647);
+ hidarimae.setInputLimits(-2147483648, 2147483647);
+ hidariusiro.setInputLimits(-2147483648, 2147483647);
+
+ //制御量の最小、最大
+ //右進(目標まで達していない)
+ if((x_pulse1 < target) && (x_pulse2 < target)) {
+ migimae.setOutputLimits(0x00, 0x7B);
+ migiusiro.setOutputLimits(0x84, 0xFF);
+ hidarimae.setOutputLimits(0x84, 0xFF);
+ hidariusiro.setOutputLimits(0x00, 0x7B);
+ }
+ //左進(目標より行き過ぎ)
+ else if((x_pulse1 > target) && (x_pulse2 > target)) {
+ migimae.setOutputLimits(0x84, 0xFF);
+ migiusiro.setOutputLimits(0x00, 0x7B);
+ hidarimae.setOutputLimits(0x00, 0x7B);
+ hidariusiro.setOutputLimits(0x84, 0xFF);
}
- //移動(中)スイッチでフラグ立てる
- /*
- if(user_switch4 == 0) {
- medium_table_flag = 1;
- myled4 = 1;
- }
- */
+ //よくわからんやつ
+ migimae.setMode(AUTO_MODE);
+ migiusiro.setMode(AUTO_MODE);
+ hidarimae.setMode(AUTO_MODE);
+ hidariusiro.setMode(AUTO_MODE);
+
+ //目標値
+ migimae.setSetPoint(target);
+ migiusiro.setSetPoint(target);
+ hidarimae.setSetPoint(target);
+ hidariusiro.setSetPoint(target);
- //移動(高)スイッチでフラグ立てる
- if(user_switch5 == 0) {
- high_table_flag = 1;
- myled5 = 1;
- }
+ //センサ出力
+ migimae.setProcessValue(x_pulse1);
+ migiusiro.setProcessValue(x_pulse1);
+ hidarimae.setProcessValue(x_pulse2);
+ hidariusiro.setProcessValue(x_pulse2);
+
+ //制御量(計算結果)
+ migimae_data[0] = migimae.compute();
+ migiusiro_data[0] = migiusiro.compute();
+ hidarimae_data[0] = hidarimae.compute();
+ hidariusiro_data[0] = hidariusiro.compute();
- /*
- //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);
+ //制御量をPWM値に変換
+ //右進(目標まで達していない)
+ if((x_pulse1 < target) && (x_pulse2 < target)) {
+ true_migimae_data[0] = 0x7B - migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
}
- */
+ //左進(目標より行き過ぎ)
+ else if((x_pulse1 > target) && (x_pulse2 > target)) {
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+ } else {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
+}
- //フォトインタラプタ検知でLチカ
- if(photo_interrupter == 1) {
- myled4 = 1;
- } else {
- myled4 = 0;
- }
+void left_PID(int target) {
+
+ //センサ出力値の最小、最大
+ migimae.setInputLimits(-2147483648, 2147483647);
+ migiusiro.setInputLimits(-2147483648, 2147483647);
+ hidarimae.setInputLimits(-2147483648, 2147483647);
+ hidariusiro.setInputLimits(-2147483648, 2147483647);
- //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);
- }
+ //制御量の最小、最大
+ //左進(目標まで達していない)
+ if((abs(x_pulse1) < abs(target)) && (abs(x_pulse2) < abs(target))) {
+ migimae.setOutputLimits(0x84, 0xFF);
+ migiusiro.setOutputLimits(0x00, 0x7B);
+ hidarimae.setOutputLimits(0x00, 0x7B);
+ hidariusiro.setOutputLimits(0x84, 0xFF);
+ }
+ //右進(目標より行き過ぎ)
+ else if((abs(x_pulse1) > abs(target)) && (abs(x_pulse2) > abs(target))) {
+ migimae.setOutputLimits(0x00, 0x7B);
+ migiusiro.setOutputLimits(0x84, 0xFF);
+ hidarimae.setOutputLimits(0x84, 0xFF);
+ hidariusiro.setOutputLimits(0x00, 0x7B);
}
- //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();
+ //よくわからんやつ
+ migimae.setMode(AUTO_MODE);
+ migiusiro.setMode(AUTO_MODE);
+ hidarimae.setMode(AUTO_MODE);
+ hidariusiro.setMode(AUTO_MODE);
- //発射距離に到達して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);
- }
+ //目標値
+ migimae.setSetPoint(abs(target));
+ migiusiro.setSetPoint(abs(target));
+ hidarimae.setSetPoint(abs(target));
+ hidariusiro.setSetPoint(abs(target));
- //上記以外の距離でライントレースするよん
- } 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;
- }
- }
+ //センサ出力
+ migimae.setProcessValue(abs(x_pulse1));
+ migiusiro.setProcessValue(abs(x_pulse1));
+ hidarimae.setProcessValue(abs(x_pulse2));
+ hidariusiro.setProcessValue(abs(x_pulse2));
- //前が右寄りの時
- 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)) {
+ //制御量(計算結果)
+ migimae_data[0] = migimae.compute();
+ migiusiro_data[0] = migiusiro.compute();
+ hidarimae_data[0] = hidarimae.compute();
+ hidariusiro_data[0] = hidariusiro.compute();
- //前が左寄りで後ろが右寄りの時右旋回
- 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;
+ //制御量をPWM値に変換
+ //左進(目標まで達していない)
+ if((abs(x_pulse1) < abs(target)) && (abs(x_pulse2) < abs(target))) {
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+ }
+ //右進(目標より行き過ぎ)
+ else if((abs(x_pulse1) > abs(target)) && (abs(x_pulse2) > abs(target))) {
+ true_migimae_data[0] = 0x7B - migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
+ } else {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
+}
- //スタートゾーンに近づいたら減速
- 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;
+void turn_right_PID(int target) {
+
+ //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
+ migimae.setInputLimits(-2147483648, 2147483647);
+ migiusiro.setInputLimits(-2147483648, 2147483647);
+ hidarimae.setInputLimits(-2147483648, 2147483647);
+ hidariusiro.setInputLimits(-2147483648, 2147483647);
- 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;
- }
- }
- }*/
+ //制御量の最小、最大
+ //右旋回(目標に達してない)
+ if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse2 < target)) {
+ migimae.setOutputLimits(0x00, 0x7B);
+ migiusiro.setOutputLimits(0x00, 0x7B);
+ hidarimae.setOutputLimits(0x84, 0xFF);
+ hidariusiro.setOutputLimits(0x84, 0xFF);
}
-
- //テーブルごとに目標値を変えてローラー回転
- 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;
+ //左旋回(目標より行き過ぎ)
+ else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) {
+ migimae.setOutputLimits(0x84, 0xFF);
+ migiusiro.setOutputLimits(0x84, 0xFF);
+ hidarimae.setOutputLimits(0x00, 0x7B);
+ hidariusiro.setOutputLimits(0x00, 0x7B);
}
- 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;
+ //よくわからんやつ
+ migimae.setMode(AUTO_MODE);
+ migiusiro.setMode(AUTO_MODE);
+ hidarimae.setMode(AUTO_MODE);
+ hidariusiro.setMode(AUTO_MODE);
- //赤ゾーンでライン検知しないと低速左移動
- 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;
+ //目標値
+ migimae.setSetPoint(target);
+ migiusiro.setSetPoint(target);
+ hidarimae.setSetPoint(target);
+ hidariusiro.setSetPoint(target);
- //超低速左移動
- 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;
+ //センサ出力
+ migimae.setProcessValue(x_pulse1);
+ migiusiro.setProcessValue(x_pulse2);
+ hidarimae.setProcessValue(y_pulse1);
+ hidariusiro.setProcessValue(y_pulse2);
- //左旋回
- 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;
+ //制御量(計算結果)
+ migimae_data[0] = migimae.compute();
+ migiusiro_data[0] = migiusiro.compute();
+ hidarimae_data[0] = hidarimae.compute();
+ hidariusiro_data[0] = hidariusiro.compute();
- //前前進後ろ右旋回
- 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;
+ //制御量をPWM値に変換
+ //右旋回(目標に達してない)
+ if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse1 < target)) {
+ true_migimae_data[0] = 0x7B - migimae_data[0];
+ true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+ }
+ //左旋回(目標より行き過ぎ)
+ else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) {
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
+ } else {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
+}
- 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;
+void turn_left_PID(int target) {
+
+ //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
+ migimae.setInputLimits(-2147483648, 2147483647);
+ migiusiro.setInputLimits(-2147483648, 2147483647);
+ hidarimae.setInputLimits(-2147483648, 2147483647);
+ hidariusiro.setInputLimits(-2147483648, 2147483647);
- 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;
+ //制御量の最小、最大
+ //右旋回(目標に達してない)
+ if((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < target)) {
+ migimae.setOutputLimits(0x84, 0xFF);
+ migiusiro.setOutputLimits(0x84, 0xFF);
+ hidarimae.setOutputLimits(0x00, 0x7B);
+ hidariusiro.setOutputLimits(0x00, 0x7B);
+ }
+ //左旋回(目標より行き過ぎ)
+ else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) {
+ migimae.setOutputLimits(0x00, 0x7B);
+ migiusiro.setOutputLimits(0x00, 0x7B);
+ hidarimae.setOutputLimits(0x84, 0xFF);
+ hidariusiro.setOutputLimits(0x84, 0xFF);
}
- /*
- //前進
- 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);
+ //よくわからんやつ
+ migimae.setMode(AUTO_MODE);
+ migiusiro.setMode(AUTO_MODE);
+ hidarimae.setMode(AUTO_MODE);
+ hidariusiro.setMode(AUTO_MODE);
- //後進
- 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);
+ //目標値
+ migimae.setSetPoint(abs(target));
+ migiusiro.setSetPoint(abs(target));
+ hidarimae.setSetPoint(abs(target));
+ hidariusiro.setSetPoint(abs(target));
- //左進行
- 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);
+ //センサ出力
+ migimae.setProcessValue(abs(x_pulse1));
+ migiusiro.setProcessValue(abs(y_pulse1));
+ hidarimae.setProcessValue(abs(y_pulse2));
+ hidariusiro.setProcessValue(abs(x_pulse2));
- //斜め右後
- 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);
+ //制御量(計算結果)
+ migimae_data[0] = migimae.compute();
+ migiusiro_data[0] = migiusiro.compute();
+ hidarimae_data[0] = hidarimae.compute();
+ hidariusiro_data[0] = hidariusiro.compute();
- //右旋回
- 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);
+ //制御量をPWM値に変換
+ //右旋回(目標に達してない)
+ if((abs(x_pulse1) < abs(target)) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < abs(target))) {
+ true_migimae_data[0] = migimae_data[0];
+ true_migiusiro_data[0] = migiusiro_data[0];
+ true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
+ true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
+ }
+ //左旋回(目標より行き過ぎ)
+ else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) {
+ true_migimae_data[0] = 0x7B - migimae_data[0];
+ true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
+ true_hidarimae_data[0] = hidarimae_data[0];
+ true_hidariusiro_data[0] = hidariusiro_data[0];
+ } else {
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ }
+}
- //前前進後ろ右旋回
- 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);
+void dondonkasoku(void) {
- //前右旋回後ろ前進
- 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(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++){
+ i2c.write(0x10, init_send_data, 1);
+ i2c.write(0x12, init_send_data, 1);
+ i2c.write(0x14, init_send_data, 1);
+ i2c.write(0x16, init_send_data, 1);
+ wait(0.05);
+ }
+ //どんどん減速(正転)
+ for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--){
+ i2c.write(0x10, init_send_data, 1);
+ i2c.write(0x12, init_send_data, 1);
+ i2c.write(0x14, init_send_data, 1);
+ i2c.write(0x16, init_send_data, 1);
+ wait(0.05);
+ }
+ //だんだん加速(逆転)
+ for(init_send_data[0] = 0x7B; init_send_data[0] <= 0x00; init_send_data[0]--){
+ i2c.write(0x10, init_send_data, 1);
+ i2c.write(0x12, init_send_data, 1);
+ i2c.write(0x14, init_send_data, 1);
+ i2c.write(0x16, init_send_data, 1);
+ wait(0.05);
}
//だんだん減速(逆転)
- 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);
+ for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++){
+ i2c.write(0x10, init_send_data, 1);
+ i2c.write(0x12, init_send_data, 1);
+ i2c.write(0x14, init_send_data, 1);
+ i2c.write(0x16, init_send_data, 1);
+ wait(0.05);
}
- */
- }
}