Diff: header.h

Revision:

9:97941581fe81

Parent:

8:22e399fe87a4

--- a/header.h	Tue Nov 28 21:45:07 2017 +0000
+++ b/header.h	Fri Dec 08 05:14:27 2017 +0000
@@ -15,6 +15,7 @@
 // Front Left PC_1
 // Front Right PA_4
 // Side Left PA_0
+DigitalIn PushButton(PC_13);
 DigitalOut LeftIR(PB_7);
 DigitalOut FrontLeftIR(PB_0);
 DigitalOut FrontRightIR(PC_11);
@@ -34,8 +35,8 @@
 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.15f; //.09
-const float lbase = 0.163f;  //.1
+const float rbase = 0.146f; //.09
+const float lbase = 0.17f;  //.1
 
 static int global_orientation = 0;
 struct pid;
@@ -52,7 +53,7 @@
         kd = 0.8f;
       }
   float integral;
-  int prev;
+  float prev;
   float kp; //the ks should be negative to counteract error
   float ki;
   float kd;
@@ -64,22 +65,43 @@
     lpwmf.period(0.01f);
     lpwmb.period(0.01f);
     lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea)
-    rpwmb=0;
+    rpwmb = 0;
     rpwmf.period(0.01f);
     rpwmb.period(0.01f);
-    rpwmb=0;
+    rpwmb = 0;
     rpwmf = 0;
-    lman.kp = .009f;
-    lman.ki = .0f;
-    lman.kd = .0f;
-    rman.kp = .5f;
-    rman.ki = .1f;
-    rman.kd = .4f;
+    lman.kp = .0021f;
+    lman.ki = .0000f;
+    lman.kd = -.0005f;
+    rman.kp = .0019f;
+    rman.ki = .0000f;
+    rman.kd = -.0005;
 }
 void resetpid(struct pid *e) {
   e->integral = 0.0f;
   e->prev = 0.0f;
 }
