Diff: arm.cpp

Revision:

0:494acf21d3bc

Child:

2:fc869e45e672

diff -r 000000000000 -r 494acf21d3bc arm.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/arm.cpp	Mon Oct 31 13:05:53 2016 +0000
@@ -0,0 +1,54 @@
+#include "mbed.h"
+#include "arm.h"
+
+/*
+    Arm Constructor
+*/
+
+// Create an arm
+Arm::Arm(float initialLength, PinName motorPWM, PinName motorDir): motorControl(motorPWM), motorDirection(motorDir) {
+    length = initialLength;
+    velocity = 0;
+    motorDirection = clockwise;
+    ticker.attach(this, &Arm::doTick, tick);
+}
+
+/*
+    Methods
+*/
+
+// Update the arm's instance variables
+void Arm::doTick() {
+    // TODO -- Read velocity from encoder
+    length += tick * velocity * gearRadius; // Update length from angular velocity
+}
+
+// Set a new reference velocity
+void Arm::setVelocity(float referenceVelocity) {
+    if (referenceVelocity < 0) {                                                    // Determine direction
+        motorDirection = counterClockwise;
+        referenceVelocity = -1 * referenceVelocity;
+    } else {
+        motorDirection = clockwise;
+    }
+    velocity = (referenceVelocity > maxVelocity)? maxVelocity:referenceVelocity;    // Ensure velocity does not exceed maximum velocity
+    setMotor(velocity/motorGain);                                                   // Instruct motor
+}
+
+// Set motor value
+void Arm::setMotor(float motorValue) {
+    motorControl.write(motorValue > 1 ? 1 : motorValue);
+//    motorControl.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
+}
+
+/*
+    Getters and Setters
+*/
+
+float Arm::getLength()   {
+    return length;
+}
+
+float Arm::getVelocity() {
+    return velocity;
+}
\ No newline at end of file