Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of nhk_2018_undercarry_test07 by
Diff: main.cpp
- Revision:
- 0:f6828b914b1c
- Child:
- 1:f4dbd6b9cc27
diff -r 000000000000 -r f6828b914b1c main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 21 06:51:59 2018 +0000 @@ -0,0 +1,163 @@ +#include "mbed.h" +#include "config.h" +#include "moter.h" +#include "PID.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); + +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( PC_7 ), + DigitalOut( PB_15) +}; + +DigitalOut dir_pins_2[] = { + DigitalOut( PB_12 ), + DigitalOut( PA_6 ), + DigitalOut( PB_6 ), + DigitalOut( PB_1) +}; + +bool mode = true;//trueでラジコン、falseでオート +bool stopflag = true;//trueで機体停止 +int posit[2] = {0};//機体の現在位置 +int nextposit[2] = {0};//機体の位置指定 +double power = POWER; +int kbtread = 0; +int count = 0; +int counter = 0; +int linedata = 0; +int prelinedata = 0; +double output = 0.0; +bool rpower = false; +bool lpower = false; + +int linecheck(char *buff){ + if(buff[0] == 24){ + linedata = 0; + }else if(buff[0] == 56){ + linedata = 1; + }else if(buff[0] == 48){ + linedata = 2; + }else if(buff[0] == 112){ + linedata = 3; + }else if(buff[0] == 96){ + linedata = 4; + }else if(buff[0] == 224){ + linedata = 5; + }else if(buff[0] == 192){ + linedata = 6; + }else if(buff[0] == 128){ + linedata = 7; + }else if(buff[0] == 28){ + linedata = -1; + }else if(buff[0] == 12){ + linedata = -2; + }else if(buff[0] == 14){ + linedata = -3; + }else if(buff[0] == 6){ + linedata = -4; + }else if(buff[0] == 7){ + linedata = -5; + }else if(buff[0] == 3){ + linedata = -6; + }else if(buff[0] == 1){ + linedata = -7; + }else if(buff[0] == 15 && buff[1] == 15){ + linedata = 255; + }else if(buff[0] == 0 && buff[1] == 0){ + linedata = -255; + }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); + } +}