Diff: header.h

Revision:

1:fb18b43590e6

Parent:

0:f7fc09f9c7ce

--- a/header.h	Mon Nov 20 01:49:19 2017 +0000
+++ b/header.h	Tue Nov 21 23:06:09 2017 +0000
@@ -29,12 +29,12 @@
 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;    
+const float rbase = 0.12f;
+const float lbase = 0.185f;
+const float WALL_IR_L = 0.74f;
+const float WALL_IR_R = 0.74f;
+const float WALL_IR_FL = 0.74f;
+const float WALL_IR_FR = 0.74f;    
 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(){
@@ -55,12 +55,12 @@
   e->integral = 0.0f;
   e->prev = 0.0f;
 }
-
 float getFix(struct pid *e, float error, float time) {
-  float d = (error - e->prev)/(float)time;
+  float d = (error - e->prev)/time;
   e->integral += error * time;
   e->prev = error;
-  return (float)(e->kp * error + e->ki * e->integral + e->kd * d);
+  float temp = (e->kp * error + e->ki * e->integral + e->kd * d);
+  return temp;
 }
 
 float constrain(float l_limit, float u_limit, float val){
@@ -107,23 +107,29 @@
 void turn_right(){
     int l_init = LeftEncoder.getPulses();
     pc.printf("l_init %d \n", l_init);
-    while((LeftEncoder.getPulses() - l_init) < 1000){
+    while((LeftEncoder.getPulses() - l_init) < 3000){
         lpwmb = 0.0f;
         rpwmf = 0.0f;
         lpwmf = lbase;
         rpwmb = rbase;
+        pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
+        pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses() - l_init);
     }
-    pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses() - l_init);
+        //lpwmb = 0.0f;
+        //rpwmf = 0.0f;
+        //lpwmf = 0.0f;
+        //rpwmb = 0.0f;
 }
 
 void turn_left(){
     int r_init = RightEncoder.getPulses();
-    pc.printf("r_init %f \n", r_init);
-    while((RightEncoder.getPulses() - r_init) < 1000){
+    pc.printf("r_init %d \n", r_init);
+    while((RightEncoder.getPulses() - r_init) < 2000){
         lpwmf = 0.0f;
         rpwmb = 0.0f;
         lpwmb = lbase;
         rpwmf = rbase;
+        pc.printf("RightEncoder: %d \n", RightEncoder.getPulses());
         pc.printf("rEncoderDifference %f: \n", RightEncoder.getPulses()-r_init);
     }
 }
@@ -137,14 +143,14 @@
     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){    
+    float error_ir = (ir_read.rightIR - ir_read.leftIR);
+    /*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);
+    lspeed = constrain(0.0f, 0.3f, lbase - adjust_l);
+    rspeed = constrain(0.0f, 0.3f, rbase);
+    pc.printf("adjust_l: %f lspeed: %f rspeed: %f error_ir: %f dt: %f \n", adjust_l, lspeed, rspeed, error_ir, dt);
 }
     
 #endif
\ No newline at end of file