File content as of revision 12:8295c02d740f:

#include "mbed.h"
#include "QEI.h"

/*
    Constants
*/

const float pi = 3.14159265359f;

const bool clockwise = true;
const bool counterClockwise = false;

const float motorGain = 8.4f;                                                           // Rad/s
const float maxVelocity = 8.4f;                                                         // Rad/s (max velocity: 80 rpm = 80*2*pi/60 = 8.4 rad/s)

const float tick = 0.1f;                                                                // In seconds
const float gearRadius = 5.2f;                                                          // In cm

const float gearRatio = 1/131.25;                                                       // Gear ratio of the motor
const int pulsesPerRevolution = 32;                                                     // Encoder pulses per revolution
const float outputShaftResolution = pulsesPerRevolution/(gearRatio * 2 * pi);           // In counts per rad

/*
    Arm class
*/

class Arm {
    private:
        // Current length of the arm in centimeters
        volatile float length;
        // Reference angular velocity in rad/s
        volatile float velocity;
        // Real velocity according to the encoder                                                     
        volatile float encoderVelocity;
        // Maximum allowed velocity in rad/s
        float maxAllowedVelocity;
        // Motor PWM
        PwmOut motorControl;
        // Motor direction
        DigitalOut motorDirection;
        // Motor encoder
        QEI qei;
        
        // Directly instruct motor (0 <= motorValue <= 1)
        void setMotor(float motorValue);
        // Read encoder angular velocity estimation
        void updateEncoderVelocity();
    public:
        // Constructor
        Arm(float initialLength, float maxVelocity, PinName motorPWM, PinName motorDir, PinName encoderPin1, PinName encoderPin2);
        
        // Update instance variables
        void update();
        // Get current arm length
        float getLength();
        // Get reference angular velocity
        float getVelocity();
        // Get encoder angular velocity estimation
        float getEncoderVelocity();
        // Set reference velocity
        void setVelocity(float referenceVelocity);
        // Stop moving
        void kill();
        
};