Diff: actuators.cpp

Revision:

68:21cb054f1399

Parent:

66:d1ab5904f8e5

Child:

69:37f75a7d36d8

--- a/actuators.cpp	Thu Oct 15 16:08:03 2015 +0200
+++ b/actuators.cpp	Thu Oct 15 16:15:05 2015 +0200
@@ -8,6 +8,9 @@
     // functions for controlling the motors
     bool motorsEnable = false;
 
+    double encoder1Counts = 0;
+    double encoder2Counts = 0;
+
     bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
     bool direction2 = false;
 
@@ -34,8 +37,8 @@
     double Ki2 = 0.32; 
     double Kd2 = 0;
 
-    double motor1PrevPos = 0;
-    double motor2PrevPos = 0;
+    double motor1PrevCounts = 0;
+    double motor2PrevCounts = 0;
     double prevTime = 0;
     double now = 0;
     double timechange;
@@ -87,18 +90,23 @@
         // 131.25:1 gear ratio
         // getPosition uses X2 configuration, so 32 counts per revolution
         // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
-        motor1Pos = -((encoder1.getPosition()/32)/131.25)*360;
-        motor2Pos = -((encoder2.getPosition()/32)/131.25)*360;
+
+        encoder1Counts = encoder1.getPosition();
+        encoder2Counts = encoder2.getPosition();
+
+
+        motor1Pos = -((encoder1Counts/32)/131.25)*360;
+        motor2Pos = -((encoder2Counts/32)/131.25)*360;
 
         // check if motor's are within rotational boundarys
     // 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 = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
+        motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
         prevTime = now;
-        motor1PrevPos = encoder1.getPosition();
-        motor2PrevPos = encoder2.getPosition();
+        motor1PrevCounts = encoder1Counts;
+        motor2PrevCounts = encoder2Counts;
         
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
@@ -122,16 +130,16 @@
     }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);
     
     motor1.write(abs(motor1PWM));
-    motor2.write(abs(motor2SetSpeed)/300);
+    motor2.write(abs(motor2PWM));
 }
 
 void servoControl(){