Revision 1:fb18b43590e6, committed 2017-11-21

Comitter:

Showboo

Date:

Tue Nov 21 23:06:09 2017 +0000

Parent:

0:f7fc09f9c7ce

Commit message:

Update

Changed in this revision

--- 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

--- a/main.cpp	Mon Nov 20 01:49:19 2017 +0000
+++ b/main.cpp	Tue Nov 21 23:06:09 2017 +0000
@@ -3,15 +3,18 @@
 #include "header.h"
 
 inline void pulse_ir(int in){
+    for(int i = 0; i < 4; i++){
         LeftIR = in;
+        
         FrontLeftIR = in;
         FrontRightIR = in;
         RightIR = in;
+    }
 }
 
 int main() {
     pid lman, rman;
-    lman.kp = .004f;
+    lman.kp = .009f;
     lman.ki = .0f;
     lman.kd = .0f;
     rman.kp = .5f;
@@ -19,11 +22,16 @@
     rman.kd = .4f;
     
     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;
     rpwmf.period(0.01f);
+    rpwmb.period(0.01f);
+    rpwmb=0;
     rpwmf = 0;
+    
     pid ir_lman, ir_rman;
-    ir_lman.kp = .004f;
+    ir_lman.kp = .0001f;
     ir_lman.ki = .0f;
     ir_lman.kd = .0f;
     ir_rman.kp = .5f;
@@ -32,12 +40,24 @@
     t_time.start();
     while(1){       
        float dt = t_time.read();
-       float lspeed; float rspeed;
-       pulse_ir(1);
+       float lspeed = 0; float rspeed = 0;
+            LeftIR = 1;
+            wait(.01f);
+            FrontLeftIR=1;
+            wait(.01f);
+            FrontRightIR=1;
+            wait(.01f);
+            RightIR=1;
        ProcessIR(dt, ir_lman, lspeed, rspeed);
        lpwmb = 0; rpwmb = 0;
-       //lpwmf = lspeed; rpwmf = rspeed;
-       pulse_ir(0);
+       lpwmf = lspeed; rpwmf = rspeed;
+            LeftIR = 0;
+            wait(.01f);
+            FrontLeftIR=0;
+            wait(.01f);
+            FrontRightIR=0;
+            wait(.01f);
+            RightIR=0;
        t_time.reset();
     }
 }