Diff: actuators.cpp

Revision:

61:157df6f8ceec

Parent:

60:20945383ad1b

Child:

62:6c566e6f9664

--- a/actuators.cpp	Mon Oct 12 11:46:05 2015 +0000
+++ b/actuators.cpp	Tue Oct 13 18:28:39 2015 +0200
@@ -94,11 +94,11 @@
     // get  encoder speeds in deg/sec
         now = t.read();
         timechange = (now - prevTime);
-        motor1Speed = (motor1Pos - motor1PrevPos)/timechange;
-        motor2Speed = (motor2Pos - motor2PrevPos)/timechange;
+        motor1Speed = ((((encoder1.getPosition() - motor1PrevPos)/32)/131.25)*360)/timechange;
+        motor2Speed = ((((encoder2.getPosition() - motor2PrevPos)/32)/131.25)*360)/timechange;
         prevTime = now;
-        motor1PrevPos = motor1Pos;
-        motor2PrevPos = motor2Pos;
+        motor1PrevPos = encoder1.getPosition();
+        motor2PrevPos = encoder2.getPosition();
         
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
@@ -122,11 +122,11 @@
     }else{
         direction1 = true; // CW rotation
     }
-    if (motor2PWM >= 0 ){ // CCW rotation 
-        direction2 = false;
-    }else{
-        direction2 = true; // CW rotation
-    }
+    // if (motor2PWM >= 0 ){ // CCW rotation 
+    //     direction2 = false;
+    // }else{
+    //     direction2 = true; // CW rotation
+    // }
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);