Diff: actuators.cpp

Revision:

62:6c566e6f9664

Parent:

61:157df6f8ceec

Child:

66:d1ab5904f8e5

--- a/actuators.cpp	Tue Oct 13 18:28:39 2015 +0200
+++ b/actuators.cpp	Wed Oct 14 13:52:16 2015 +0200
@@ -11,34 +11,34 @@
     bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
     bool direction2 = false;
 
-    float motor1Pos = 0;
-    float motor2Pos = 0;
+    double motor1Pos = 0;
+    double motor2Pos = 0;
 
-    float motor1Speed = 0;
-    float motor2Speed = 0;
+    double motor1Speed = 0;
+    double motor2Speed = 0;
 
-    float motor1SetSpeed = 0;
-    float motor2SetSpeed = 0;
+    double motor1SetSpeed = 0;
+    double motor2SetSpeed = 0;
 
-    float servoPos = 0;
+    double servoPos = 0;
 
-    float motor1PWM = 0;
-    float motor2PWM = 0;
+    double motor1PWM = 0;
+    double motor2PWM = 0;
 
     // Set PID values
-    float Kp1 = 1; 
-    float Ki1 = 0; 
-    float Kd1 = 0;
+    double Kp1 = 1; 
+    double Ki1 = 0; 
+    double Kd1 = 0;
 
-    float Kp2 = 1; 
-    float Ki2 = 0; 
-    float Kd2 = 0;
+    double Kp2 = 1; 
+    double Ki2 = 0; 
+    double Kd2 = 0;
 
-    float motor1PrevPos = 0;
-    float motor2PrevPos = 0;
-    float prevTime = 0;
-    float now = 0;
-    float timechange;
+    double motor1PrevPos = 0;
+    double motor2PrevPos = 0;
+    double prevTime = 0;
+    double now = 0;
+    double timechange;
     bool pidOut = 0;
 
     // Create object instances
@@ -94,8 +94,8 @@
     // get  encoder speeds in deg/sec
         now = t.read();
         timechange = (now - prevTime);
-        motor1Speed = ((((encoder1.getPosition() - motor1PrevPos)/32)/131.25)*360)/timechange;
-        motor2Speed = ((((encoder2.getPosition() - motor2PrevPos)/32)/131.25)*360)/timechange;
+        motor1Speed = -((((encoder1.getPosition() - motor1PrevPos)/32)/131.25)*360)/timechange;
+        motor2Speed = -((((encoder2.getPosition() - motor2PrevPos)/32)/131.25)*360)/timechange;
         prevTime = now;
         motor1PrevPos = encoder1.getPosition();
         motor2PrevPos = encoder2.getPosition();