Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test07 by
Diff: main.cpp
- Revision:
- 3:aaa2fde4fafd
- Parent:
- 2:c5996dd62e9c
- Child:
- 4:eabb7a738ff8
--- a/main.cpp Fri Mar 23 12:34:57 2018 +0000 +++ b/main.cpp Sat Mar 24 10:50:25 2018 +0000 @@ -13,6 +13,9 @@ 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); @@ -60,25 +63,44 @@ float tar_y_dist = 0.0; bool ontheline = false; int directmode = STRAIGHT; - -int main() { +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); - if(directmode == RIGHT) master.read(addr2,buff2,2); - if(directmode == LEFT) master.read(addr3,buff3,2); - + //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); + //linecheck(buff1,linedata_1); x_dist = Enc_Carry_X.read_rotate(); y_dist = Enc_Carry_Y.read_rotate(); - kbtpc.putc(1); + kbtpc.putc(state); if(kbtpc.readable()){ kbtread = kbtpc.getc(); } @@ -96,6 +118,10 @@ directmode = RIGHT; }else if(kbtread == LEFT){ directmode = LEFT; + }else if(kbtread == MANUAL){ + state = MANUAL; + }else if(kbtread == START){ + state = START; } //--------------手動-------------------------------------- if(mode == true){ @@ -114,23 +140,155 @@ //--------------自動--------------------------------- }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; - }else{ - switch(directmode){ + } + */ + 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); + //linecheck(buff1, linedata_1); output = S_pid.compute((double)linedata_1[0]); if(output >= 0){ rpower = power; @@ -144,7 +302,7 @@ break; case RIGHT: //master.read(addr2,buff2,2); - linecheck(buff2, linedata_2); + //linecheck(buff2, linedata_2); output = R_pid.compute((double)linedata_2[0]); if(output >= 0){ rpower = power; @@ -158,12 +316,12 @@ break; case LEFT: //master.read(addr3,buff3,2); - linecheck(buff3, linedata_3); + //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); + Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); }else { rpower = power + (-1 * output); lpower = power; @@ -172,16 +330,18 @@ break; default: break; - } } } } - //pc.printf("kbtread = %d \r" ,kbtread); + //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("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); @@ -189,8 +349,8 @@ //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("lpower = %lf" ,lpower); + pc.printf("dicmode = %d \r",directmode); pc.printf("output = %lf \n" ,output); } } @@ -202,25 +362,29 @@ tar_y_dist = y_dist; } if(ontheline == true){ - if((y_dist - tar_y_dist) >= STOP_DIST){ + if((y_dist - tar_y_dist) >= Y_STOP_DIST){ ontheline = false; crosscount++; } } }else if(dmode == RIGHT){ - ontheline = true; - tar_x_dist = x_dist; + if(linedata_2[1] == 1){ + ontheline = true; + tar_x_dist = x_dist; + } if(ontheline == true){ - if((x_dist - tar_x_dist) >= STOP_DIST){ + if((x_dist - tar_x_dist) >= X_STOP_DIST){ ontheline = false; crosscount++; } } }else if(dmode == LEFT){ - ontheline = true; - tar_x_dist = x_dist; + if(linedata_3[1] == 1){ + ontheline = true; + tar_x_dist = x_dist; + } if(ontheline == true){ - if((x_dist - tar_x_dist) <= STOP_DIST){ + if((tar_x_dist - x_dist) >= X_STOP_DIST){ ontheline = false; crosscount++; } @@ -285,5 +449,4 @@ data[1] = 0; } prelinedata = data[0]; - -} \ No newline at end of file +}