Diff: arm.cpp

Revision:

2:fc869e45e672

Parent:

0:494acf21d3bc

Child:

7:a80cb6b06320

--- a/arm.cpp	Mon Oct 31 13:05:53 2016 +0000
+++ b/arm.cpp	Wed Nov 02 08:51:12 2016 +0000
@@ -6,11 +6,13 @@
 */
 
 // Create an arm
-Arm::Arm(float initialLength, PinName motorPWM, PinName motorDir): motorControl(motorPWM), motorDirection(motorDir) {
+Arm::Arm(float initialLength, PinName motorPWM, PinName motorDir, PinName encoderPin1, PinName encoderPin2): motorControl(motorPWM), 
+                                                                                                             motorDirection(motorDir),
+                                                                                                             qei(encoderPin1, encoderPin2, NC, pulsesPerRevolution) {
     length = initialLength;
     velocity = 0;
+    encoderVelocity = 0;
     motorDirection = clockwise;
-    ticker.attach(this, &Arm::doTick, tick);
 }
 
 /*
@@ -18,27 +20,38 @@
 */
 
 // Update the arm's instance variables
-void Arm::doTick() {
-    // TODO -- Read velocity from encoder
-    length += tick * velocity * gearRadius; // Update length from angular velocity
+void Arm::update() {
+    updateEncoderVelocity();
+//    length += (motorDirection? 1:-1) * tick * velocity * gearRadius;                // Update length from angular velocity
+    length += tick * encoderVelocity * gearRadius;
+}
+
+// Read pulses since previous measurement to estimate angular velocity
+void Arm::updateEncoderVelocity() {
+    encoderVelocity = ((qei.getPulses()/outputShaftResolution))/tick;
+    qei.reset();
 }
 
 // Set a new reference velocity
 void Arm::setVelocity(float referenceVelocity) {
-    if (referenceVelocity < 0) {                                                    // Determine direction
+    if (referenceVelocity < 0) {
         motorDirection = counterClockwise;
-        referenceVelocity = -1 * referenceVelocity;
+        velocity = (referenceVelocity < -1 * maxVelocity)? -1 * maxVelocity:referenceVelocity;
     } else {
         motorDirection = clockwise;
+        velocity = (referenceVelocity > maxVelocity)? maxVelocity:referenceVelocity;
     }
-    velocity = (referenceVelocity > maxVelocity)? maxVelocity:referenceVelocity;    // Ensure velocity does not exceed maximum velocity
-    setMotor(velocity/motorGain);                                                   // Instruct motor
+    setMotor(fabs(velocity)/motorGain);
 }
 
-// Set motor value
+// 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));
+    motorControl.write((motorValue > 1)? 1 : motorValue);
+}
+
+// Stop moving
+void Arm::kill() {
+    setVelocity(0);
 }
 
 /*
@@ -49,6 +62,10 @@
     return length;
 }
 
+float Arm::getEncoderVelocity() {
+    return encoderVelocity;
+}
+
 float Arm::getVelocity() {
     return velocity;
 }
\ No newline at end of file