File content as of revision 18:1c9dc6caab9d:

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

/*
    Arm Constructor
*/

// Create an arm
Arm::Arm(float initialLength, float maxVelocity, PinName motorPWM, PinName motorDir, PinName encoderPin1, PinName encoderPin2)
: motorControl(motorPWM)
, motorDirection(motorDir)
, qei(encoderPin1, encoderPin2, NC, pulsesPerRevolution)
{
    length = initialLength;
    maxAllowedVelocity = maxVelocity;
    velocity = 0;
    encoderVelocity = 0;
    motorDirection = clockwise;
}

/*
    Methods
*/

// Update the arm's instance variables
void Arm::update() {
    updateEncoderVelocity();           
    length += tick * encoderVelocity * gearRadius; // Update length from angular velocity
}

// 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) {
    // Counterclockwise rotation
    if (referenceVelocity < 0) {
        motorDirection = counterClockwise;
        velocity = (referenceVelocity < (-1 * maxAllowedVelocity))? (-1 * maxAllowedVelocity):referenceVelocity;
    // Clockwise rotation
    } else {
        motorDirection = clockwise;
        velocity = (referenceVelocity > maxAllowedVelocity)? maxAllowedVelocity:referenceVelocity;
    }
    // Output to motor
    setMotor(velocity/motorGain);
}

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

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

/*
    Getters
*/

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

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

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