Anon Anon / Mbed 2 deprecated Nucleo_pwm_example

here

Dependencies:   QEI mbed

header.h@1:18f0113ac5a2, 2017-11-20 (annotated)

Committer:
Showboo
Date:
Mon Nov 20 01:16:14 2017 +0000
Revision:
1:18f0113ac5a2
New Code updated for Assignment 4. Values for LED IR created and found. Debugging needed for left wheel which currently isn't spinning.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Showboo 1:18f0113ac5a2 1 #ifndef Header_H
Showboo 1:18f0113ac5a2 2 #define Header_H
Showboo 1:18f0113ac5a2 3 //IR:
Showboo 1:18f0113ac5a2 4 //PB 7 Left Side
Showboo 1:18f0113ac5a2 5 //PB 0 Front Left
Showboo 1:18f0113ac5a2 6 //PC 11 Front Right
Showboo 1:18f0113ac5a2 7 //PC 10 Side Right
Showboo 1:18f0113ac5a2 8 //Receiver:
Showboo 1:18f0113ac5a2 9 // Left Side PC_0
Showboo 1:18f0113ac5a2 10 // Front Left PC_1
Showboo 1:18f0113ac5a2 11 // Front Right PA_4
Showboo 1:18f0113ac5a2 12 // Side Left PA_0
Showboo 1:18f0113ac5a2 13 DigitalOut LeftIR(PB_7);
Showboo 1:18f0113ac5a2 14 DigitalOut FrontLeftIR(PB_0);
Showboo 1:18f0113ac5a2 15 DigitalOut FrontRightIR(PC_11);
Showboo 1:18f0113ac5a2 16 DigitalOut RightIR(PC_10);
Showboo 1:18f0113ac5a2 17 AnalogIn LeftReceiver(PC_0);
Showboo 1:18f0113ac5a2 18 AnalogIn FrontLeftReceiver(PC_1);
Showboo 1:18f0113ac5a2 19 AnalogIn FrontRightReceiver(PA_4);
Showboo 1:18f0113ac5a2 20 AnalogIn RightReceiver(PA_0);
Showboo 1:18f0113ac5a2 21 PwmOut lpwmf(PA_7);
Showboo 1:18f0113ac5a2 22 PwmOut lpwmb(PB_6);
Showboo 1:18f0113ac5a2 23 PwmOut rpwmf(PB_10);
Showboo 1:18f0113ac5a2 24 PwmOut rpwmb(PC_7);
Showboo 1:18f0113ac5a2 25 PinName rfront(PB_3);
Showboo 1:18f0113ac5a2 26 PinName rback(PA_15);
Showboo 1:18f0113ac5a2 27 PinName lfront(PA_1);
Showboo 1:18f0113ac5a2 28 PinName lback(PC_4);
Showboo 1:18f0113ac5a2 29 Serial pc(SERIAL_TX, SERIAL_RX);
Showboo 1:18f0113ac5a2 30 QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
Showboo 1:18f0113ac5a2 31 QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);
Showboo 1:18f0113ac5a2 32 const float rbase = 0.16f +.1f;
Showboo 1:18f0113ac5a2 33 const float lbase = 0.09f + .1f;
Showboo 1:18f0113ac5a2 34 const float WALL_IR_L = 0.9f;
Showboo 1:18f0113ac5a2 35 const float WALL_IR_R = 0.9f;
Showboo 1:18f0113ac5a2 36 const float WALL_IR_FL = 0.92f;
Showboo 1:18f0113ac5a2 37 const float WALL_IR_FR = 0.92f;
Showboo 1:18f0113ac5a2 38 Timer t_time;
Showboo 1:18f0113ac5a2 39 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?)
Showboo 1:18f0113ac5a2 40 pid(){
Showboo 1:18f0113ac5a2 41 integral = 0.0f;
Showboo 1:18f0113ac5a2 42 prev = 0.0f;
Showboo 1:18f0113ac5a2 43 kp = .01f; //the ks should be negative to counteract error
Showboo 1:18f0113ac5a2 44 ki = 0.0f;
Showboo 1:18f0113ac5a2 45 kd = 0.8f;
Showboo 1:18f0113ac5a2 46 }
Showboo 1:18f0113ac5a2 47 float integral;
Showboo 1:18f0113ac5a2 48 int prev;
Showboo 1:18f0113ac5a2 49 float kp; //the ks should be negative to counteract error
Showboo 1:18f0113ac5a2 50 float ki;
Showboo 1:18f0113ac5a2 51 float kd;
Showboo 1:18f0113ac5a2 52 };
Showboo 1:18f0113ac5a2 53
Showboo 1:18f0113ac5a2 54 void resetpid(struct pid *e) {
Showboo 1:18f0113ac5a2 55 e->integral = 0.0f;
Showboo 1:18f0113ac5a2 56 e->prev = 0.0f;
Showboo 1:18f0113ac5a2 57 }
Showboo 1:18f0113ac5a2 58
Showboo 1:18f0113ac5a2 59 float getFix(struct pid *e, float error, float time) {
Showboo 1:18f0113ac5a2 60 float d = (error - e->prev)/(float)time;
Showboo 1:18f0113ac5a2 61 e->integral += error * time;
Showboo 1:18f0113ac5a2 62 e->prev = error;
Showboo 1:18f0113ac5a2 63 return (float)(e->kp * error + e->ki * e->integral + e->kd * d);
Showboo 1:18f0113ac5a2 64 }
Showboo 1:18f0113ac5a2 65
Showboo 1:18f0113ac5a2 66 float constrain(float l_limit, float u_limit, float val){
Showboo 1:18f0113ac5a2 67 if (val < l_limit){
Showboo 1:18f0113ac5a2 68 return l_limit;
Showboo 1:18f0113ac5a2 69 }
Showboo 1:18f0113ac5a2 70 else if(val > u_limit){
Showboo 1:18f0113ac5a2 71 return u_limit;
Showboo 1:18f0113ac5a2 72 }
Showboo 1:18f0113ac5a2 73 return val;
Showboo 1:18f0113ac5a2 74 }
Showboo 1:18f0113ac5a2 75
Showboo 1:18f0113ac5a2 76 struct IRstruct{
Showboo 1:18f0113ac5a2 77 IRstruct(){
Showboo 1:18f0113ac5a2 78 statuscode = 0;
Showboo 1:18f0113ac5a2 79 }
Showboo 1:18f0113ac5a2 80 float leftIR, leftfrontIR, rightfrontIR, rightIR;
Showboo 1:18f0113ac5a2 81 unsigned int statuscode;
Showboo 1:18f0113ac5a2 82 };
Showboo 1:18f0113ac5a2 83
Showboo 1:18f0113ac5a2 84 unsigned int readIR(IRstruct& in_ir){
Showboo 1:18f0113ac5a2 85 in_ir.leftIR = LeftReceiver.read();
Showboo 1:18f0113ac5a2 86 in_ir.rightIR = RightReceiver.read();
Showboo 1:18f0113ac5a2 87 in_ir.leftfrontIR = FrontLeftReceiver.read();
Showboo 1:18f0113ac5a2 88 in_ir.rightfrontIR = FrontRightReceiver.read();
Showboo 1:18f0113ac5a2 89 pc.printf("LeftIR: %f RightIR: %f leftfronIR: %f rightfrontIR: %f \n", in_ir.leftIR, in_ir.rightIR, in_ir.leftfrontIR, in_ir.rightfrontIR);
Showboo 1:18f0113ac5a2 90
Showboo 1:18f0113ac5a2 91 //We then set the status codes to be analyzed later for what we should do
Showboo 1:18f0113ac5a2 92 in_ir.statuscode = (in_ir.statuscode|0x0);
Showboo 1:18f0113ac5a2 93
Showboo 1:18f0113ac5a2 94 //0000...FL,FR,L,R
Showboo 1:18f0113ac5a2 95 //Bottom is more of an abstraction, we probably don't even need it
Showboo 1:18f0113ac5a2 96 if(in_ir.leftIR > WALL_IR_L)
Showboo 1:18f0113ac5a2 97 in_ir.statuscode = (in_ir.statuscode|2); // 0...0 | 1 -> 0...1
Showboo 1:18f0113ac5a2 98 if(in_ir.rightIR > WALL_IR_R)
Showboo 1:18f0113ac5a2 99 in_ir.statuscode = in_ir.statuscode|1;
Showboo 1:18f0113ac5a2 100 if(in_ir.leftfrontIR > WALL_IR_FL)
Showboo 1:18f0113ac5a2 101 in_ir.statuscode = in_ir.statuscode|8;
Showboo 1:18f0113ac5a2 102 if(in_ir.rightfrontIR > WALL_IR_FR)
Showboo 1:18f0113ac5a2 103 in_ir.statuscode = in_ir.statuscode|4;
Showboo 1:18f0113ac5a2 104 return in_ir.statuscode;
Showboo 1:18f0113ac5a2 105 }
Showboo 1:18f0113ac5a2 106
Showboo 1:18f0113ac5a2 107 void turn_right(){
Showboo 1:18f0113ac5a2 108 float l_init = LeftEncoder.getPulses();
Showboo 1:18f0113ac5a2 109 while((LeftEncoder.getPulses() - l_init) < 10.0f){
Showboo 1:18f0113ac5a2 110 lpwmb = 0.0f;
Showboo 1:18f0113ac5a2 111 rpwmf = 0.0f;
Showboo 1:18f0113ac5a2 112 lpwmf = lbase;
Showboo 1:18f0113ac5a2 113 rpwmb = rbase;
Showboo 1:18f0113ac5a2 114 }
Showboo 1:18f0113ac5a2 115 }
Showboo 1:18f0113ac5a2 116
Showboo 1:18f0113ac5a2 117 void turn_left(){
Showboo 1:18f0113ac5a2 118 float r_init = RightEncoder.getPulses();
Showboo 1:18f0113ac5a2 119 while((RightEncoder.getPulses() - r_init) < 1000.0f){
Showboo 1:18f0113ac5a2 120 lpwmf = 0.0f;
Showboo 1:18f0113ac5a2 121 rpwmb = 0.0f;
Showboo 1:18f0113ac5a2 122 lpwmb = lbase;
Showboo 1:18f0113ac5a2 123 rpwmf = rbase;
Showboo 1:18f0113ac5a2 124 }
Showboo 1:18f0113ac5a2 125 }
Showboo 1:18f0113ac5a2 126
Showboo 1:18f0113ac5a2 127 void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){
Showboo 1:18f0113ac5a2 128 IRstruct ir_read;
Showboo 1:18f0113ac5a2 129 int status = readIR(ir_read);
Showboo 1:18f0113ac5a2 130 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
Showboo 1:18f0113ac5a2 131 turn_right();
Showboo 1:18f0113ac5a2 132 }
Showboo 1:18f0113ac5a2 133 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
Showboo 1:18f0113ac5a2 134 turn_left();
Showboo 1:18f0113ac5a2 135 }
Showboo 1:18f0113ac5a2 136 float error_ir = (ir_read.rightIR - ir_read.leftIR)/16.0f;
Showboo 1:18f0113ac5a2 137 if(error_ir < 0.0001f){
Showboo 1:18f0113ac5a2 138 resetpid(&IR_lman);
Showboo 1:18f0113ac5a2 139 }
Showboo 1:18f0113ac5a2 140 float adjust_l = getFix(&IR_lman, error_ir, dt);
Showboo 1:18f0113ac5a2 141 lspeed = constrain(0.0f, 0.4f, lbase - adjust_l);
Showboo 1:18f0113ac5a2 142 rspeed = constrain(0.0f, 0.4f, lbase);
Showboo 1:18f0113ac5a2 143 }
Showboo 1:18f0113ac5a2 144
Showboo 1:18f0113ac5a2 145 #endif