File content as of revision 0:494acf21d3bc:

#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;
}