File content as of revision 2:fc869e45e672:

#include "mbed.h"
#include "arm.h"

/*
    Arm Constructor
*/

// Create an arm
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;
}

/*
    Methods
*/

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

// 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) {
        motorDirection = counterClockwise;
        velocity = (referenceVelocity < -1 * maxVelocity)? -1 * maxVelocity:referenceVelocity;
    } else {
        motorDirection = clockwise;
        velocity = (referenceVelocity > maxVelocity)? maxVelocity:referenceVelocity;
    }
    setMotor(fabs(velocity)/motorGain);
}

// Set motor value (ranges from 0 to 1)
void Arm::setMotor(float motorValue) {
    motorControl.write((motorValue > 1)? 1 : motorValue);
}

// Stop moving
void Arm::kill() {
    setVelocity(0);
}

/*
    Getters and Setters
*/

float Arm::getLength()   {
    return length;
}

float Arm::getEncoderVelocity() {
    return encoderVelocity;
}

float Arm::getVelocity() {
    return velocity;
}