File content as of revision 3:aaa2fde4fafd:

#include "mbed.h"
#include "config.h"
#include "moter.h"
#include "PID.h"
#include "encoder.h"

Serial pc(USBTX, USBRX);
Serial kbtpc(PC_12 ,PD_2);//serial5
I2C master(sda, scl);
DigitalOut led2(LED2);
PID S_pid(SP, SI, SD, 0.0);
PID R_pid(RP, RI, RD, 0.0);
PID L_pid(LP, LI, LD, 0.0);
Encoder Enc_Carry_Y(PA_15,PA_14,PC_1);
Encoder Enc_Carry_X(PA_4,PB_0,PB_7);
DigitalOut buzzer(PA_10);
DigitalOut lamp(PC_3);
Timer timer;
void linecheck(char *buff ,int data[2]);
void LineCheck(int dmode);

PwmOut pwm_pins[] = {
    PwmOut( PC_8 ),
    PwmOut( PC_6 ),
    PwmOut( PB_14 ),
    PwmOut( PB_13 )
};

DigitalOut dir_pins_1[] = {
    DigitalOut( PC_5 ),
    DigitalOut( PA_7 ),
    DigitalOut( PB_6 ),
    DigitalOut( PB_1)
};

DigitalOut dir_pins_2[] = {
    DigitalOut( PB_12 ),
    DigitalOut( PA_6 ),
    DigitalOut( PC_7 ),
    DigitalOut( PB_15)
};

