File content as of revision 4:5353c5d0d2ed:

#pragma once

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"

#include "pid.h"

// Amount of motor encoder pulses per rotation. When using X4 encoding.
const int PULSES_PER_ROTATION = 6533;

const double MOTOR_THRESHOLD_RPS = 0.1; // Rad/s under which we send 0 to the motor, to prevent it from jittering around.
const double MOTOR_STALL_PWM = 0.45; // PWM fraction above which the motor starts to move.

class Motor {
    /// Rotates a motor to a given angle using PID control.
    /// Sets the pwm frequency to 60 HZ.
private:
    const float PI = 3.14159265359;

    Ticker motor_ticker;

    FastPWM pwm_out;
    DigitalOut dir_out;
    QEI encoder;
    PID pid;
    
    double target_angle;
    
    // For debugging purposes;
    Serial* pc;
    bool serial_debugging;
    int printcount;
    float pid_period;
public:
    // Initializes the motor with the two driver pins, and the two encoder pins.
    Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc);
    // Initialize the motor without serial output.
    Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b);
    // Set the angle the motor needs to rotate to. In radians.
    void set_target_angle(double angle);
    void set_pid_k_values(double k_p, double k_i, double k_d);
    // Starts the motor controller with the specified update period.
    void start(float period);
    
    // Stops the motor immediately.
    void stop();
    
    // Returns the current angle of the motor.
    double get_current_angle();
    
private:
    void update();
    // Updates the motor with the given speed.
    // The speed can be both positive and negative, in the range [-1, 1].
    void update_motor_speed(double speed);
    double encoder_pulses_to_radians(int pulses);
    // Converts radians/s values into PWM values for motor controll.
    // Both positive and negative values.
    double radians_per_second_to_pwm(double rps);
};