+
+inline void brake(){
+    lpwmf = 1.0f;
+    lpwmb = 1.0f;
+    rpwmf = 1.0f;
+    rpwmb = 1.0f;
+}
+
+void printIRSensors(float leftValue, float rightValue, float leftFrontValue, float rightFrontValue){
+    pc.printf("left %f \n", leftValue);
+    pc.printf("right %f \n\n", rightValue);
+    pc.printf("front left%f \n", leftFrontValue);
+    pc.printf("front right%f \n", rightFrontValue);
+    pc.printf(" \n");
+}
+
+void printEncoders(){
+    pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
+    pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses());
+}
+
 float getFix(struct pid *e, float error, float time) {
   float d = (error - e->prev)/time;
   e->integral += error * time;
@@ -108,38 +130,52 @@
 
 void turn_right(int num_times){ //Turns clockwise
     int l_init = LeftEncoder.getPulses();
-    pc.printf("l_init %d \n", l_init);
-    for(int i = 0; i < num_times; i++){
+    //pc.printf("l_init %d \n", l_init);
+    /*for(int i = 0; i < num_times; i++){
         if(global_orientation == 1){
             global_orientation = 7; //It's now east
         }    
         else
             global_orientation-=2;
-    }
-    while((LeftEncoder.getPulses() - l_init) < 166*num_times){
+    }*/
+    while((LeftEncoder.getPulses() - l_init) < 214*num_times){
         lpwmb = 0.0f;
         rpwmf = 0.0f;
         lpwmf = lbase;
         rpwmb = rbase;
+        //printEncoders();   
     }
     lpwmf = 0.0f;
     rpwmb = 0.0f;
     wait(1.0);
+    return;
 }
 
 void turn_left(int num_times){
     int r_init = RightEncoder.getPulses();
-    pc.printf("r_init %d \n", r_init);
-    while((RightEncoder.getPulses() - r_init) < 150*num_times){
+    //pc.printf("r_init %d \n", r_init);
+    while((RightEncoder.getPulses() - r_init) < 142*num_times){
         lpwmf = 0.0f;
         rpwmb = 0.0f;
         lpwmb = lbase;
         rpwmf = rbase;
+        //printEncoders();
     }
     lpwmb = 0.0f;
     rpwmf = 0.0f;
     wait(1.0);
 }
+
+void FlashFrontIRs(float& flIR, float& frIR){
+    using namespace constants;
+    FrontLeftIR = 1;
+    FrontRightIR = 1;
+    wait_ms(10);
+    flIR = FrontLeftReceiver;
+    frIR = FrontRightReceiver;
+    return;
+}
+
 void FlashIRs(float& leftIRBase, float& rightIRBase, float& leftFrontIRBase, float& rightFrontIRBase){
     using namespace constants;
     LeftIR = 1;
@@ -158,19 +194,6 @@
     FrontRightIR = 0;
 }
 
-void printIRSensors(float leftValue, float rightValue, float leftFrontValue, float rightFrontValue){
-    pc.printf("left %f \n", leftValue);
-    pc.printf("right %f \n\n", rightValue);
-    pc.printf("front left%f \n", leftFrontValue);
-    pc.printf("front right%f \n", rightFrontValue);
-    pc.printf(" \n");
-}
-
-void printEncoders(){
-    pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
-    pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses());
-}
-
 void usePIDBoth(){
      using namespace constants;
         while(LeftEncoder.getPulses() < 130 && RightEncoder.getPulses() < 130){       
@@ -211,8 +234,8 @@
            
            prevleftError = leftError;
            prevrightError = rightError;
-            lspeed = constrain(0.05f, 1.0f, lbase - adjust_l - frontP*totalFrontChange);
-            rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
+            lspeed = constrain(0.03f, 1.0f, lbase - adjust_l - frontP*totalFrontChange);
+            rspeed = constrain(0.03f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
            //Set the motors to the desired speed. 0 Values indicate to the H-Bridge to go forward
            lpwmb = 0; rpwmb = 0;
            lpwmf = lspeed; rpwmf = rspeed;
@@ -220,38 +243,37 @@
         }
 }
 
-void usePIDEncoder(){
+void usePIDEncoder(){ //We only use this when there is nothing to the sides of the walls
     using namespace constants;
-        while(LeftEncoder.getPulses() < 130 && RightEncoder.getPulses() < 130){ //Only traverse one cell at a time.
-            float dt = t_time.read();
-            float left_encoder = LeftEncoder.getPulses();
-            float right_encoder = RightEncoder.getPulses();
-            printf("Left Encoder: %.2f %%\n", left_encoder);
-            printf("Right Encoder: %.2f %%\n", right_encoder);
-            float error = left_encoder - right_encoder; //Can be pos or neg
-            if(error == 0){
+        //brake();
+        //wait(0.1);
+        while(LeftEncoder.getPulses() < 435 && RightEncoder.getPulses() < 435){ //Only traverse one cell at a time.
+            float dt = 1.0f;
+            float FLIR, FRIR;
+            FlashFrontIRs(FLIR, FRIR);
+            /*if(FLIR > 0.15f || FRIR > 0.15f){
+                brake();
+                return;
+            }*/
+            float left_encoder = (float)LeftEncoder.getPulses();
+            float right_encoder = (float)RightEncoder.getPulses();
+            float error = (float)left_encoder - (float)right_encoder; //Can be pos or neg
+            if(error == 0.0f){
                 resetpid(&lman);
                 resetpid(&rman);
             }
             float adjust_r = getFix(&rman, error, dt);
-            lpwmf = lbase;
+            float adjust_l = getFix(&lman, error, dt);
+            lpwmf = constrain(0.06f, 0.45f, lbase-adjust_l);
             lpwmb = 0.0f;
-            rpwmf = constrain(0.04f, 0.5f, rbase-adjust_r);
+            rpwmf = constrain(0.06f, 0.45f, rbase+adjust_r);
             rpwmb = 0.0f;
-            if(RightEncoder.getPulses() > cell_length && LeftEncoder.getPulses() > cell_length){ //We've traversed a cell, update cell count;
-                RightEncoder.reset(); 
-                LeftEncoder.reset();   
-            }
-        if(leftValue> 0.6f && leftFrontValue > 0.6f)//Maybe we should brake in here?
-        {
-            return;
-        }
-        else if(rightFrontValue > 0.6f && rightValue > 0.6f)
-        {
-            return;
-        }   
-       t_time.reset();
+            printf("Left Encoder: %.2f Right Encoder: %.2f DT: %.2f Error: %.2f adjust_l: %.2f adjust_r: %.2f \n", left_encoder, right_encoder, dt, error, adjust_l, adjust_r);
+            t_time.reset();
     }
-  }
+    RightEncoder.reset();
+    LeftEncoder.reset();
+    return;
+}
     
 #endif
\ No newline at end of file