int crosscount = 0;
bool mode = true;//trueでラジコン、falseでオート
bool stopflag = true;//trueで機体停止
double power = POWER;
double rpower = POWER;
double lpower = POWER;
int kbtread = 0;
int count = 0;
int counter = 0;
int linedata_1[2] = {0};
int linedata_2[2] = {0};
int linedata_3[2] = {0};
int prelinedata = 0;
double output = 0.0;
char buff1[2];
char buff2[2];
char buff3[2];
float x_dist = 0.0;
float y_dist = 0.0;
float tar_x_dist = 0.0;
float tar_y_dist = 0.0;
bool ontheline = false;
int directmode = STRAIGHT;
int state = START;
int times = 0;
bool throwing = false;
int main() {
    timer.reset();
    timer.stop();
    //timer.start();
    S_pid.init();
    R_pid.init();
    L_pid.init();
    pwm_pins[0].period(0.00005);
    pwm_pins[1].period(0.00005);
    pwm_pins[2].period(0.00005);
    pwm_pins[3].period(0.00005);
    pc.baud(115200);
    kbtpc.baud(9600);
    master.frequency(100000);
    lamp = 1.0;
    while (true) {
        //if(directmode == STRAIGHT){
            master.read(addr1,buff1,2);
            linecheck(buff1,linedata_1);
        //}
        //if(directmode == RIGHT){
            master.read(addr2,buff2,2);
            linecheck(buff2,linedata_2);
        //}
        //if(directmode == LEFT){
            master.read(addr3,buff3,2);
            linecheck(buff3,linedata_3);
        //}
        //master.read(addr1,buff1,2);
        //linecheck(buff1,linedata_1);
        //linecheck(buff1,linedata_1);
        //linecheck(buff1,linedata_1);
        x_dist = Enc_Carry_X.read_rotate();
        y_dist = Enc_Carry_Y.read_rotate();
        kbtpc.putc(state);
        if(kbtpc.readable()){
            kbtread = kbtpc.getc();
        }
        if(kbtread == 255){
            mode = true;
        }else if(kbtread == 150){
            mode = false;
        }else if(kbtread == 100){
            stopflag = true;
        }else if(kbtread == 200){
            stopflag = false;
        }else if(kbtread == STRAIGHT){
            directmode = STRAIGHT;
        }else if(kbtread == RIGHT){
            directmode = RIGHT;
        }else if(kbtread == LEFT){
            directmode = LEFT;
        }else if(kbtread == MANUAL){
            state = MANUAL;
        }else if(kbtread == START){
            state = START;
        }
//--------------手動--------------------------------------
        if(mode == true){
            led2 = 1.0;
            if(kbtread == STRAIGHT){
                Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else if(kbtread == BACK){
                Back(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else if(kbtread == RIGHT){
                Right(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else if(kbtread == LEFT){
                Left(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else{
                Stop(pwm_pins, dir_pins_1, dir_pins_2);
            }
//--------------自動---------------------------------
        }else if(mode == false){
            led2 = 0.0;
            //timer.reset();
            //timer.start();
            if(stopflag == true){
                Stop(pwm_pins, dir_pins_1, dir_pins_2);
            }else{
                LineCheck(directmode);
                switch(state){
                    case MANUAL:
                    if(crosscount == 1){
                        directmode = STOP;
                        Stop(pwm_pins, dir_pins_1, dir_pins_2);
                        crosscount = 0;
                    }
                    break;
                    
                    //------------------------------------------
                    case START:
                    if(crosscount == 0){
                        directmode = STRAIGHT;
                    }else if(crosscount == 1 && directmode == STRAIGHT){
                        directmode = LEFT;
                        prelinedata = 0;
                    }else if(crosscount == 1 && directmode == LEFT){
                        directmode = LEFT;
                    }else if(crosscount == 2){
                        directmode = STOP;
                        state = STAND_BY_1;
                        crosscount = 0;
                        prelinedata = 0;
                    }
                    break;
                    //-----------------------------------------------
                    case STAND_BY_1:
                    crosscount = 0;
                    directmode = STOP;
                    //buzzer = 1.0;
                    if(kbtread == RED_RECEIVE){
                        state = RED_RECEIVE;
                    }else if(kbtread == BLUE_RECEIVE){
                        state = BLUE_RECEIVE_1;
                    }
                    break;
                    case RED_RECEIVE:
                    if(crosscount == 0){
                        directmode = LEFT;
                    }else if(crosscount == 1 && throwing == false){
                        directmode = STOP;
                        prelinedata = 0;
                        if(kbtread == THROWING){
                            throwing = true;
                            buzzer = 1.0;
                            timer.start();
                            //lamp = 0.0;
                            //wait(3.0);
                            //buzzer = 0.0;
                            prelinedata = 0;
                            //directmode = RIGHT;
                        }
                    }else if(crosscount == 1 && throwing == true){
                        times = timer.read();
                        if(times > 2){
                            lamp = 0.0;
                            buzzer = 0.0;
                            directmode = RIGHT;
                            timer.reset();
                            timer.stop();
                        }
                    }else if(crosscount == 2){
                        directmode = STOP;
                        state = STAND_BY_1;
                        throwing = false;
                        crosscount = 0;
                    }
                    break;
                    case BLUE_RECEIVE_1:
                    if(crosscount == 0){
                        directmode = RIGHT;
                    }else if(crosscount == 1){
                        directmode = STRAIGHT;
                    }else if(crosscount == 2){
                        directmode = LEFT;
                    }else if(crosscount == 3){
                        directmode = STOP;
                        prelinedata = 0;
                        state = STAND_BY_2;
                        crosscount = 0;
                    }
                    break;
                    case STAND_BY_2:
                    crosscount = 0;
                    if(kbtread == BLUE_RECEIVE){
                        state = BLUE_RECEIVE_2;
                    }else if(kbtread == YELLOW_RECEIVE){
                        state = YELLOW_RECEIVE;
                    }
                    break;
                    case BLUE_RECEIVE_2:
                    if(crosscount == 0){
                        directmode = LEFT;
                    }else if(crosscount == 1){
                        if(throwing == false){
                            directmode = STOP;
                            prelinedata = 0;
                            if(kbtread == THROWING){
                                throwing = true;
                                buzzer = 1.0;
                                timer.start();
                                prelinedata = 0;
                            }
                        }else{
                            times = timer.read();
                            if(times > 2){
                                buzzer = 0.0;
                                directmode = RIGHT;
                                timer.reset();
                                timer.stop();
                            }
                        }
                    }else if(crosscount == 2){
                        directmode = STOP;
                        state = STAND_BY_2;
                        throwing = false;
                        crosscount = 0;
                    }
                    break;
                    case YELLOW_RECEIVE:
                    if(crosscount != 4){
                        directmode = LEFT;
                    }else{
                        directmode = STOP;
                    }
                    break;
                    default:
                    break;
                }
                /*
                if(crosscount == 1){
                    directmode = STOP;
                    Stop(pwm_pins, dir_pins_1, dir_pins_2);
                    crosscount = 0;
                }
                */
                switch(directmode){
                    case STOP:
                    Stop(pwm_pins, dir_pins_1, dir_pins_2);
                    break;
                    case STRAIGHT:
                    //master.read(addr1,buff1,2);
                    //linecheck(buff1, linedata_1);
                    output = S_pid.compute((double)linedata_1[0]);
                    if(output >= 0){
                        rpower = power;
                        lpower = power + output;
                        Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        rpower = power + (-1 * output);
                        lpower = power;
                        Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    case RIGHT:
                    //master.read(addr2,buff2,2);
                    //linecheck(buff2, linedata_2);
                    output = R_pid.compute((double)linedata_2[0]);
                    if(output >= 0){
                        rpower = power;
                        lpower = power + output;
                        Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        rpower = power + (-1 * output);
                        lpower = power;
                        Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    case LEFT:
                    //master.read(addr3,buff3,2);
                    //linecheck(buff3, linedata_3);
                    output = L_pid.compute((double)linedata_3[0]);
                    if(output >= 0){
                        rpower = power;
                        lpower = power + output;
                        Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        rpower = power + (-1 * output);
                        lpower = power;
                        Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    default:
                    break;
                }
            }
        }
        //pc.printf("kbtread = %d \r" ,kbtread); 
        //pc.printf("buff1[0] = %d \r" ,buff1[0]);
        //pc.printf("buff2[0] = %d \r" ,buff2[0]);
        //pc.printf("line1 = %d \r" ,linedata_1[0]);
        //pc.printf("line2 = %d \r" ,linedata_3[1]);
        //pc.printf("xdist = %f \r",x_dist);
        //pc.printf("tar_xdist = %f \r",tar_x_dist);
        //pc.printf("ontheline = %d \r",ontheline);
        
        //pc.printf("ydist = %f \r",y_dist);
        //pc.printf("tary = %f \r",tar_y_dist);
        //pc.printf("stopflag = %d \r",stopflag);
        //pc.printf("cross = %d \r",crosscount);
        //pc.printf("buff[1] = %d" ,buff1[1]);
        //pc.printf("line    = %d" ,line);
        //pc.printf("rpower    = %lf" ,rpower);
        //pc.printf("lpower    = %lf" ,lpower);  
        pc.printf("dicmode = %d \r",directmode);
        pc.printf("output  = %lf \n" ,output);
    }
}

void LineCheck(int dmode){
    if(dmode == STRAIGHT){
        if(linedata_1[1] == 1){
            ontheline = true;
            tar_y_dist = y_dist;
        }
        if(ontheline == true){
            if((y_dist - tar_y_dist) >= Y_STOP_DIST){
                ontheline = false;
                crosscount++;
            }
        }
    }else if(dmode == RIGHT){
        if(linedata_2[1] == 1){
            ontheline = true;
            tar_x_dist = x_dist;
        }
        if(ontheline == true){
            if((x_dist - tar_x_dist) >= X_STOP_DIST){
                ontheline = false;
                crosscount++;
            }
        }
    }else if(dmode == LEFT){
        if(linedata_3[1] == 1){
            ontheline = true;
            tar_x_dist = x_dist;
        }
        if(ontheline == true){
            if((tar_x_dist - x_dist) >= X_STOP_DIST){
                ontheline = false;
                crosscount++;
            }
        }
    }
}

void linecheck(char *buff, int data[2]){
    if(buff[0] == 24){
        data[0] = 0;
        data[1] = 0;
    }else if(buff[0] == 56){
        data[0] = 1;
        data[1] = 0;
    }else if(buff[0] == 48){
        data[0] = 2;
        data[1] = 0;
    }else if(buff[0] == 112){
        data[0] = 3;
        data[1] = 0;
    }else if(buff[0] == 96){
        data[0] = 4;
        data[1] = 0;
    }else if(buff[0] == 224){
        data[0] = 5;
        data[1] = 0;
    }else if(buff[0] == 192){
        data[0] = 6;
        data[1] = 0;
    }else if(buff[0] == 128){
        data[0] = 7;
        data[1] = 0;
    }else if(buff[0] == 28){
        data[0] = -1;
        data[1] = 0;
    }else if(buff[0] == 12){
        data[0] = -2;
        data[1] = 0;
    }else if(buff[0] == 14){
        data[0] = -3;
        data[1] = 0;
    }else if(buff[0] == 6){
        data[0] = -4;
        data[1] = 0;
    }else if(buff[0] == 7){
        data[0] = -5;
        data[1] = 0;
    }else if(buff[0] == 3){
        data[0] = -6;
        data[1] = 0;
    }else if(buff[0] == 1){
        data[0] = -7;
        data[1] = 0;
    }else if(buff[0] == 255){
        data[0] = prelinedata;
        data[1] = 1;
    }else if(buff[0] == 0){
        data[0] = prelinedata;
        data[1] = 0;
    }else{
        data[0] = prelinedata;
        data[1] = 0;
    }
    prelinedata = data[0];
}