Anon Anon / Mbed 2 deprecated Nucleo_pwm_example

here

Dependencies:   QEI mbed

header.h@3:81d97506bd46, 2017-11-20 (annotated)

Committer:
Showboo
Date:
Mon Nov 20 01:20:16 2017 +0000
Revision:
3:81d97506bd46
Parent:
1:18f0113ac5a2 
Assignment 4 updated, IR readings found. Left Motor not turning for some reason, maybe someone can find this out.

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