Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test09 by
Diff: main.cpp
- Revision:
- 1:f4dbd6b9cc27
- Parent:
- 0:f6828b914b1c
- Child:
- 2:c5996dd62e9c
diff -r f6828b914b1c -r f4dbd6b9cc27 main.cpp --- a/main.cpp Wed Mar 21 06:51:59 2018 +0000 +++ b/main.cpp Thu Mar 22 06:16:03 2018 +0000 @@ -2,12 +2,16 @@ #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); +Encoder Enc_Carry_X(PA_15,PA_14,PC_1); +Encoder Enc_Carry_Y(PF_1,PA_1,PA_0); +int linecheck(char *buff); PwmOut pwm_pins[] = { PwmOut( PC_8 ), @@ -41,8 +45,84 @@ int linedata = 0; int prelinedata = 0; double output = 0.0; -bool rpower = false; -bool lpower = false; +char buff1[2]; +char buff2[2]; +char buff3[2]; +float x_dist = 0.0; +float y_dist = 0.0; + + +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_pins_1, dir_pins_2); + }else if(kbtread == 3){ + Right(power, power, pwm_pins, dir_pins_1, dir_pins_2); + }else if(kbtread == 4){ + 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{ + if(line == 255){ + stopflag = true; + Stop(pwm_pins, dir_pins_1, dir_pins_2); + }else{ + output = S_pid.compute((double)line); + if(output >= 0){ + Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2); + }else { + Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, 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); + } +} + int linecheck(char *buff){ if(buff[0] == 24){ @@ -78,86 +158,11 @@ }else if(buff[0] == 15 && buff[1] == 15){ linedata = 255; }else if(buff[0] == 0 && buff[1] == 0){ - linedata = -255; + linedata = prelinedata; }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); - } -} +} \ No newline at end of file