Evan Brown
/
MM_rat_Assignment4-newwest
p
Fork of MM_rat_Assignment4 by
header.h@7:7cdb0381e1b8, 2017-11-28 (annotated)
- Committer:
- evenbrownie
- Date:
- Tue Nov 28 19:58:02 2017 +0000
- Revision:
- 7:7cdb0381e1b8
- Parent:
- 6:ce498700a28c
PID with front stopping capability
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Showboo | 0:f7fc09f9c7ce | 1 | #ifndef Header_H |
Showboo | 0:f7fc09f9c7ce | 2 | #define Header_H |
Showboo | 0:f7fc09f9c7ce | 3 | //IR: |
Showboo | 0:f7fc09f9c7ce | 4 | //PB 7 Left Side |
Showboo | 0:f7fc09f9c7ce | 5 | //PB 0 Front Left |
Showboo | 0:f7fc09f9c7ce | 6 | //PC 11 Front Right |
Showboo | 0:f7fc09f9c7ce | 7 | //PC 10 Side Right |
Showboo | 0:f7fc09f9c7ce | 8 | //Receiver: |
Showboo | 0:f7fc09f9c7ce | 9 | // Left Side PC_0 |
Showboo | 0:f7fc09f9c7ce | 10 | // Front Left PC_1 |
Showboo | 0:f7fc09f9c7ce | 11 | // Front Right PA_4 |
Showboo | 0:f7fc09f9c7ce | 12 | // Side Left PA_0 |
Showboo | 0:f7fc09f9c7ce | 13 | DigitalOut LeftIR(PB_7); |
Showboo | 0:f7fc09f9c7ce | 14 | DigitalOut FrontLeftIR(PB_0); |
Showboo | 0:f7fc09f9c7ce | 15 | DigitalOut FrontRightIR(PC_11); |
Showboo | 0:f7fc09f9c7ce | 16 | DigitalOut RightIR(PC_10); |
Showboo | 0:f7fc09f9c7ce | 17 | AnalogIn LeftReceiver(PC_0); |
Showboo | 0:f7fc09f9c7ce | 18 | AnalogIn FrontLeftReceiver(PC_1); |
Showboo | 0:f7fc09f9c7ce | 19 | AnalogIn FrontRightReceiver(PA_4); |
Showboo | 0:f7fc09f9c7ce | 20 | AnalogIn RightReceiver(PA_0); |
evenbrownie | 6:ce498700a28c | 21 | PwmOut lpwmf(PB_6); |
evenbrownie | 6:ce498700a28c | 22 | PwmOut lpwmb(PA_7); |
Showboo | 0:f7fc09f9c7ce | 23 | PwmOut rpwmf(PB_10); |
Showboo | 0:f7fc09f9c7ce | 24 | PwmOut rpwmb(PC_7); |
Showboo | 0:f7fc09f9c7ce | 25 | PinName rfront(PB_3); |
Showboo | 0:f7fc09f9c7ce | 26 | PinName rback(PA_15); |
Showboo | 0:f7fc09f9c7ce | 27 | PinName lfront(PA_1); |
Showboo | 0:f7fc09f9c7ce | 28 | PinName lback(PC_4); |
Showboo | 0:f7fc09f9c7ce | 29 | Serial pc(SERIAL_TX, SERIAL_RX); |
Showboo | 0:f7fc09f9c7ce | 30 | QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING); |
Showboo | 0:f7fc09f9c7ce | 31 | QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING); |
evenbrownie | 7:7cdb0381e1b8 | 32 | const float rbase = 0.15f; //.09 |
evenbrownie | 7:7cdb0381e1b8 | 33 | const float lbase = 0.163f; //.1 |
Showboo | 1:fb18b43590e6 | 34 | const float WALL_IR_L = 0.74f; |
Showboo | 1:fb18b43590e6 | 35 | const float WALL_IR_R = 0.74f; |
Showboo | 1:fb18b43590e6 | 36 | const float WALL_IR_FL = 0.74f; |
Showboo | 1:fb18b43590e6 | 37 | const float WALL_IR_FR = 0.74f; |
Showboo | 0:f7fc09f9c7ce | 38 | Timer t_time; |
Showboo | 0:f7fc09f9c7ce | 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 | 0:f7fc09f9c7ce | 40 | pid(){ |
Showboo | 0:f7fc09f9c7ce | 41 | integral = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 42 | prev = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 43 | kp = .01f; //the ks should be negative to counteract error |
Showboo | 0:f7fc09f9c7ce | 44 | ki = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 45 | kd = 0.8f; |
Showboo | 0:f7fc09f9c7ce | 46 | } |
Showboo | 0:f7fc09f9c7ce | 47 | float integral; |
Showboo | 0:f7fc09f9c7ce | 48 | int prev; |
Showboo | 0:f7fc09f9c7ce | 49 | float kp; //the ks should be negative to counteract error |
Showboo | 0:f7fc09f9c7ce | 50 | float ki; |
Showboo | 0:f7fc09f9c7ce | 51 | float kd; |
Showboo | 0:f7fc09f9c7ce | 52 | }; |
Showboo | 0:f7fc09f9c7ce | 53 | |
Showboo | 0:f7fc09f9c7ce | 54 | void resetpid(struct pid *e) { |
Showboo | 0:f7fc09f9c7ce | 55 | e->integral = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 56 | e->prev = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 57 | } |
Showboo | 0:f7fc09f9c7ce | 58 | float getFix(struct pid *e, float error, float time) { |
Showboo | 1:fb18b43590e6 | 59 | float d = (error - e->prev)/time; |
Showboo | 0:f7fc09f9c7ce | 60 | e->integral += error * time; |
Showboo | 0:f7fc09f9c7ce | 61 | e->prev = error; |
Showboo | 1:fb18b43590e6 | 62 | float temp = (e->kp * error + e->ki * e->integral + e->kd * d); |
Showboo | 1:fb18b43590e6 | 63 | return temp; |
Showboo | 0:f7fc09f9c7ce | 64 | } |
Showboo | 0:f7fc09f9c7ce | 65 | |
Showboo | 0:f7fc09f9c7ce | 66 | float constrain(float l_limit, float u_limit, float val){ |
Showboo | 0:f7fc09f9c7ce | 67 | if (val < l_limit){ |
Showboo | 0:f7fc09f9c7ce | 68 | return l_limit; |
Showboo | 0:f7fc09f9c7ce | 69 | } |
Showboo | 0:f7fc09f9c7ce | 70 | else if(val > u_limit){ |
Showboo | 0:f7fc09f9c7ce | 71 | return u_limit; |
Showboo | 0:f7fc09f9c7ce | 72 | } |
Showboo | 0:f7fc09f9c7ce | 73 | return val; |
Showboo | 0:f7fc09f9c7ce | 74 | } |
Showboo | 0:f7fc09f9c7ce | 75 | |
Showboo | 0:f7fc09f9c7ce | 76 | struct IRstruct{ |
Showboo | 0:f7fc09f9c7ce | 77 | IRstruct(){ |
Showboo | 0:f7fc09f9c7ce | 78 | statuscode = 0; |
Showboo | 0:f7fc09f9c7ce | 79 | } |
Showboo | 0:f7fc09f9c7ce | 80 | float leftIR, leftfrontIR, rightfrontIR, rightIR; |
Showboo | 0:f7fc09f9c7ce | 81 | unsigned int statuscode; |
Showboo | 0:f7fc09f9c7ce | 82 | }; |
Showboo | 0:f7fc09f9c7ce | 83 | |
Showboo | 0:f7fc09f9c7ce | 84 | unsigned int readIR(IRstruct& in_ir){ |
Showboo | 0:f7fc09f9c7ce | 85 | in_ir.leftIR = LeftReceiver.read(); |
Showboo | 0:f7fc09f9c7ce | 86 | in_ir.rightIR = RightReceiver.read(); |
Showboo | 0:f7fc09f9c7ce | 87 | in_ir.leftfrontIR = FrontLeftReceiver.read(); |
Showboo | 0:f7fc09f9c7ce | 88 | in_ir.rightfrontIR = FrontRightReceiver.read(); |
Showboo | 0:f7fc09f9c7ce | 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 | 0:f7fc09f9c7ce | 90 | |
Showboo | 0:f7fc09f9c7ce | 91 | //We then set the status codes to be analyzed later for what we should do |
Showboo | 0:f7fc09f9c7ce | 92 | in_ir.statuscode = (in_ir.statuscode|0x0); |
Showboo | 0:f7fc09f9c7ce | 93 | |
Showboo | 0:f7fc09f9c7ce | 94 | //0000...FL,FR,L,R |
Showboo | 0:f7fc09f9c7ce | 95 | //Bottom is more of an abstraction, we probably don't even need it |
Showboo | 0:f7fc09f9c7ce | 96 | if(in_ir.leftIR > WALL_IR_L) |
Showboo | 0:f7fc09f9c7ce | 97 | in_ir.statuscode = (in_ir.statuscode|2); // 0...0 | 1 -> 0...1 |
Showboo | 0:f7fc09f9c7ce | 98 | if(in_ir.rightIR > WALL_IR_R) |
Showboo | 0:f7fc09f9c7ce | 99 | in_ir.statuscode = in_ir.statuscode|1; |
Showboo | 0:f7fc09f9c7ce | 100 | if(in_ir.leftfrontIR > WALL_IR_FL) |
Showboo | 0:f7fc09f9c7ce | 101 | in_ir.statuscode = in_ir.statuscode|8; |
Showboo | 0:f7fc09f9c7ce | 102 | if(in_ir.rightfrontIR > WALL_IR_FR) |
Showboo | 0:f7fc09f9c7ce | 103 | in_ir.statuscode = in_ir.statuscode|4; |
Showboo | 0:f7fc09f9c7ce | 104 | return in_ir.statuscode; |
Showboo | 0:f7fc09f9c7ce | 105 | } |
Showboo | 0:f7fc09f9c7ce | 106 | |
Showboo | 0:f7fc09f9c7ce | 107 | void turn_right(){ |
Showboo | 0:f7fc09f9c7ce | 108 | int l_init = LeftEncoder.getPulses(); |
Showboo | 0:f7fc09f9c7ce | 109 | pc.printf("l_init %d \n", l_init); |
evenbrownie | 3:4e34437daef4 | 110 | while((LeftEncoder.getPulses() - l_init) < 166){ |
Showboo | 0:f7fc09f9c7ce | 111 | lpwmb = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 112 | rpwmf = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 113 | lpwmf = lbase; |
Showboo | 0:f7fc09f9c7ce | 114 | rpwmb = rbase; |
evenbrownie | 3:4e34437daef4 | 115 | //pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses()); |
evenbrownie | 3:4e34437daef4 | 116 | // pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses() - l_init); |
Showboo | 0:f7fc09f9c7ce | 117 | } |
Showboo | 1:fb18b43590e6 | 118 | //lpwmb = 0.0f; |
Showboo | 1:fb18b43590e6 | 119 | //rpwmf = 0.0f; |
evenbrownie | 3:4e34437daef4 | 120 | lpwmf = 0.0f; |
evenbrownie | 3:4e34437daef4 | 121 | rpwmb = 0.0f; |
evenbrownie | 3:4e34437daef4 | 122 | wait(1.0); |
Showboo | 0:f7fc09f9c7ce | 123 | } |
Showboo | 0:f7fc09f9c7ce | 124 | |
Showboo | 0:f7fc09f9c7ce | 125 | void turn_left(){ |
Showboo | 0:f7fc09f9c7ce | 126 | int r_init = RightEncoder.getPulses(); |
Showboo | 1:fb18b43590e6 | 127 | pc.printf("r_init %d \n", r_init); |
evenbrownie | 3:4e34437daef4 | 128 | while((RightEncoder.getPulses() - r_init) < 150){ |
Showboo | 0:f7fc09f9c7ce | 129 | lpwmf = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 130 | rpwmb = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 131 | lpwmb = lbase; |
Showboo | 0:f7fc09f9c7ce | 132 | rpwmf = rbase; |
evenbrownie | 3:4e34437daef4 | 133 | // pc.printf("RightEncoder: %d \n", RightEncoder.getPulses()); |
evenbrownie | 3:4e34437daef4 | 134 | //pc.printf("rEncoderDifference %f: \n", RightEncoder.getPulses()-r_init); |
Showboo | 0:f7fc09f9c7ce | 135 | } |
evenbrownie | 3:4e34437daef4 | 136 | lpwmb = 0.0f; |
evenbrownie | 3:4e34437daef4 | 137 | rpwmf = 0.0f; |
evenbrownie | 3:4e34437daef4 | 138 | wait( 1.0); |
Showboo | 0:f7fc09f9c7ce | 139 | } |
Showboo | 0:f7fc09f9c7ce | 140 | |
Showboo | 0:f7fc09f9c7ce | 141 | void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){ |
Showboo | 0:f7fc09f9c7ce | 142 | IRstruct ir_read; |
Showboo | 0:f7fc09f9c7ce | 143 | int status = readIR(ir_read); |
Showboo | 0:f7fc09f9c7ce | 144 | if(ir_read.leftIR > WALL_IR_L && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR && ir_read.rightIR < WALL_IR_R){ //High in front and left -> Turn right |
Showboo | 0:f7fc09f9c7ce | 145 | turn_right(); |
Showboo | 0:f7fc09f9c7ce | 146 | } |
Showboo | 0:f7fc09f9c7ce | 147 | 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 | 0:f7fc09f9c7ce | 148 | turn_left(); |
Showboo | 0:f7fc09f9c7ce | 149 | } |
Showboo | 1:fb18b43590e6 | 150 | float error_ir = (ir_read.rightIR - ir_read.leftIR); |
Showboo | 1:fb18b43590e6 | 151 | /*if(error_ir < 0.0001f){ |
Showboo | 0:f7fc09f9c7ce | 152 | resetpid(&IR_lman); |
Showboo | 1:fb18b43590e6 | 153 | }*/ |
Showboo | 0:f7fc09f9c7ce | 154 | float adjust_l = getFix(&IR_lman, error_ir, dt); |
Showboo | 1:fb18b43590e6 | 155 | lspeed = constrain(0.0f, 0.3f, lbase - adjust_l); |
Showboo | 1:fb18b43590e6 | 156 | rspeed = constrain(0.0f, 0.3f, rbase); |
Showboo | 1:fb18b43590e6 | 157 | pc.printf("adjust_l: %f lspeed: %f rspeed: %f error_ir: %f dt: %f \n", adjust_l, lspeed, rspeed, error_ir, dt); |
Showboo | 0:f7fc09f9c7ce | 158 | } |
Showboo | 0:f7fc09f9c7ce | 159 | |
Showboo | 0:f7fc09f9c7ce | 160 | #endif |