Diff: pidControl.cpp

Revision:

7:0fb420b3434f

Parent:

6:48bb8aa4888b

Child:

8:ef6b758b73da

--- a/pidControl.cpp	Tue Oct 20 09:56:40 2015 +0000
+++ b/pidControl.cpp	Tue Oct 20 11:23:35 2015 +0000
@@ -35,8 +35,6 @@
 
 float toRadians(int pulses);
 
-PinDetect stop(SW2);
-PinDetect start(SW3);
 AnalogIn pot1(m1_pot);
 AnalogIn pot2(m2_pot);
 
@@ -91,16 +89,6 @@
     go_motor = true;
 }
 
-void stopMotors()
-{
-    shutup = true;
-}
-
-void startMotors()
-{
-    shutup = false;
-}
-
 void sendGraph()
 {
     go_graph = true;
@@ -118,13 +106,13 @@
     return result;
 }
 
-double pid_control(double error, double kp, double ki, double ts, double &error_integral, 
-    double kd, double previous_error, double &error_derivative, biquadFilter filter)
+double pid_control(double error, double kp, double ki, double ts, double &error_integral,
+                   double kd, double previous_error, double &error_derivative, biquadFilter filter)
 {
     error_integral = error_integral + ts * error;
     error_derivative = (error - previous_error) / ts;
     // error_derivative = filter.step(error_derivative);
-    
+
     double result = kp * error + ki * error_integral + kd * error_derivative;
     return result;
 }
@@ -137,20 +125,10 @@
         return (motor == 1)?0:1;
 }
 
-void initialize()
+void PID_initialize()
 {
     pc.printf("Initializing...\r\n");
 
-    // Set the shutup and start buttons
-    stop.mode(PullUp);
-    stop.attach_deasserted(&stopMotors);
-    stop.setSampleFrequency();
-    start.mode(PullUp);
-    start.attach_deasserted(&startMotors);
-    start.setSampleFrequency();
-    
-    pc.printf("Buttons done\r\n");
-
     // Set proper baudrate
     // pc.baud(115200);
 
@@ -159,6 +137,8 @@
     enc2.reset();
     pc.printf("Encoders reset\r\n");
 
+    go_motor = true;
+
     // Start tickers
     potTicker.attach(&readPot, tickRate);
     motorTicker.attach(&getMotorRotation, tickRate);
@@ -168,52 +148,44 @@
     pc.printf("Initialized\r\n");
 }
 
-void getCurrent(double& a, double& b) {
+void getCurrent(double& a, double& b)
+{
     a = toRadians(enc1.getPulses());
-    b = toRadians(enc2.getPulses());   
+    b = toRadians(enc2.getPulses());
 }
 
-int move(double a, double b) {
-    while (true) {
-        if (shutup) {
-            pwm1 = 0;
-            pwm2 = 0;
-        } else {
-            if (go_pot) {
-                desiredRotation1 = a;
-                desiredRotation2 = b;
-                
-                go_pot = false;
-            }
+int move(double a, double b)
+{
+    if (shutup) {
+        pwm1 = 0;
+        pwm2 = 0;
+    } else {
+        desiredRotation1 = a;
+        desiredRotation2 = b;
 
-            if (go_motor) {
-                currentRotation1 = toRadians(enc1.getPulses());
-                currentRotation2 = toRadians(enc2.getPulses());
+        currentRotation1 = toRadians(enc1.getPulses());
+        currentRotation2 = toRadians(enc2.getPulses());
 
-                double previous_error1 = error1;
-                double previous_error2 = error2;
+        double previous_error1 = error1;
+        double previous_error2 = error2;
 
-                error1 = desiredRotation1 - currentRotation1;
-                error2 = desiredRotation2 - currentRotation2;
-                
-                // PID control
-                double control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
-                double control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
+        error1 = desiredRotation1 - currentRotation1;
+        error2 = desiredRotation2 - currentRotation2;
+
+        // PID control
+        double control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
+        double control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
 
-                int d1 = getPDirection(control1,1);
-                int d2 = getPDirection(control2,2);
-                float speed1 = fabs(control1);
-                float speed2 = fabs(control2);
+        int d1 = getPDirection(control1,1);
+        int d2 = getPDirection(control2,2);
+        float speed1 = fabs(control1);
+        float speed2 = fabs(control2);
 
-                if (speed1 < 0.1f) speed1 = 0.0f;
-                if (speed2 < 0.1f) speed2 = 0.0f;
-                dir1 = d1;
-                dir2 = d2;
-                pwm1 = speed1;
-                pwm2 = speed2;
-
-                go_motor = false;
-            }
-        }
+        if (speed1 < 0.1f) speed1 = 0.0f;
+        if (speed2 < 0.1f) speed2 = 0.0f;
+        dir1 = d1;
+        dir2 = d2;
+        pwm1 = speed1;
+        pwm2 = speed2;
     }
 }
\ No newline at end of file