File content as of revision 0:caa8ee3bd882:

#ifndef _MOTOR_H_
#define _MOTOR_H_

#include "FastPWM.h"
#include "QEI.h"

#define PWM_PERIOD_US 10

class Motor
{
public:
    Motor(PinName pwm, PinName dir, PinName enc_a, PinName enc_b,
          int pulses_per_rev, bool invert_dir = false) :
        encoder_(enc_a, enc_b, NC, NULL, QEI::X4_ENCODING),
        dir_(dir),
        speed_(pwm),
        kPulsesPerRev_(pulses_per_rev),
        kInvertDir_(invert_dir) 
    {
        Stop();
        offset_ = 0;
        speed_.period_us(PWM_PERIOD_US);
    }

    void Stop() {
        power_ = false;
        dir_=0;
        speed_=0;
    }

    void Start() {
        power_ = true;
    }

    bool has_power() {
        return power_;
    }

    void set_pwm(double pwm) {
        if (power_) {
            if (pwm<0) {
                dir_ = kInvertDir_;
            } else {
                dir_ = !kInvertDir_;
            }
            if (abs(pwm)>1) {
                speed_ = 1;
            } else {
                speed_ = abs(pwm);
            }
        }
    }

    double get_angle() {
        return offset_ + 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
    }

    void set_angle(double angle) {
        offset_ = angle - 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
    }

private:

    QEI encoder_;
    DigitalOut dir_;
    FastPWM speed_;
    const int kPulsesPerRev_;
    const bool kInvertDir_;
    bool power_;
    double offset_;

    int get_pulses() {
        return kInvertDir_?-encoder_.getPulses():encoder_.getPulses();
    }

};

#endif