Dependencies:   PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter

Fork of nhk_2018_undercarry_test09 by ケンタ ミヤザキ

main.cpp

Committer:
kenken0721
Date:
2018-03-23
Revision:
2:c5996dd62e9c
Parent:
1:f4dbd6b9cc27
Child:
3:aaa2fde4fafd

File content as of revision 2:c5996dd62e9c:

#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);
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 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(9600);
    master.frequency(100000);
    while (true) {
        if(directmode == STRAIGHT) master.read(addr1,buff1,2);
        if(directmode == RIGHT) master.read(addr2,buff2,2);
        if(directmode == LEFT) master.read(addr3,buff3,2);
            
        //master.read(addr1,buff1,2);
        linecheck(buff1,linedata_1);
        x_dist = Enc_Carry_X.read_rotate();
        y_dist = Enc_Carry_Y.read_rotate();
        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;
        }else if(kbtread == STRAIGHT){
            directmode = STRAIGHT;
        }else if(kbtread == RIGHT){
            directmode = RIGHT;
        }else if(kbtread == LEFT){
            directmode = LEFT;
        }
//--------------手動--------------------------------------
        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;
            
            if(stopflag == true){
                Stop(pwm_pins, dir_pins_1, dir_pins_2);
            }else{
                LineCheck(directmode);
                if(crosscount == 1){
                    directmode = STOP;
                    Stop(pwm_pins, dir_pins_1, dir_pins_2);
                    crosscount = 0;
                }else{
                    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(lpower, rpower, 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_1[1]);
        //pc.printf("xdist = %f \r",x_dist);
        //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) >= STOP_DIST){
                ontheline = false;
                crosscount++;
            }
        }
    }else if(dmode == RIGHT){
        ontheline = true;
        tar_x_dist = x_dist;
        if(ontheline == true){
            if((x_dist - tar_x_dist) >= STOP_DIST){
                ontheline = false;
                crosscount++;
            }
        }
    }else if(dmode == LEFT){
        ontheline = true;
        tar_x_dist = x_dist;
        if(ontheline == true){
            if((x_dist - tar_x_dist) <= 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];

}