Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test07 by
main.cpp
- Committer:
- kenken0721
- Date:
- 2018-03-24
- Revision:
- 3:aaa2fde4fafd
- Parent:
- 2:c5996dd62e9c
- Child:
- 4:eabb7a738ff8
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]; }