File content as of revision 0:5e29061237cd:

#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);
    }
}