Diff: arm.cpp

Revision:

7:a80cb6b06320

Parent:

2:fc869e45e672

Child:

12:8295c02d740f

--- a/arm.cpp	Wed Nov 02 09:26:38 2016 +0000
+++ b/arm.cpp	Wed Nov 02 16:29:13 2016 +0000
@@ -21,9 +21,8 @@
 
 // Update the arm's instance variables
 void Arm::update() {
-    updateEncoderVelocity();
-//    length += (motorDirection? 1:-1) * tick * velocity * gearRadius;                // Update length from angular velocity
-    length += tick * encoderVelocity * gearRadius;
+    updateEncoderVelocity();           
+    length += tick * encoderVelocity * gearRadius; // Update length from angular velocity
 }
 
 // Read pulses since previous measurement to estimate angular velocity
@@ -34,19 +33,22 @@
 
 // Set a new reference velocity
 void Arm::setVelocity(float referenceVelocity) {
+    // Counterclockwise rotation
     if (referenceVelocity < 0) {
         motorDirection = counterClockwise;
         velocity = (referenceVelocity < -1 * maxVelocity)? -1 * maxVelocity:referenceVelocity;
+    // Clockwise rotation
     } else {
         motorDirection = clockwise;
         velocity = (referenceVelocity > maxVelocity)? maxVelocity:referenceVelocity;
     }
-    setMotor(fabs(velocity)/motorGain);
+    // Output to motor
+    setMotor(velocity/motorGain);
 }
 
 // Set motor value (ranges from 0 to 1)
 void Arm::setMotor(float motorValue) {
-    motorControl.write((motorValue > 1)? 1 : motorValue);
+    motorControl.write((fabs(motorValue) > 1)? 1 : fabs(motorValue));
 }
 
 // Stop moving
@@ -55,7 +57,7 @@
 }
 
 /*
-    Getters and Setters
+    Getters
 */
 
 float Arm::getLength()   {