Anon Anon / Mbed 2 deprecated MM_rat_Assignment4

Test

Dependencies:   QEI mbed

header.h@1:fb18b43590e6, 2017-11-21 (annotated)

Committer:
Showboo
Date:
Tue Nov 21 23:06:09 2017 +0000
Revision:
1:fb18b43590e6
Parent:
0:f7fc09f9c7ce 
Update

Who changed what in which revision?

UserRevisionLine numberNew 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 1:fb18b43590e6 32 const float rbase = 0.12f;
Showboo 1:fb18b43590e6 33 const float lbase = 0.185f;
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);
Showboo 1:fb18b43590e6 110 while((LeftEncoder.getPulses() - l_init) < 3000){
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 1:fb18b43590e6 115 pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
Showboo 1:fb18b43590e6 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;
Showboo 1:fb18b43590e6 120 //lpwmf = 0.0f;
Showboo 1:fb18b43590e6 121 //rpwmb = 0.0f;
Showboo 0:f7fc09f9c7ce 122 }
Showboo 0:f7fc09f9c7ce 123
Showboo 0:f7fc09f9c7ce 124 void turn_left(){
Showboo 0:f7fc09f9c7ce 125 int r_init = RightEncoder.getPulses();
Showboo 1:fb18b43590e6 126 pc.printf("r_init %d \n", r_init);
Showboo 1:fb18b43590e6 127 while((RightEncoder.getPulses() - r_init) < 2000){
Showboo 0:f7fc09f9c7ce 128 lpwmf = 0.0f;
Showboo 0:f7fc09f9c7ce 129 rpwmb = 0.0f;
Showboo 0:f7fc09f9c7ce 130 lpwmb = lbase;
Showboo 0:f7fc09f9c7ce 131 rpwmf = rbase;
Showboo 1:fb18b43590e6 132 pc.printf("RightEncoder: %d \n", RightEncoder.getPulses());
Showboo 0:f7fc09f9c7ce 133 pc.printf("rEncoderDifference %f: \n", RightEncoder.getPulses()-r_init);
Showboo 0:f7fc09f9c7ce 134 }
Showboo 0:f7fc09f9c7ce 135 }
Showboo 0:f7fc09f9c7ce 136
Showboo 0:f7fc09f9c7ce 137 void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){
Showboo 0:f7fc09f9c7ce 138 IRstruct ir_read;
Showboo 0:f7fc09f9c7ce 139 int status = readIR(ir_read);
Showboo 0:f7fc09f9c7ce 140 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 141 turn_right();
Showboo 0:f7fc09f9c7ce 142 }
Showboo 0:f7fc09f9c7ce 143 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 144 turn_left();
Showboo 0:f7fc09f9c7ce 145 }
Showboo 1:fb18b43590e6 146 float error_ir = (ir_read.rightIR - ir_read.leftIR);
Showboo 1:fb18b43590e6 147 /*if(error_ir < 0.0001f){
Showboo 0:f7fc09f9c7ce 148 resetpid(&IR_lman);
Showboo 1:fb18b43590e6 149 }*/
Showboo 0:f7fc09f9c7ce 150 float adjust_l = getFix(&IR_lman, error_ir, dt);
Showboo 1:fb18b43590e6 151 lspeed = constrain(0.0f, 0.3f, lbase - adjust_l);
Showboo 1:fb18b43590e6 152 rspeed = constrain(0.0f, 0.3f, rbase);
Showboo 1:fb18b43590e6 153 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 154 }
Showboo 0:f7fc09f9c7ce 155
Showboo 0:f7fc09f9c7ce 156 #endif