Diff: main.cpp

Revision:

1:fe23126b0389

Parent:

0:d38eb4622914

--- a/main.cpp	Wed Oct 14 13:47:18 2015 +0000
+++ b/main.cpp	Mon Oct 19 11:43:21 2015 +0000
@@ -26,7 +26,7 @@
 Ticker motorTicker;
 
 Ticker graphTicker;
-HIDScope grapher(4);
+//HIDScope grapher(4);
 
 float currentRotation1 = 0, currentRotation2 = 0;
 float desiredRotation1 = 0, desiredRotation2 = 0;
@@ -81,7 +81,6 @@
 
 double p_control(double error, double kp)
 {
-    double result = kp * error;
     return kp * error;
 }
 
@@ -103,12 +102,12 @@
     return result;
 }
 
-int getPDirection(double control)
+int getPDirection(double control, int motor)
 {
     if (control >= 0)
-        return 1;
+        return (motor == 1)?1:0;
     else
-        return 0;
+        return (motor == 1)?0:1;
 }
 
 void initialize()
@@ -130,7 +129,7 @@
 
     // Reset encoders
     enc1.reset();
-    // enc2.reset();
+    enc2.reset();
     pc.printf("Encoders reset\r\n");
 
     // Start tickers
@@ -154,22 +153,22 @@
         } else {
             if (go_pot) {
                 desiredRotation1 = getPotRad(pot1);
-                //desiredRotation2 = getPotRad(pot2);
+                desiredRotation2 = getPotRad(pot2);
                 
                 go_pot = false;
             }
 
             if (go_motor) {
                 currentRotation1 = toRadians(enc1.getPulses());
-                //currentRotation2 = toRadians(enc2.getPulses());
+                currentRotation2 = toRadians(enc2.getPulses());
                 
-                pc.printf("PULSE: %i     | CUR ROT: %f         | ERR: %f        | DES ROT: %f\r\n",enc1.getPulses(),currentRotation1, error1,desiredRotation1);
+                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;
+                double previous_error2 = error2;
 
                 error1 = desiredRotation1 - currentRotation1;
-               // error2 = desiredRotation2 - currentRotation2;
+                error2 = desiredRotation2 - currentRotation2;
                 // P control
                 // double control1 = p_control(error1, motor1_kp);
                 // double control2 = p_control(error2, motor2_kp);
@@ -180,25 +179,25 @@
                 
                 // 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);
+                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);
-                //int d2 = getPDirection(control2);
+                int d1 = getPDirection(control1,1);
+                int d2 = getPDirection(control2,2);
                 float speed1 = fabs(control1);
-               // float speed2 = fabs(control2);
+                float speed2 = fabs(control2);
 
                 if (speed1 < 0.1f) speed1 = 0.0f;
-               // if (speed2 < 0.1f) speed2 = 0.0f;
+                if (speed2 < 0.1f) speed2 = 0.0f;
                 dir1 = d1;
-               // dir2 = d2;
+                dir2 = d2;
                 pwm1 = speed1;
-                //pwm2 = speed2;
-                pc.printf("SPEED: %f      | DIR: %i\r\n",speed1,d1);
+                pwm2 = speed2;
+                pc.printf("SPEED: %f      | DIR: %i\r\n",speed2,d2);
 
                 go_motor = false;
             }
 
-            if (go_graph) {
+            /*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);
@@ -208,7 +207,7 @@
                 grapher.set(3, currentRotation2);
                 grapher.send();
                 go_graph = false;
-            }
+            }*/
         }
     }
 }