Anon Anon
/
Nucleo_pwm_example
here
Diff: header.h
- Revision:
- 1:18f0113ac5a2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/header.h Mon Nov 20 01:16:14 2017 +0000 @@ -0,0 +1,145 @@ +#ifndef Header_H +#define Header_H +//IR: +//PB 7 Left Side +//PB 0 Front Left +//PC 11 Front Right +//PC 10 Side Right +//Receiver: +// Left Side PC_0 +// Front Left PC_1 +// Front Right PA_4 +// Side Left PA_0 +DigitalOut LeftIR(PB_7); +DigitalOut FrontLeftIR(PB_0); +DigitalOut FrontRightIR(PC_11); +DigitalOut RightIR(PC_10); +AnalogIn LeftReceiver(PC_0); +AnalogIn FrontLeftReceiver(PC_1); +AnalogIn FrontRightReceiver(PA_4); +AnalogIn RightReceiver(PA_0); +PwmOut lpwmf(PA_7); +PwmOut lpwmb(PB_6); +PwmOut rpwmf(PB_10); +PwmOut rpwmb(PC_7); +PinName rfront(PB_3); +PinName rback(PA_15); +PinName lfront(PA_1); +PinName lback(PC_4); +Serial pc(SERIAL_TX, SERIAL_RX); +QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING); +QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING); +const float rbase = 0.16f +.1f; +const float lbase = 0.09f + .1f; +const float WALL_IR_L = 0.9f; +const float WALL_IR_R = 0.9f; +const float WALL_IR_FL = 0.92f; +const float WALL_IR_FR = 0.92f; +Timer t_time; +struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?) + pid(){ + integral = 0.0f; + prev = 0.0f; + kp = .01f; //the ks should be negative to counteract error + ki = 0.0f; + kd = 0.8f; + } + float integral; + int prev; + float kp; //the ks should be negative to counteract error + float ki; + float kd; +}; + +void resetpid(struct pid *e) { + e->integral = 0.0f; + e->prev = 0.0f; +} + +float getFix(struct pid *e, float error, float time) { + float d = (error - e->prev)/(float)time; + e->integral += error * time; + e->prev = error; + return (float)(e->kp * error + e->ki * e->integral + e->kd * d); +} + +float constrain(float l_limit, float u_limit, float val){ + if (val < l_limit){ + return l_limit; + } + else if(val > u_limit){ + return u_limit; + } + return val; +} + +struct IRstruct{ + IRstruct(){ + statuscode = 0; + } + float leftIR, leftfrontIR, rightfrontIR, rightIR; + unsigned int statuscode; +}; + +unsigned int readIR(IRstruct& in_ir){ + in_ir.leftIR = LeftReceiver.read(); + in_ir.rightIR = RightReceiver.read(); + in_ir.leftfrontIR = FrontLeftReceiver.read(); + in_ir.rightfrontIR = FrontRightReceiver.read(); + pc.printf("LeftIR: %f RightIR: %f leftfronIR: %f rightfrontIR: %f \n", in_ir.leftIR, in_ir.rightIR, in_ir.leftfrontIR, in_ir.rightfrontIR); + + //We then set the status codes to be analyzed later for what we should do + in_ir.statuscode = (in_ir.statuscode|0x0); + + //0000...FL,FR,L,R + //Bottom is more of an abstraction, we probably don't even need it + if(in_ir.leftIR > WALL_IR_L) + in_ir.statuscode = (in_ir.statuscode|2); // 0...0 | 1 -> 0...1 + if(in_ir.rightIR > WALL_IR_R) + in_ir.statuscode = in_ir.statuscode|1; + if(in_ir.leftfrontIR > WALL_IR_FL) + in_ir.statuscode = in_ir.statuscode|8; + if(in_ir.rightfrontIR > WALL_IR_FR) + in_ir.statuscode = in_ir.statuscode|4; + return in_ir.statuscode; +} + +void turn_right(){ + float l_init = LeftEncoder.getPulses(); + while((LeftEncoder.getPulses() - l_init) < 10.0f){ + lpwmb = 0.0f; + rpwmf = 0.0f; + lpwmf = lbase; + rpwmb = rbase; + } +} + +void turn_left(){ + float r_init = RightEncoder.getPulses(); + while((RightEncoder.getPulses() - r_init) < 1000.0f){ + lpwmf = 0.0f; + rpwmb = 0.0f; + lpwmb = lbase; + rpwmf = rbase; + } +} + +void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){ + IRstruct ir_read; + int status = readIR(ir_read); + if(ir_read.leftIR > WALL_IR_L && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR){ //High in front and left -> Turn right + turn_right(); + } + else if(ir_read.rightIR > WALL_IR_R && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR){ //High in front and right -> Turn left + turn_left(); + } + float error_ir = (ir_read.rightIR - ir_read.leftIR)/16.0f; + if(error_ir < 0.0001f){ + resetpid(&IR_lman); + } + float adjust_l = getFix(&IR_lman, error_ir, dt); + lspeed = constrain(0.0f, 0.4f, lbase - adjust_l); + rspeed = constrain(0.0f, 0.4f, lbase); +} + +#endif \ No newline at end of file