File content as of revision 2:df0c6af898ac:

#ifndef _MOTOR_H_
#define _MOTOR_H_

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

// Very high-frequency PWM signal so as to cut out noise.
#define PWM_PERIOD_US 10

/**
 * MOTOR CLASS
 * The D-action is used on a moving average filtered version of the error that
 * is put in, against (numerical) noise.
 *
 * __implementation can be found in main.cpp__
 */
class Motor
{
public:
    /**
     * Constructor
     */
    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);
    }

    /**
     * Turn off motor
     */
    void Stop() {
        power_ = false;
        dir_=0;
        speed_=0;
    }

    /**
     * Start motor
     */
    void Start() {
        power_ = true;
    }

    /**
     * Is the motor allowed to move?
     */
    bool has_power() {
        return power_;
    }

    /**
     * Apply PWM value in [-1,1] to motor
     * Direction changes accordingly
     * Only has effect when this.has_power()
     */
    void set_pwm(double pwm) {
        // only when allowed
        if (power_) {
            if (pwm<0) {
                dir_ = kInvertDir_;
            } else {
                dir_ = !kInvertDir_;
            }
            if (abs(pwm)>1) {
                speed_ = 1;
            } else {
                speed_ = abs(pwm);
            }
        }
    }

    /**
     * Get motor angle [deg]
     */
    double get_angle() {
        return offset_ + 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
    }

    /**
     * Set the current motor angle to a given value [deg] (stored in offset)
     */
    void set_angle(double angle) {
        offset_ = angle - 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
    }

private:

    // Encoder, direction pin, pwm pin
    QEI encoder_;
    DigitalOut dir_;
    FastPWM speed_;

    // encoder/gear box constants
    const int kPulsesPerRev_;
    const bool kInvertDir_;

    // allowed to move
    bool power_;

    // internal offset s.t. the angle can be set.
    double offset_;

    // get encoder pulses; only for internal use
    int get_pulses() {
        return kInvertDir_?-encoder_.getPulses():encoder_.getPulses();
    }

};

#endif