Diff: main.cpp

Revision:

0:baf1c4d4d305

diff -r 000000000000 -r baf1c4d4d305 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jan 09 04:40:33 2020 +0000
@@ -0,0 +1,279 @@
+#include "mbed.h"
+
+// 定数
+#define PI 3.14159265358979F
+
+#define STATE_ALL_BLACK   0
+#define STATE_ALL_WHITE   1
+#define STATE_LEFT        2
+#define STATE_RIGHT       3
+#define STATE_B_W_B       4
+
+#define LR_STRAIGHT       0
+#define LR_LEFT           1
+#define LR_RIGHT          2
+
+// パラメータ
+#define INTERVAL  0.001F
+#define BASE_SPEED 3400.0F
+#define MIN_DUTY_RATIO  0.00F
+#define MAX_DUTY_RATIO  0.85F
+
+#define v_Kp        0.000040F  // 比例ゲイン　（速度）
+#define BASE_DUTY   0.43F      // 定数ゲイン
+
+#define r_Kp        2200.0F    // 比例ゲイン　（転回）
+#define r_Ki        3200.0F    // 積分ゲイン　
+#define r_Kd        300.0F     // 微分ゲイン
+
+#define LR_ADJ_RATIO    0.04F  // 転回時　内側の減回転数と外側の増回転数の比（1.0:全部外側/0.0:全部内側）
+
+//left side motor
+PwmOut L_motor_out_back(D0);
+PwmOut L_motor_out_forw(D3);
+
+//right side motor
+PwmOut R_motor_out_back(D5);
+PwmOut R_motor_out_forw(D6);
+
+//mode
+DigitalOut mode(D1);
+
+//read interupter voltage
+InterruptIn L_motor_interupter_in(D12);
+InterruptIn R_motor_interupter_in(D11);
+
+//read reflector voltage
+AnalogIn L_reflector_in(A2);
+AnalogIn M_reflector_in(A1);
+AnalogIn R_reflector_in(A0);
+
+//電源確認用
+DigitalOut led_out_power(D9);
+//measure 200mm
+DigitalOut led_out_mileage(D10);
+
+
+int L_pulse_per_interval; //1ループのパルス数
+int R_pulse_per_interval;
+int L_pulse_per_interval_prev; //1ループのパルス数　前回
+int R_pulse_per_interval_prev;
+int L_pulse_total; //パルスカウンター（トータル）
+int R_pulse_total;
+int L_pulse_total_prev; //パルスカウンター（トータル）前回
+int R_pulse_total_prev;
+int k_counter1; //オドメトリ用のカウンター
+int k_counter2;
+
+//======================================================================
+//      回転数カウウント用イベントハンドラー
+//======================================================================
+void L_motor_event_handler(void)
+{
+    L_pulse_total++;
+    k_counter1++;
+}
+
+void R_motor_event_handler(void)
+{
+    R_pulse_total++;
+    k_counter2++;
+}
+//======================================================================
+//      転回のための左右速度調整幅決定関数
+//======================================================================
+void get_speed_adjust( float *L_speed_adjust, float *R_speed_adjust )
+{    
+    static float deviation[2];  // 進行方向偏差
+    static float integral;      // 進行方向偏差の積分
+
+    static int state[3];        // リフレクターの常態 (STATE_ALL_BLACK/STATE_ALL_WHITE/STATE_B_W_B/STATE_LEFT/STATE_RIGHT)
+    static int LR_dir[3];       // 進行方向 (LR_STRAIGHT/LR_LEFT/LR_RIGHT)
+
+    // リフレクターの状態取得
+    //unit conversion (to V)
+    float L_ref_V = 3.3F * L_reflector_in;
+    float M_ref_V = 3.3F * M_reflector_in;
+    float R_ref_V = 3.3F * R_reflector_in;
+
+    state[2] = state[1];
+    state[1] = state[0];
+
+    state[0] =
+        ( L_ref_V  > 2.5F && M_ref_V > 2.5F && R_ref_V > 2.5F)?     STATE_ALL_BLACK:
+        ( L_ref_V  < 1.3F && M_ref_V < 1.3F && R_ref_V < 1.3F)?     STATE_ALL_WHITE:
+        ( L_ref_V  > 2.0F && M_ref_V < 1.3F && R_ref_V > 2.0F)?     STATE_B_W_B:
+        ( L_ref_V  > R_ref_V)?                                      STATE_LEFT : STATE_RIGHT;
+
+
+    // 進行方向の決定
+    if (LR_dir[0] != LR_dir[1]) {LR_dir[2] = LR_dir[1];}
+    
+    LR_dir[1] = LR_dir[0];
+
+    LR_dir[0] =
+        (state[0] == STATE_ALL_BLACK)?                                                  // 全黒　直進
+            ((state[1] == STATE_ALL_BLACK)? LR_STRAIGHT : LR_dir[1]) :                  // 最初の全黒はノイズの可能性があるので無視して２回目から直進
+        (state[0] == STATE_B_W_B)? LR_dir[1] :                                          // 真ん中白・両端黒はノイズを拾っている可能性があるので無視
+        (state[0] == STATE_ALL_WHITE)?                                                  // 全白　旋回継続
+            ((LR_dir[1] == LR_STRAIGHT)? LR_dir[2] : LR_dir[1]) :                       // 全白の前全黒の場合はその前の旋回方向を選択
+        (state[0] == STATE_LEFT)?
+            ((state[1] == STATE_ALL_WHITE && LR_dir[1] == LR_RIGHT)? LR_RIGHT : LR_LEFT) : // 全白旋回中の突然の方向転換はライン以外の黒を拾っている可能性があるので無視
+            ((state[1] == STATE_ALL_WHITE && LR_dir[1] == LR_LEFT)?  LR_LEFT : LR_RIGHT);
+
+    if (LR_dir[0] == LR_LEFT  && L_ref_V < M_ref_V) {                               // 乖離方向と反対のリフレクタが真ん中より明るい場合、反対のリフレクタの明るさを真ん中の明るさと同じと仮定
+        L_ref_V = M_ref_V;
+    }
+    if (LR_dir[0] == LR_RIGHT && R_ref_V < M_ref_V) {
+        R_ref_V = M_ref_V;
+    }
+
+    // 進行方向偏差
+    deviation[0] =                                                                  // 偏差はリフレクターの明るさ度合いの合計
+          ((L_ref_V > 2.5F)? 0 : 2.5F - L_ref_V)
+        + ((M_ref_V > 2.5F)? 0 : 2.5F - M_ref_V)
+        + ((R_ref_V > 2.5F)? 0 : 2.5F - R_ref_V);
+
+    // 進行方向偏差の積分
+    integral =
+        (LR_dir[0] == LR_STRAIGHT)? 0.0F:                                               // 直進時リセット
+        (LR_dir[0] == LR_dir[1])? integral + deviation[0] * INTERVAL : 0.0F;     // 方向転換時リセット
+
+    integral = (integral > 0.9F)? 0.9F :  integral; // 積分要素の上限
+
+    // 操作幅　比例成分
+    float adjust = deviation[0] * r_Kp;
+
+    // 操作幅に積分成分追加
+    adjust += integral * r_Ki;
+
+    // 操作幅に微分成分追加
+    adjust += (deviation[0] - deviation[1]) * r_Kd / INTERVAL;
+
+    // 操作を左右の車輪に分配
+    *L_speed_adjust =
+        (LR_dir[0] == LR_STRAIGHT)?  0.0F :
+        (LR_dir[0] == LR_LEFT)?     (LR_ADJ_RATIO -1.0F)*adjust : adjust * LR_ADJ_RATIO;
+
+    *R_speed_adjust =
+        (LR_dir[0] == LR_STRAIGHT)?  0.0F :
+        (LR_dir[0] == LR_RIGHT)?    (LR_ADJ_RATIO -1.0F)*adjust : adjust * LR_ADJ_RATIO;
+ 
+        
+    deviation[1] = deviation[0];
+}
+//======================================================================
+//    メイン
+//======================================================================
+int main()
+{
+
+    float L_speed_adjust;
+    float R_speed_adjust;
+    
+    float L_duty_ratio;
+    float R_duty_ratio;
+
+    float L_target; //left target level 左の目標値 (pulse per loop)
+    float R_target; //right target level 右の目標値 (pulse per loop)
+
+    int oddeven; //LED点滅の偶数回奇数回を判定
+    
+    
+    float k = 0.0;  //距離計測
+    float is_stop;       // 静止状態の時間計測
+    
+    mode = 0;
+
+    // motor output period
+    L_motor_out_back.period_us(32);
+    L_motor_out_forw.period_us(32);
+    R_motor_out_back.period_us(32);
+    R_motor_out_forw.period_us(32);
+
+    L_motor_out_back.write(0.0);
+    L_motor_out_forw.write(0.0);
+    R_motor_out_back.write(0.0);
+    R_motor_out_forw.write(0.0);
+
+    led_out_power = 1;
+
+    wait(4.0);
+
+    led_out_mileage = 1;
+
+    L_pulse_total = 0;
+    R_pulse_total = 0;
+    L_pulse_total_prev = 0;
+    R_pulse_total_prev = 0;
+    L_pulse_per_interval = 0;
+    R_pulse_per_interval = 0;
+
+    oddeven = 1;
+
+    L_duty_ratio = 0;
+    R_duty_ratio = 0;
+
+    L_motor_interupter_in.rise(&L_motor_event_handler);
+    L_motor_interupter_in.fall(&L_motor_event_handler);
+    R_motor_interupter_in.rise(&R_motor_event_handler);
+    R_motor_interupter_in.fall(&R_motor_event_handler);
+
+    while(1) {
+
+        get_speed_adjust( &L_speed_adjust, &R_speed_adjust );
+
+        L_target = BASE_SPEED + L_speed_adjust;
+        R_target = BASE_SPEED + R_speed_adjust;
+
+        L_pulse_per_interval_prev = L_pulse_per_interval;
+        R_pulse_per_interval_prev = R_pulse_per_interval;
+
+        L_pulse_per_interval = L_pulse_total - L_pulse_total_prev;
+        R_pulse_per_interval = R_pulse_total - R_pulse_total_prev;
+
+        L_pulse_total_prev = L_pulse_total_prev + L_pulse_per_interval;
+        R_pulse_total_prev = R_pulse_total_prev + R_pulse_per_interval;
+
+        L_duty_ratio = BASE_DUTY + L_target * v_Kp
+                       + (L_target - L_pulse_per_interval/INTERVAL) * v_Kp;
+
+        R_duty_ratio = BASE_DUTY + R_target * v_Kp
+                       + (R_target - R_pulse_per_interval/INTERVAL) * v_Kp;
+        // 上限チェック
+        L_duty_ratio = (L_duty_ratio > MAX_DUTY_RATIO)? MAX_DUTY_RATIO : L_duty_ratio;
+        R_duty_ratio = (R_duty_ratio > MAX_DUTY_RATIO)? MAX_DUTY_RATIO : R_duty_ratio;
+        // 下限チェック
+        L_duty_ratio = (L_duty_ratio < MIN_DUTY_RATIO)? MIN_DUTY_RATIO : L_duty_ratio;
+        R_duty_ratio = (R_duty_ratio < MIN_DUTY_RATIO)? MIN_DUTY_RATIO : R_duty_ratio;
+
+        is_stop = (L_pulse_per_interval == 0 && R_pulse_per_interval == 0)?
+                  is_stop + INTERVAL : 0.0F;
+
+        // 静止状態が3秒続いたら、高 duty 比を入力
+        if (is_stop > 3.0F) {
+            L_motor_out_forw.write(0.87);
+            R_motor_out_forw.write(0.87);
+        } else {
+            L_motor_out_forw.write(L_duty_ratio);
+            R_motor_out_forw.write(R_duty_ratio);
+        }
+
+
+
+        k = (((k_counter1 + k_counter2) / (24.0F * 38.2F)) * 55.0F * PI) / 2.0F;
+
+        if(k >= 200.0F) {
+            if((oddeven % 2) == 1) { //200mmおきにLEDを点滅。奇数回目のループだったらLEDを消す
+                led_out_mileage = 0;
+            } else {
+                led_out_mileage = 1;
+            }
+            oddeven++;
+            k_counter1 = 0;
+            k_counter2 = 0;
+        }
+        wait(INTERVAL);
+    }
+}
+