Diff: main.cpp

Revision:

0:5e29061237cd

diff -r 000000000000 -r 5e29061237cd main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jan 14 04:31:57 2020 +0000
@@ -0,0 +1,401 @@
+#include "mbed.h"
+
+int mode_debug = 1; //printf()表示
+
+
+
+//走行用プログラム
+//モータードライバー＋回転数計測プログラム
+//走るためのプログラム+道が白か黒かを判定するプログラム
+//A入力に右のモーター、B入力に左のモーター
+//58mm径タイヤの一周は約182.21mm
+
+InterruptIn enc_R(D11);//フォトインタラプタ右
+InterruptIn enc_L(D12);//フォトインタラプタ左
+
+AnalogIn sensor_R(A0);//進行方向右のフォトリフレクタセンサー    
+AnalogIn sensor_L(A2);//進行方向左のセンサー
+AnalogIn sensor_C(A1);//中央配置のセンサー
+int R_on_off =0;//左右の白黒
+int L_on_off =0;
+int C_on_off =0;
+int pre_R_on_off =0;//1つ前の過程の、センサで決めた白黒（白なら0、黒なら1）
+int pre_L_on_off =0;
+int pre_C_on_off =1;//スタート時、中央センサーのみ黒の上
+
+int frame_counter = 0; //時間変化関数
+
+DigitalOut myled(A5);//200mmごとに光らせるLED用
+DigitalOut power_on_led(A6);
+
+
+PwmOut AIN1(D6);//右モーター
+PwmOut AIN2(D5);
+PwmOut BIN1(D9);//左モーター
+PwmOut BIN2(D10);//D9とD10は回路上で逆にしないと、なぜか回転方向が逆転
+
+//デューティー比関連の変数、定数
+
+const float R_duty_basic = 0.22f;//基本のデューティー比//0.25
+const float L_duty_basic = 0.23f;//基本のデューティー比//0.25
+const float roll_duty_small = 0.19f;//右左折時に作用 白黒黒//0.25
+const float roll_duty_large = 0.21f;//右左折時に作用 白黒黒//0.30
+const float medium_roll_duty_small = 0.19f;//中右左折時に作用 白白黒//0.25
+const float medium_roll_duty_large = 0.25f;//中右左折時に作用 白白黒//0.35
+const float sharp_roll_duty_forward = 0.20f;//急カーブ時に作用    超信地旋回   白白白//0.25
+const float sharp_roll_duty_back = 0.20f;//急カーブ時に作用    超信地旋回   白白白//0.25
+//const float duty_ignition = 0.20f;//急カーブ修了時にスピードアップ   
+
+//int re_ignition_counter = 0;//下の格納
+//int re_ignition_limit = 2;//スピードアップのカウンター(回数＊0.01s)
+
+//フォトリフレクタセンサ関連の定数
+
+const float L_black = 2.3;//左センサの黒値
+const float L_white = 0.3;//白値
+const float R_black = 0.8;//右センサの黒値
+const float R_white = 0.2;//
+const float C_black = 1.2;//中心センサの黒値
+const float C_white = 0.2;//
+
+
+//float L_target = (L_black + L_white) / 2;
+//float R_target = (R_black + R_white) / 2;
+//float C_target = (C_black + C_white) / 2;
+
+float L_target = 1.0;
+float R_target = 0.6;
+float C_target = 0.6;
+
+//回転数関連の変数、定数
+float R_rot=0;//右タイヤの１秒間あたり回転数
+float L_rot=0;
+float pre_R_rot=0;//1つ前の過程の右モーターの累計回転数
+float pre_L_rot=0;
+float R_target_rot=0;//１秒間あたりの右目標タイヤ回転数
+float L_target_rot=0;//１秒間あたりの左目標タイヤ回転数
+float counter_R =0;//右モーターの累計回転数
+float pre_counter_R = 0;//1つ前の過程の右のモーターの累計回転数
+float counter_L =0;
+float pre_counter_L = 0;
+
+//距離、時間関係の変数、定数
+float distance_per_frame = 0.0f;//距離の初期値
+float total_distance = 0.0f;//距離のトータル
+float delta = 0.01;//計測、判断時間の周期
+
+//モーター回転数計測
+void event_handler_R(void){//右モーター回転数計測
+    counter_R++;
+}
+
+void event_handler_L(void){//左モーター
+    counter_L++;
+}
+
+
+//ブレーキ
+void motorStop(PwmOut IN1,PwmOut IN2) {
+    IN1 = 1;
+    IN2 = 1;
+}
+
+//正転
+
+void motorForward(PwmOut IN1,PwmOut IN2,float duty) {
+    IN1 = duty;
+    IN2 = 0;
+}
+
+//逆転
+void motorReverse(PwmOut IN1,PwmOut IN2,float duty) {
+    IN1 = 0;
+    IN2 = duty;
+}
+//空転
+void motorIdling(PwmOut IN1,PwmOut IN2) {
+    IN1 = 0;
+    IN2 = 0;
+}
+
+//停止
+void machine_Stop() {
+    motorStop(AIN1,AIN2);
+    motorStop(BIN1,BIN2);
+}
+
+//前進
+void machine_Forward(float duty_R,float duty_L) {
+    motorReverse(AIN1,AIN2,duty_R);//逆転のほうが制御しやすいので前進時は逆転を使用
+    motorReverse(BIN1,BIN2,duty_L);
+}
+
+//後退
+void machine_Back(float duty_R,float duty_L) {
+    machine_Stop();
+    wait(2);
+    motorForward(AIN1,AIN2,duty_R);
+    motorForward(BIN1,BIN2,duty_L);
+}
+/*
+//右カーブ
+void Right_rotation(float duty_R,float duty_L) {
+
+    motorReverse(AIN1,AIN2,duty_R);
+    motorReverse(BIN1,BIN2,duty_L);
+}
+
+//左カーブ
+void Left_rotation(float duty_R,float duty_L) {
+
+    motorReverse(AIN1,AIN2,duty_R);
+    motorReverse(BIN1,BIN2,duty_L);
+}
+*/
+
+
+//右カーブ(超信地旋回
+void Right_high_rotation(float duty_R,float duty_L) {
+
+    motorForward(AIN1,AIN2,duty_R);
+    motorReverse(BIN1,BIN2,duty_L);
+}
+
+//左カーブ(超信地旋回
+void Left_high_rotation(float duty_R,float duty_L) {
+    
+    motorReverse(AIN1,AIN2,duty_R);
+    motorForward(BIN1,BIN2,duty_L);
+}
+
+enum branch_mode{//制御の状態記号
+    STRAIGHT =0,
+    RIGHT,
+    LEFT,
+    MEDIUM_R,
+    MEDIUM_L,
+    SHARP_R,
+    SHARP_L,
+    REVERSE
+    };
+    
+int mode;//展開中の制御状態記号の格納
+int pre_mode = mode;//一つ前の状態記号の格納:線を外れた際に引用
+
+        
+enum restart_ignition{//減速後の復帰のための操作に使用する信号
+    no=0,
+    caution,
+    ready,
+    yes
+    };
+    
+//int re_ignition;//上restart_ignitionの格納
+
+/*
+restart_ignitionについて
+
+[re_ignition = no]では、通常の走行状態
+[re_ignition = ready]は急カーブ中の状態
+この状態で中央のセンサーが黒を感知すると[re_ignition = yes]となり、
+一時的にduty比を上げて、走行させる。その後[re_ignition = no]に戻る。
+*/
+
+//センサーの色判定と走行モード決め
+void Check_reflecter(AnalogIn sensor_R, AnalogIn sensor_L,AnalogIn sensor_C){//リフレクタの黒白判定＆移動方向の決定
+    if(mode_debug==1){    
+    printf("sensor_Right:L432[%.3f]>\n",sensor_R*3.3F);//電圧測定
+    printf("sensor_Left:L432[%.3f]>\n",sensor_L*3.3F);//電圧測定
+    printf("sensor_Center:L432[%.3f]>\n\n",sensor_C*3.3F);//電圧測定  
+    }
+
+    if(sensor_R*3.3F>R_target)
+    {
+        R_on_off=1;
+    }else{
+        R_on_off=0;
+    }
+    if(sensor_L*3.3F>L_target)
+    {
+        L_on_off=1;
+    }else{
+        L_on_off=0;
+    }
+    if(sensor_C*3.3F>C_target)
+    {
+        C_on_off=1;
+    }else{
+        C_on_off=0;
+    }
+    
+    
+    if(L_on_off==0 && C_on_off==0 && R_on_off==0)//線の外
+    {
+        if(mode == MEDIUM_R || mode == RIGHT || mode == SHARP_R)
+        {
+            mode = SHARP_R;
+            Right_high_rotation(sharp_roll_duty_back,sharp_roll_duty_forward);
+            ////re_ignition = ready;
+        }else if(mode == MEDIUM_L || mode == LEFT || mode == SHARP_L){
+            mode = SHARP_L;
+            Right_high_rotation(sharp_roll_duty_forward,sharp_roll_duty_back);
+            //re_ignition = ready;
+        }
+            
+        if(mode_debug==1){
+        printf("I'm going out of line! Previous condition is %d \n",mode);
+        }
+        
+    }
+    else if(L_on_off==1 && C_on_off==0 && R_on_off==0)//左折
+    {
+        mode = MEDIUM_L;
+        machine_Forward(medium_roll_duty_large,medium_roll_duty_small);
+        //re_ignition = caution;
+        if(mode_debug==1){
+        printf("M_LEFT\n");
+        }
+    }
+    else if(L_on_off==0 && C_on_off==0 && R_on_off==1)//右折
+    {
+        mode = MEDIUM_R;
+        machine_Forward(medium_roll_duty_small,medium_roll_duty_large);
+        //re_ignition = caution;
+        if(mode_debug==1){
+        printf("M_RIGHT\n");
+        }
+    }
+    else if(L_on_off==1 && C_on_off==0 && R_on_off==1)//黒白黒、エラー、back
+    {
+        //mode = REVERSE;
+        //re_ignition = no;
+        if(mode_debug==1){
+        printf("REVERSE\n");
+        }
+    }
+    else if(L_on_off==0 && C_on_off==1 && R_on_off==0)//前進
+    {
+        //re_ignition = no;
+        machine_Forward(R_duty_basic,L_duty_basic);
+        mode = STRAIGHT;
+        if(mode_debug==1){
+        printf("STRAIGHT\n");
+        }
+    }
+    else if(L_on_off==1 && C_on_off==1 && R_on_off==0)//左折
+    {
+        //re_ignition =no;
+        machine_Forward(roll_duty_large,roll_duty_small);
+        mode = LEFT;
+        if(mode_debug==1){
+        printf("LEFT\n");
+        }
+    }
+    else if(L_on_off==0 && C_on_off==1 && R_on_off==1)//右折
+    {
+        //re_ignition = no;
+        machine_Forward(roll_duty_small,roll_duty_large);
+        mode = RIGHT;
+        if(mode_debug==1){
+        printf("RIGHT\n");
+        }
+    }
+    else if(L_on_off==1 && C_on_off==1 && R_on_off==1)//all black（空中or十字部)よって直進
+    {
+        //re_ignition = no;
+        machine_Forward(R_duty_basic,L_duty_basic);
+        mode = STRAIGHT;
+        if(mode_debug==1){
+        printf("STRAIGHT\n");
+        }
+    }
+    
+    if(pre_C_on_off != C_on_off || pre_R_on_off != R_on_off || pre_L_on_off != L_on_off)//新地回転の前と後でre_ignitionを添加
+    {
+        /*if(re_ignition == caution || re_ignition ==ready)
+        {
+            re_ignition = yes;
+        }*/
+        if(mode_debug==1)
+        {
+             printf("Judgement of sensor L[%d] C[%d] R[%d]\n",L_on_off,C_on_off,R_on_off);//[1]なら黒判定
+        }
+        //frame_counter = 0;
+    }
+    else{
+        //frame_counter++;
+        }       
+}
+
+
+
+//距離計算関数
+void distance_calculate (void)
+{
+    distance_per_frame = (R_rot+L_rot)/2.0f*182.12f;//右の回転数と左の回転数の平均に、一周の距離をかける
+    total_distance += distance_per_frame;
+    printf("Total Distance [%f]     Distance per Frame [%f]\n",total_distance,distance_per_frame);
+    printf("Left and right rotation speed per frame R[%f]  L[%f]\n",R_rot,L_rot);
+    
+
+}
+
+//LED点灯プログラム
+void led_lightning(float total_distance)
+{
+    int period_100mm =0;
+    for (int i =0;i<500;i++)
+    {
+        if( total_distance >= (float)i*200 && total_distance < ((float)i+1)*200)
+        {
+            period_100mm = i;
+            break;
+        }
+    }        
+    if(period_100mm%2 == 1)
+    {
+        myled = 1;
+        printf("LED IS [ON]\n");
+        
+    }else
+    {
+            myled = 0;
+            printf("LED IS [OFF]\n\n\n");
+    }
+}
+    
+
+void memorize_privious_value(void){
+    pre_mode = mode;
+    
+    pre_C_on_off = C_on_off;
+    pre_R_on_off = R_on_off;
+    pre_L_on_off = L_on_off;
+    
+    pre_counter_R = counter_R;
+    pre_counter_L = counter_L;   
+    pre_R_rot=R_rot;
+    pre_R_rot=R_rot;
+}
+
+int main() {    
+    AIN1.period(0.001);
+    AIN2.period(0.001);
+    BIN1.period(0.001);
+    BIN2.period(0.001);
+    
+    power_on_led=1;
+    machine_Stop();
+    mode = STRAIGHT;
+    //re_ignition=no;
+    
+    while(1) {
+ //       wait(delta);
+        Check_reflecter(sensor_R,sensor_L,sensor_C);
+
+        distance_calculate();
+        
+        memorize_privious_value();
+        led_lightning(total_distance);
+    }
+} 
\ No newline at end of file