File content as of revision 0:617b5fe8f980:

#include "mbed.h"

//走行用プログラムver3.2
//11/11pm21:00時点
//モータードライバー＋回転数計測プログラム
//走るためのプログラム+道が白か黒かを判定するプログラム
//A入力に右のモーター、B入力に左のモーター

InterruptIn enc_R(D12);//フォトインタラプタ右
InterruptIn enc_L(D11);//フォトインタラプタ左

AnalogIn sensor_R(A0);//進行方向右のフォトリフレクタセンサー    
AnalogIn sensor_L(A1);//進行方向左のセンサー
AnalogIn sensor_C(A2);//中央配置のセンサー

DigitalOut led(LED1);

int counter =0;

void event_handler(void){
    counter++;
    led =!led;
}


PwmOut AIN1(D6);
PwmOut AIN2(D5);
PwmOut BIN1(D9);
PwmOut BIN2(D10);//D9とD10は回路上で逆にしないと、なぜか回転方向が逆転
 
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 machine_Stop(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2) {
    motorStop(AIN1,AIN2);
    motorStop(BIN1,BIN2);
}

//前進
void machine_Forward(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
    motorForward(AIN1,AIN2,duty);
    motorForward(BIN1,BIN2,duty);
}

//後退
void machine_Back(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
    machine_Stop(AIN1,AIN2,BIN1,BIN2);
    wait(2);
    motorReverse(AIN1,AIN2,duty);
    motorReverse(BIN1,BIN2,duty);
}

//右カーブ
void Right_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {
    motorForward(AIN1,AIN2,duty-0.4f);
    motorForward(BIN1,BIN2,duty);
}

//左カーブ
void Left_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {

    motorForward(AIN1,AIN2,duty);
    motorForward(BIN1,BIN2,duty-0.4f);
}


//右急カーブ
void Right_high_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {

    motorForward(AIN1,AIN2,duty-0.6f);
    motorForward(BIN1,BIN2,duty);
}

//左急カーブ
void Left_high_rotation(PwmOut AIN1,PwmOut AIN2,PwmOut BIN1,PwmOut BIN2,float duty) {

    motorForward(AIN1,AIN2,duty);
    motorForward(BIN1,BIN2,duty-0.6f);
}
//制御(maenoshin)
void control (int Left, int Center, int Right, int sec,float duty){
    if(Left == 0 && Center == 1 && Right == 0){//全速前進だ！
        machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);
        printf("Direction%d:Forward\n",sec);
        }
    if(Left == 1 && Center == 1 && Right == 0){//左折
        Left_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
        printf("Direction%d:Left turn\n",sec);
        }
    if(Left == 0 && Center == 1 && Right == 1){//右折
        Right_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
        printf("Direction%d:Right turn\n",sec);
        }
    if(Left == 1 && Center == 0 && Right == 0){//左折(急)
        Left_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
        printf("Direction%d:Left turn hurry!\n",sec);
        }
    if(Left == 0 && Center == 0 && Right == 1){//右折(急)
        Right_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
        printf("Direction%d:Right turn hurry!\n",sec);
        }
    else{ printf("Direction%d:error\n",sec);
    }
    }
    
    
        
        
//最終的には
//左のモーターのスピード = 比例ゲイン×（左のセンサ値 - 左の黒値） / （左の白値 - 左の黒値）＋基本最低スピード
//右のモーターのスピード = 比例ゲイン×（右のセンサ値 - 右の黒値） / （右の白値 - 右の黒値）＋基本最低スピード

//白値はセンサーで白を検出する最大値
//黒値はセンサーで黒を検出する最少値

int main() {
 
    machine_Stop(AIN1,AIN2,BIN1,BIN2);
    int sec =0;

    while(1) {
        float duty = 0.9f;//デューティー比90%
        int Center = 0;
        int Right = 0;
        int Left =0;
        if(sensor_C*3.3f>1.0f){//中央センサーが黒
            Center = 1;
        }else{Center =0;}
        if(sensor_L*3.3f>1.0f){//左センサーが黒
            Left = 1;
        }else{Left =0;}
        if(sensor_R*3.3f>1.0f){//右センサーが黒
            Right = 1;
        }else{Right =0;}
        
        control(Left,Center,Right,sec,duty);
        wait (5);
        machine_Stop(AIN1,AIN2,BIN1,BIN2);
        wait (2);
        sec++;
          
         
        
        
            //if(sensor_L*3.3f<1.2f){//左センサーだけ白
            //    Right_rotation(AIN1,AIN2,BIN1,BIN2,duty);
            //    printf("Direction:Right turn\n");
            //}
            
            //else if(sensor_R*3.3f<1.2f){//右センサーだけ白
            //    Left_rotation(AIN1,AIN2,BIN1,BIN2,duty); 
            //    printf("Direction:Left turn\n");
            //}
            //else{
            //   machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);
            //    printf("Direction:Forward\n");
            //}
        //}
            
        //else if(sensor_C<1.2f){//中央センサーが白
            
           // duty = 0.6f;//デューティー比60%
            //machine_Forward(AIN1,AIN2,BIN1,BIN2,duty);//ゆっくり進む
            
            //if(sensor_R*3.3f<1.0f){//中央が出た後右センサーが出たら、左に急カーブ
             //   Left_high_rotation(AIN1,AIN2,BIN1,BIN2,duty);
               // printf("Direction:Left high speed turn\n");
            //} 

            //else if(sensor_L*3.3f<1.0f){//中央が出た後左センサーが出たら、右に急カーブ
              //  Right_high_rotation(AIN1,AIN2,BIN1,BIN2,duty);
               // printf("Direction:Right high speed turn\n");
            //} 
            //else if((sensor_R*3.3f<1.0f)&&(sensor_L*3.3f<1.0f)){//全部白（勢いがつきすぎて飛び出たなど）
            //    duty = 0.6f;//デューティー比60%
             //   machine_Back(AIN1,AIN2,BIN1,BIN2,duty);//ゆっくり後ろにさがる
              //  printf("Direction:Back\n");
           // }
        //}

        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",sensor_C*3.3F);//電圧測定
        wait(3);
    }
}