Diff: pidControl.cpp

Revision:

5:937b2f34a1ca

Parent:

4:a722452d8fd1

Child:

6:48bb8aa4888b

--- a/pidControl.cpp	Tue Oct 20 08:35:36 2015 +0000
+++ b/pidControl.cpp	Tue Oct 20 09:42:24 2015 +0000
@@ -8,6 +8,33 @@
 
 Serial pc(USBTX, USBRX);
 
+PinName m1_enc_a = D12;
+PinName m1_enc_b = D13;
+PinName m1_pwm = D6;
+PinName m1_dir = D7;
+
+PinName m2_enc_a = D11;
+PinName m2_enc_b = D10;
+PinName m2_pwm = D5;
+PinName m2_dir = D4;
+
+PinName m2_pot = A0;
+PinName m1_pot = A1;
+
+const double pid_upper_limit = 1, pid_lower_limit = 0;
+
+const double motor1_kp = 0.75f, motor1_ki = 0.001f, motor1_kd = 0.005f;
+const double motor2_kp = 0.75f, motor2_ki = 0.001f, motor2_kd = 0.005f;
+
+const double m1_f_a1 = 1.0, m1_f_a2 = 2.0, m1_f_b0 = 1.0, m1_f_b1 = 3.0, m1_f_b2 = 4.0;
+const double m2_f_a1 = 1.0, m2_f_a2 = 2.0, m2_f_b0 = 1.0, m2_f_b1 = 3.0, m2_f_b2 = 4.0;
+
+int sigPerRev = 4192;
+float tickRate = 0.01f;
+float graphTickRate = 0.01f;
+
+float toRadians(int pulses);
+
 PinDetect stop(SW2);
 PinDetect start(SW3);
 AnalogIn pot1(m1_pot);
@@ -140,20 +167,16 @@
 
     pc.printf("Initialized\r\n");
 }
-/*
-int main()
-{
-    pc.printf("Started\r\n");
-    initialize();
 
+int move(double a, double b) {
     while (true) {
         if (shutup) {
             pwm1 = 0;
             pwm2 = 0;
         } else {
             if (go_pot) {
-                desiredRotation1 = getPotRad(pot1);
-                desiredRotation2 = getPotRad(pot2);
+                desiredRotation1 = a;
+                desiredRotation2 = b;
                 
                 go_pot = false;
             }
@@ -161,21 +184,12 @@
             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);
@@ -192,22 +206,9 @@
                 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;
-            }*/
-//        }
-//    }
-//}
+        }
+    }
+}
\ No newline at end of file