Diff: PID_control.cpp

Revision:

4:90303483fd5f

Parent:

2:aa961ba3199e

Child:

5:a49a77ddf4e3

--- a/PID_control.cpp	Fri Nov 10 19:26:37 2017 +0000
+++ b/PID_control.cpp	Fri Nov 10 23:05:18 2017 +0000
@@ -2,84 +2,133 @@
 #include "QEI.h"
 
 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 = .02;
-double Ki = .0000001;
-double Kd = 0.001; 
+QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
+double Kp = .005;
+double Ki = 0;//.0000001;
+double Kd = 0.001;
 PwmOut m_Right_F(PB_10);
 PwmOut m_Right_B(PC_7);
-PwmOut m_Left_F(PA_7); 
-PwmOut m_Left_B(PB_6); 
-double i_speed = 0.3;
+PwmOut m_Left_F(PA_7);
+PwmOut m_Left_B(PB_6);
+double i_speed = 0.1;
+double C_speed(0);
 int integrator = 0;
 int decayFactor = 1;
-int prevError = 0; 
+double Error = 0;
+double prevError = 0;
 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
-Timer timer; 
+Timer timer;
+int counter = 0;
 
-double P_controller(int error) {
+double P_controller(int error)
+{
     double correction = (Kp*error);
+    //pc.printf("correction is: %d\n", correction);
     return correction;
 }
 
-double I_controller(int error) {
+double I_controller(int error)
+{
     integrator += error;
     double correction = Ki*integrator;
     integrator /= decayFactor;
-    
+
     return correction;
 }
 
-double D_controller(int error) {
+double D_controller(int error)
+{
     int dError = error - prevError;
     int dt = timer.read_us();
     timer.reset();
     prevError = error;
-    int correction = Kd*dError/dt;
+    double correction = Kd*dError/dt;
     return correction;
 }
 
 Ticker systicker;
 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
-void systick() {
-     int R_en_count = encoder_Right.getPulses()/100;
-     int L_en_count = encoder_Left.getPulses()/100;
-     int error = R_en_count - L_en_count; 
-     double C_speed = P_controller(error) + I_controller(error) + D_controller(error);
-     double f1_speed = C_speed + i_speed;
-     double f2_speed = C_speed - i_speed;
-     if(f1_speed >= 0.7)
-     {
-        f1_speed = 0.7;
-     }
-     if(f2_speed <= 0.2)
-     {
-         f2_speed = 0.2;
-     }
-       
-     if (error > 0)
-     {
-         m_Left_F.write(f1_speed); 
-         m_Right_F.write(f2_speed);
-     }
-     else
-     {
-        m_Right_F.write(f1_speed); 
-        m_Left_F.write(f2_speed);  
-     }
-     
-      
+void systick()
+{
+    double R_en_count = encoder_Right.getPulses()/100;
+    double L_en_count = encoder_Left.getPulses()/100;
+    Error = R_en_count - L_en_count;
+    double ex = D_controller(Error);
+    // if (ex < 0)
+    //    ex = -ex;
+    C_speed = P_controller(Error) + I_controller(Error) + ex;
 }
 
-int main() {
-    systicker.attach_us(&systick, 1000);
-    m_Left_F.write(i_speed);
-    m_Right_F.write(i_speed); 
-    while(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) {
+        f1_speed = 0.7;
+    }
+    if(f2_speed <= 0.2) {
+        f2_speed = 0.2;
+    }
+    //problems when left wheel is held for the + case
+    if (Error > 0) { //right wheel is turning more
+        m_Left_F.write(f1_speed);
+        m_Right_F.write(f2_speed); //f2_speed
+    }
+    if (Error < 0) { //left wheel is turning more
+        m_Right_F.write(f1_speed);
+        m_Left_F.write(f2_speed);  //f2_speed
+    }
+    if (Error == 0)
     {
-        wait(.1);
-        pc.printf("Right Pulses sre: %i\n", encoder_Right.getPulses());
-        pc.printf("Left Pulses are: %i\n", encoder_Left.getPulses());
+        m_Right_F.write(i_speed);
+        m_Left_F.write(i_speed);    
     }
-    
+}
+
+void turnRight()
+{
+
+}
+
+void turnLeft()
+{
+
+}
+
+void turnAround()
+{
+
 }
+
+void debugEncoder()
+{
+    while(1) {
+        wait(1);
+        pc.printf("Right: %i", encoder_Right.getPulses());
+        pc.printf("  Left: %i", encoder_Left.getPulses(), "\n");
+        pc.printf("\n");
+    }
+}
+
+void debugError()
+{
+    while(1) {
+        pc.printf("Error: %i\n", Error);
+    }
+}
+
+int main()      //only runs once
+{
+    systicker.attach_us(&systick, 1000);    //enable interrupt
+    while (1) {forward();}
+    //
+}