Diff: PID_control.cpp

Revision:

5:a49a77ddf4e3

Parent:

4:90303483fd5f

Child:

6:71829ae2ee07

--- a/PID_control.cpp	Fri Nov 10 23:05:18 2017 +0000
+++ b/PID_control.cpp	Sat Nov 11 01:02:29 2017 +0000
@@ -4,7 +4,7 @@
 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
 double Kp = .005;
-double Ki = 0;//.0000001;
+double Ki = 0.0000001;
 double Kd = 0.001;
 PwmOut m_Right_F(PB_10);
 PwmOut m_Right_B(PC_7);
@@ -23,7 +23,6 @@
 double P_controller(int error)
 {
     double correction = (Kp*error);
-    //pc.printf("correction is: %d\n", correction);
     return correction;
 }
 
@@ -32,7 +31,6 @@
     integrator += error;
     double correction = Ki*integrator;
     integrator /= decayFactor;
-
     return correction;
 }
 
@@ -42,7 +40,11 @@
     int dt = timer.read_us();
     timer.reset();
     prevError = error;
-    double correction = Kd*dError/dt;
+    double correction;
+    if (dt==0)
+        correction=0;
+    else
+        correction = Kd*dError/dt;
     return correction;
 }
 
@@ -57,27 +59,35 @@
     // if (ex < 0)
     //    ex = -ex;
     C_speed = P_controller(Error) + I_controller(Error) + ex;
+    if (C_speed < 0)
+        C_speed = C_speed*-1;
 }
 
 void forward()
 {
     double f1_speed = i_speed + C_speed;
     double f2_speed = i_speed - C_speed;
-    
-    if (f2_speed > f1_speed)    //if f2>f1, KILL
-        {
-            while(1) {
-                m_Left_F.write(0);
-                m_Right_F.write(0);
-                }    
-        }
-        
-    if(f1_speed >= 0.7) {
+
+    /*pc.printf("C: %f", C_speed);
+    if (C_speed < 0)
+        pc.printf("-");
+    if (C_speed > 0)
+        pc.printf("+");
+        */
+
+    if(f1_speed >= 0.7) {   //upper bound, can not go faster
         f1_speed = 0.7;
     }
-    if(f2_speed <= 0.2) {
-        f2_speed = 0.2;
+    if (f1_speed <= 0.05)
+        f1_speed = 0.05;
+
+    if(f2_speed <= 0.05) {  //lower bound, should not be slower than this
+        f2_speed = 0.05;
     }
+
+    pc.printf(" f1: %f", f1_speed);
+    pc.printf(" f2: %f", f2_speed);
+
     //problems when left wheel is held for the + case
     if (Error > 0) { //right wheel is turning more
         m_Left_F.write(f1_speed);
@@ -87,10 +97,9 @@
         m_Right_F.write(f1_speed);
         m_Left_F.write(f2_speed);  //f2_speed
     }
-    if (Error == 0)
-    {
+    if (Error == 0) {
         m_Right_F.write(i_speed);
-        m_Left_F.write(i_speed);    
+        m_Left_F.write(i_speed);
     }
 }
 
@@ -129,6 +138,9 @@
 int main()      //only runs once
 {
     systicker.attach_us(&systick, 1000);    //enable interrupt
-    while (1) {forward();}
+    while (1) {
+        forward();
+    }
     //
+    //debugEncoder();
 }