Diff: PID_control.cpp

Revision:

2:aa961ba3199e

Child:

4:90303483fd5f

diff -r 1b18e69bf50f -r aa961ba3199e PID_control.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_control.cpp	Fri Nov 10 19:23:08 2017 +0000
@@ -0,0 +1,85 @@
+#include "mbed.h"
+#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; 
+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;
+int integrator = 0;
+int decayFactor = 1;
+int prevError = 0; 
+Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
+Timer timer; 
+
+double P_controller(int error) {
+    double correction = (Kp*error);
+    return correction;
+}
+
+double I_controller(int error) {
+    integrator += error;
+    double correction = Ki*integrator;
+    integrator /= decayFactor;
+    
+    return correction;
+}
+
+double D_controller(int error) {
+    int dError = error - prevError;
+    int dt = timer.read_us();
+    timer.reset();
+    prevError = error;
+    int 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);  
+     }
+     
+      
+}
+
+int main() {
+    systicker.attach_us(&systick, 1000);
+    m_Left_F.write(i_speed);
+    m_Right_F.write(i_speed); 
+    while(1)
+    {
+        wait(.1);
+        pc.printf("Right Pulses sre: %i\n", encoder_Right.getPulses());
+        pc.printf("Left Pulses are: %i\n", encoder_Left.getPulses());
+    }
+    
+}