Diff: header.h

Revision:

0:f7fc09f9c7ce

Child:

1:fb18b43590e6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/header.h	Mon Nov 20 01:49:19 2017 +0000
@@ -0,0 +1,150 @@
+#ifndef Header_H
+#define Header_H
+//IR:
+//PB 7 Left Side
+//PB 0 Front Left
+//PC 11 Front Right 
+//PC 10 Side Right
+//Receiver:
+// Left Side PC_0
+// Front Left PC_1
+// Front Right PA_4
+// Side Left PA_0
+DigitalOut LeftIR(PB_7);
+DigitalOut FrontLeftIR(PB_0);
+DigitalOut FrontRightIR(PC_11);
+DigitalOut RightIR(PC_10);
+AnalogIn LeftReceiver(PC_0);
+AnalogIn FrontLeftReceiver(PC_1);
+AnalogIn FrontRightReceiver(PA_4);
+AnalogIn RightReceiver(PA_0);
+PwmOut lpwmf(PA_7);
+PwmOut lpwmb(PB_6);
+PwmOut rpwmf(PB_10);
+PwmOut rpwmb(PC_7);
+PinName rfront(PB_3);
+PinName rback(PA_15);
+PinName lfront(PA_1);
+PinName lback(PC_4);
+Serial pc(SERIAL_TX, SERIAL_RX); 
+QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
+QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);       
+const float rbase = 0.16f +.1f;
+const float lbase = 0.09f + .1f;
+const float WALL_IR_L = 0.9f;
+const float WALL_IR_R = 0.9f;
+const float WALL_IR_FL = 0.92f;
+const float WALL_IR_FR = 0.92f;    
+Timer t_time;
+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?)
+  pid(){
+        integral = 0.0f;
+        prev = 0.0f;
+        kp = .01f; //the ks should be negative to counteract error
+        ki = 0.0f;
+        kd = 0.8f;
+      }
+  float integral;
+  int prev;
+  float kp; //the ks should be negative to counteract error
+  float ki;
+  float kd;
+};
+
+void resetpid(struct pid *e) {
+  e->integral = 0.0f;
+  e->prev = 0.0f;
+}
+
+float getFix(struct pid *e, float error, float time) {
+  float d = (error - e->prev)/(float)time;
+  e->integral += error * time;
+  e->prev = error;
+  return (float)(e->kp * error + e->ki * e->integral + e->kd * d);
+}
+
+float constrain(float l_limit, float u_limit, float val){
+    if (val < l_limit){
+        return l_limit;
+        }
+    else if(val > u_limit){
+        return u_limit;
+        }    
+    return val;    
+}
+
+struct IRstruct{
+    IRstruct(){
+        statuscode = 0;
+    }
+    float leftIR, leftfrontIR, rightfrontIR, rightIR;
+    unsigned int statuscode;
+};
+
+unsigned int readIR(IRstruct& in_ir){
+    in_ir.leftIR = LeftReceiver.read();
+    in_ir.rightIR = RightReceiver.read();
+    in_ir.leftfrontIR = FrontLeftReceiver.read();
+    in_ir.rightfrontIR = FrontRightReceiver.read();
+    //pc.printf("LeftIR: %f RightIR: %f leftfronIR: %f rightfrontIR: %f \n", in_ir.leftIR, in_ir.rightIR, in_ir.leftfrontIR, in_ir.rightfrontIR); 
+    
+    //We then set the status codes to be analyzed later for what we should do
+    in_ir.statuscode = (in_ir.statuscode|0x0);
+    
+    //0000...FL,FR,L,R
+    //Bottom is more of an abstraction, we probably don't even need it
+    if(in_ir.leftIR > WALL_IR_L)
+        in_ir.statuscode = (in_ir.statuscode|2); // 0...0 | 1 -> 0...1
+    if(in_ir.rightIR > WALL_IR_R)
+        in_ir.statuscode = in_ir.statuscode|1;
+    if(in_ir.leftfrontIR > WALL_IR_FL)
+        in_ir.statuscode = in_ir.statuscode|8;
+    if(in_ir.rightfrontIR > WALL_IR_FR)
+        in_ir.statuscode = in_ir.statuscode|4;
+    return in_ir.statuscode;
+}
+
+void turn_right(){
+    int l_init = LeftEncoder.getPulses();
+    pc.printf("l_init %d \n", l_init);
+    while((LeftEncoder.getPulses() - l_init) < 1000){
+        lpwmb = 0.0f;
+        rpwmf = 0.0f;
+        lpwmf = lbase;
+        rpwmb = rbase;
+    }
+    pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses() - l_init);
+}
+
+void turn_left(){
+    int r_init = RightEncoder.getPulses();
+    pc.printf("r_init %f \n", r_init);
+    while((RightEncoder.getPulses() - r_init) < 1000){
+        lpwmf = 0.0f;
+        rpwmb = 0.0f;
+        lpwmb = lbase;
+        rpwmf = rbase;
+        pc.printf("rEncoderDifference %f: \n", RightEncoder.getPulses()-r_init);
+    }
+}
+
+void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){
+    IRstruct ir_read;
+    int status = readIR(ir_read); 
+    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
+        turn_right();
+    }
+    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
+        turn_left();
+    }
+    float error_ir = (ir_read.rightIR - ir_read.leftIR)/16.0f;
+    if(error_ir < 0.0001f){    
+        resetpid(&IR_lman);
+    }
+    float adjust_l = getFix(&IR_lman, error_ir, dt);
+    lspeed = constrain(0.0f, 0.4f, lbase - adjust_l);
+    rspeed = constrain(0.0f, 0.4f, rbase);
+    pc.printf("adjust_l: %f lspeed: %f rspeed: %f error_ir: %f \n", adjust_l, lspeed, rspeed, error_ir);
+}
+    
+#endif
\ No newline at end of file