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]; }