Diff: main.cpp

Revision:

1:5681fcdc22fe

Parent:

0:bbd4e22aca8a

Child:

2:47ec66bbe8f9

--- a/main.cpp	Wed Oct 11 08:50:26 2017 +0000
+++ b/main.cpp	Wed Oct 11 09:33:32 2017 +0000
@@ -68,25 +68,29 @@
     double referenceangle2 = getreferenceangle(PI, potmeter2);
     double position2 = RAD_PER_PULSE * encoder2.getPosition();
     double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
-    motor1 = fabs(magnitude2);
+    motor2 = fabs(magnitude2);
     
     if (magnitude2 < 0)
     {
-        motor1DirectionPin = 1;
+        motor2DirectionPin = 1;
     }
     else
     {
-        motor1DirectionPin = 0;
+        motor2DirectionPin = 0;
     }
 }
 
 
 int main()
 {
-    
-    while(1) 
-    {
-        motor1_control(motor1_gain);
-        motor2_control(motor1_gain);
+   while(1) 
+   {
+     motor1_control(motor1_gain);
+     wait(0.005f);
+     motor2_control(motor2_gain);
+     wait(0.005f);
     }
+     
+   
+           
 }
\ No newline at end of file