Evan Brown
/
MM_rat_Assignment4-newwest
p
Fork of MM_rat_Assignment4 by
header.h@0:f7fc09f9c7ce, 2017-11-20 (annotated)
- Committer:
- Showboo
- Date:
- Mon Nov 20 01:49:19 2017 +0000
- Revision:
- 0:f7fc09f9c7ce
- Child:
- 1:fb18b43590e6
Had to change workspaces
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); |
Showboo | 0:f7fc09f9c7ce | 21 | PwmOut lpwmf(PA_7); |
Showboo | 0:f7fc09f9c7ce | 22 | PwmOut lpwmb(PB_6); |
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); |
Showboo | 0:f7fc09f9c7ce | 32 | const float rbase = 0.16f +.1f; |
Showboo | 0:f7fc09f9c7ce | 33 | const float lbase = 0.09f + .1f; |
Showboo | 0:f7fc09f9c7ce | 34 | const float WALL_IR_L = 0.9f; |
Showboo | 0:f7fc09f9c7ce | 35 | const float WALL_IR_R = 0.9f; |
Showboo | 0:f7fc09f9c7ce | 36 | const float WALL_IR_FL = 0.92f; |
Showboo | 0:f7fc09f9c7ce | 37 | const float WALL_IR_FR = 0.92f; |
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 | |
Showboo | 0:f7fc09f9c7ce | 59 | float getFix(struct pid *e, float error, float time) { |
Showboo | 0:f7fc09f9c7ce | 60 | float d = (error - e->prev)/(float)time; |
Showboo | 0:f7fc09f9c7ce | 61 | e->integral += error * time; |
Showboo | 0:f7fc09f9c7ce | 62 | e->prev = error; |
Showboo | 0:f7fc09f9c7ce | 63 | return (float)(e->kp * error + e->ki * e->integral + e->kd * d); |
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); |
Showboo | 0:f7fc09f9c7ce | 110 | while((LeftEncoder.getPulses() - l_init) < 1000){ |
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; |
Showboo | 0:f7fc09f9c7ce | 115 | } |
Showboo | 0:f7fc09f9c7ce | 116 | pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses() - l_init); |
Showboo | 0:f7fc09f9c7ce | 117 | } |
Showboo | 0:f7fc09f9c7ce | 118 | |
Showboo | 0:f7fc09f9c7ce | 119 | void turn_left(){ |
Showboo | 0:f7fc09f9c7ce | 120 | int r_init = RightEncoder.getPulses(); |
Showboo | 0:f7fc09f9c7ce | 121 | pc.printf("r_init %f \n", r_init); |
Showboo | 0:f7fc09f9c7ce | 122 | while((RightEncoder.getPulses() - r_init) < 1000){ |
Showboo | 0:f7fc09f9c7ce | 123 | lpwmf = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 124 | rpwmb = 0.0f; |
Showboo | 0:f7fc09f9c7ce | 125 | lpwmb = lbase; |
Showboo | 0:f7fc09f9c7ce | 126 | rpwmf = rbase; |
Showboo | 0:f7fc09f9c7ce | 127 | pc.printf("rEncoderDifference %f: \n", RightEncoder.getPulses()-r_init); |
Showboo | 0:f7fc09f9c7ce | 128 | } |
Showboo | 0:f7fc09f9c7ce | 129 | } |
Showboo | 0:f7fc09f9c7ce | 130 | |
Showboo | 0:f7fc09f9c7ce | 131 | void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){ |
Showboo | 0:f7fc09f9c7ce | 132 | IRstruct ir_read; |
Showboo | 0:f7fc09f9c7ce | 133 | int status = readIR(ir_read); |
Showboo | 0:f7fc09f9c7ce | 134 | 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 | 135 | turn_right(); |
Showboo | 0:f7fc09f9c7ce | 136 | } |
Showboo | 0:f7fc09f9c7ce | 137 | 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 | 138 | turn_left(); |
Showboo | 0:f7fc09f9c7ce | 139 | } |
Showboo | 0:f7fc09f9c7ce | 140 | float error_ir = (ir_read.rightIR - ir_read.leftIR)/16.0f; |
Showboo | 0:f7fc09f9c7ce | 141 | if(error_ir < 0.0001f){ |
Showboo | 0:f7fc09f9c7ce | 142 | resetpid(&IR_lman); |
Showboo | 0:f7fc09f9c7ce | 143 | } |
Showboo | 0:f7fc09f9c7ce | 144 | float adjust_l = getFix(&IR_lman, error_ir, dt); |
Showboo | 0:f7fc09f9c7ce | 145 | lspeed = constrain(0.0f, 0.4f, lbase - adjust_l); |
Showboo | 0:f7fc09f9c7ce | 146 | rspeed = constrain(0.0f, 0.4f, rbase); |
Showboo | 0:f7fc09f9c7ce | 147 | pc.printf("adjust_l: %f lspeed: %f rspeed: %f error_ir: %f \n", adjust_l, lspeed, rspeed, error_ir); |
Showboo | 0:f7fc09f9c7ce | 148 | } |
Showboo | 0:f7fc09f9c7ce | 149 | |
Showboo | 0:f7fc09f9c7ce | 150 | #endif |