Diff: pidControl.cpp

Revision:

2:53d2f9b8fed1

Parent:

1:fe23126b0389

Child:

4:a722452d8fd1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pidControl.cpp	Tue Oct 20 08:04:38 2015 +0000
@@ -0,0 +1,213 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+#include "PinDetect.h"
+#include "pidControl.h"
+#include "biquadFilter.h"
+
+Serial pc(USBTX, USBRX);
+
+PinDetect stop(SW2);
+PinDetect start(SW3);
+AnalogIn pot1(m1_pot);
+AnalogIn pot2(m2_pot);
+
+// PWM Speed Control：
+DigitalOut dir1(m1_dir);
+PwmOut pwm1(m1_pwm);
+DigitalOut dir2(m2_dir);
+PwmOut pwm2(m2_pwm);
+
+QEI enc1(m1_enc_a, m1_enc_b, NC, 1);
+QEI enc2(m2_enc_a, m2_enc_b, NC, 1);
+
+Ticker potTicker;
+Ticker motorTicker;
+
+Ticker graphTicker;
+//HIDScope grapher(4);
+
+float currentRotation1 = 0, currentRotation2 = 0;
+float desiredRotation1 = 0, desiredRotation2 = 0;
+double error1 = 0, error2 = 0;
+
+double m1_error_integral = 0, m2_error_integral = 0;
+double m1_error_derivative = 0, m2_error_derivative = 0;
+biquadFilter m1_filter(m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
+biquadFilter m2_filter(m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2);
+
+bool shutup = true;
+bool go_pot = false;
+bool go_motor = false;
+bool go_graph = false;
+
+float getPotRad(AnalogIn pot)
+{
+    return pot.read() * 4.0f - 2.0f;
+}
+
+float toRadians(int pulses)
+{
+    int remaining = pulses;// % sigPerRev;
+    float percent = (float) remaining / (float) sigPerRev;
+    return percent * 2.0f;
+}
+
+void readPot()
+{
+    go_pot = true;
+}
+
+void getMotorRotation()
+{
+    go_motor = true;
+}
+
+void stopMotors()
+{
+    shutup = true;
+}
+
+void startMotors()
+{
+    shutup = false;
+}
+
+void sendGraph()
+{
+    go_graph = true;
+}
+
+double p_control(double error, double kp)
+{
+    return kp * error;
+}
+
+double pi_control(double error, double kp, double ki, double ts, double &error_integral)
+{
+    error_integral = error_integral + ts * error;
+    double result =  kp * error + ki * error_integral;
+    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)
+{
+    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;
+}
+
+int getPDirection(double control, int motor)
+{
+    if (control >= 0)
+        return (motor == 1)?1:0;
+    else
+        return (motor == 1)?0:1;
+}
+
+void 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);
+
+    // Reset encoders
+    enc1.reset();
+    enc2.reset();
+    pc.printf("Encoders reset\r\n");
+
+    // Start tickers
+    potTicker.attach(&readPot, tickRate);
+    motorTicker.attach(&getMotorRotation, tickRate);
+    graphTicker.attach(&sendGraph, graphTickRate);
+    pc.printf("Tickers attached\r\n");
+
+    pc.printf("Initialized\r\n");
+}
+
+int main()
+{
+    pc.printf("Started\r\n");
+    initialize();
+
+    while (true) {
+        if (shutup) {
+            pwm1 = 0;
+            pwm2 = 0;
+        } else {
+            if (go_pot) {
+                desiredRotation1 = getPotRad(pot1);
+                desiredRotation2 = getPotRad(pot2);
+                
+                go_pot = false;
+            }
+
+            if (go_motor) {
+                currentRotation1 = toRadians(enc1.getPulses());
+                currentRotation2 = toRadians(enc2.getPulses());
+                
+                pc.printf("PULSE: %i     | CUR ROT: %f         | ERR: %f        | DES ROT: %f\r\n",enc2.getPulses(),currentRotation2, error2,desiredRotation2);
+
+                double previous_error1 = error1;
+                double previous_error2 = error2;
+
+                error1 = desiredRotation1 - currentRotation1;
+                error2 = desiredRotation2 - currentRotation2;
+                // P control
+                // double control1 = p_control(error1, motor1_kp);
+                // double control2 = p_control(error2, motor2_kp);
+                
+                // PI control
+                //double control1 = pi_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral);
+                // double control2 = pi_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral);
+                
+                // 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);
+
+                if (speed1 < 0.1f) speed1 = 0.0f;
+                if (speed2 < 0.1f) speed2 = 0.0f;
+                dir1 = d1;
+                dir2 = d2;
+                pwm1 = speed1;
+                pwm2 = speed2;
+                pc.printf("SPEED: %f      | DIR: %i\r\n",speed2,d2);
+
+                go_motor = false;
+            }
+
+            /*if (go_graph) {
+                pc.printf("Trying to send graph\r\n");
+                pc.printf("Desired Rotation: %f %f\r\n", desiredRotation1, desiredRotation2);
+                pc.printf("Rotation: %f %f\r\n", currentRotation1, desiredRotation2);
+                grapher.set(0, desiredRotation1);
+                grapher.set(1, currentRotation1);
+                grapher.set(2, desiredRotation2);
+                grapher.set(3, currentRotation2);
+                grapher.send();
+                go_graph = false;
+            }*/
+        }
+    }
+}