File content as of revision 0:f6828b914b1c:

#include "mbed.h"
#include "config.h"
#include "moter.h"
#include "PID.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);

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( PC_7 ),
    DigitalOut( PB_15)
};

DigitalOut dir_pins_2[] = {
    DigitalOut( PB_12 ),
    DigitalOut( PA_6 ),
    DigitalOut( PB_6 ),
    DigitalOut( PB_1)
};

bool mode = true;//trueでラジコン、falseでオート
bool stopflag = true;//trueで機体停止
int posit[2] = {0};//機体の現在位置
int nextposit[2] = {0};//機体の位置指定
double power = POWER;
int kbtread = 0;
int count = 0;
int counter = 0;
int linedata = 0;
int prelinedata = 0;
double output = 0.0;
bool rpower = false;
bool lpower = false;

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

    return linedata;
}
    
int main() {
    
    S_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);
    kbtpc.baud(2400);
    
    char buff1[2];
    
    while (true) {
        master.read(addr1,buff1,2);
        int line = linecheck(buff1);
        kbtpc.putc(1);
        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;
        }
//--------------手動--------------------------------------
        if(mode == true){
            led2 = 1.0;
            if(kbtread == 1){
                Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else if(kbtread == 2){
                Back(power, power, pwm_pins, dir_pins1, dir_pins_2);
            }else if(kbtread == 3){
                Right(power, power, pwm_pins, dir_pins1, dir_pins_2);
            }else if(kbtread == 4){
                Left(power, power, pwm_pins, dir_pins1, dir_pins_2);
            }else{
                Stop(pwm_pins, dir_pins1, dir_pins_2);
            }
//--------------自動---------------------------------
        }else if(mode == false){
            led2 = 0.0;
            if(stopflag == true){
                Stop(pwm_pins, dir_pins1, dir_pins_2);
            }else{
                if(line == 255 || line == -255){
                    stopflag = true;
                    Stop(pwm_pins, dir_pins1, dir_pins_2);
                    wait(1.0);
                }else{
                    output = S_pid.compute((double)line);
                    if(output >= 0){
                        rpower = false;
                        lpower = true;
                        Straight(power, power + output, pwm_pins, dir_pins1, dir_pins_2);
                    }else {
                        lpower = false;
                        rpower = true;
                        Straight(power + (-1 * output), power, pwm_pins, dir_pins1, dir_pins_2);
                    }
                }
            }
        }
        //pc.printf("kbtread = %d" ,kbtread);     
        ///pc.printf("buff[0] = %d" ,buff1[0]);
        //pc.printf("buff[1] = %d" ,buff1[1]);
        pc.printf("line    = %d" ,line);
        pc.printf("rpower    = %d" ,rpower);
        pc.printf("lpower    = %d" ,lpower);
        pc.printf("output  = %lf \n" ,output);
    }
}