File content as of revision 7:eb8787e7a5f5:

#include "motor.h"

Motor::Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b):
    pwm_out(pwm_pin),
    dir_out(dir_pin),
    encoder(encoder_a, encoder_b, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING)
    {
    pid = PID();
    
    target_angle = 0;
    extra_reduction_ratio = 1.0;
    
    printcount = 0;
    pid_period = 0;
    serial_debugging = false;
    
    // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once)
    pwm_out.period_us(60.0);
}

Motor::Motor(PinName pwm_pin, PinName dir_pin, PinName encoder_a, PinName encoder_b, Serial* pc):
    pwm_out(pwm_pin),
    dir_out(dir_pin),
    encoder(encoder_a, encoder_b, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING)
    {
    pid = PID();
    
    target_angle = 0;
    
    printcount = 0;
    pid_period = 0;
    this->pc = pc;
    serial_debugging = true;
    
    // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once)
    pwm_out.period_us(60.0);
}

void Motor::start(float period) {
    pid_period = period;
    pid.set_period(period);
    motor_ticker.attach(callback(this, &Motor::update), period);
}

void Motor::stop() {
    motor_ticker.detach();
    target_angle = get_current_angle();
    // Stop the actual motor.
    pwm_out = 0.0;
    dir_out = 0;
    
    pid.clear_state();
    
    printcount = 0;
}

void Motor::set_current_angle_as_zero() {
    encoder.reset();
    target_angle = 0;
}

void Motor::set_pid_k_values(double k_p, double k_i, double k_d) {
    pid.set_k_values(k_p, k_i, k_d);
}

void Motor::set_extra_reduction_ratio(double ratio) {
    extra_reduction_ratio = ratio;
}

void Motor::set_target_angle(double angle) {
    target_angle = angle * extra_reduction_ratio;
}

double Motor::get_current_angle() {
    return encoder_pulses_to_radians(encoder.getPulses()) * extra_reduction_ratio;
}

void Motor::update() {
    int pulses = encoder.getPulses();
    double current_angle = encoder_pulses_to_radians(pulses);
    
    double error = current_angle - target_angle;
    // PID controll.
    double speed_rps = pid.update(error);
    
    double speed_pwm = radians_per_second_to_pwm(speed_rps);
    
    if (serial_debugging) {
    
        printcount++;
        if (printcount >= 0.1L/pid_period) {
            pc->printf("c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", current_angle, target_angle, error, speed_rps, speed_pwm);
            printcount = 0;
        }
    }
    
    update_motor_speed(speed_pwm);
}


void Motor::update_motor_speed(double speed) {
    if (speed < 1.0 && speed > 0) {
        // Speed is in the range [0, 1] but the motor only moves
        // in the range [0.5, 1]. Rescale for this.
        speed = (speed * (1-MOTOR_STALL_PWM)) + MOTOR_STALL_PWM;
    }
    if (speed > -1.0 && speed < 0) {
        // Speed is in the range [-1, 0] but the motor only moves
        // in the range [-1, -0.5]. Rescale for this.
        speed = (speed * (1-MOTOR_STALL_PWM)) - MOTOR_STALL_PWM;
    }
    
    // either true or false, determines direction (0 or 1)
    dir_out = speed > 0;
    // pwm duty cycle can only be positive, floating point absolute value (if value is >0, the there still will be a positive value).
    pwm_out = fabs(speed);
}

double Motor::encoder_pulses_to_radians(int pulses) {
    return (pulses/float(PULSES_PER_ROTATION)) * 2.0f*PI;
}


// Converts radians/s values into PWM values for motor controll.
// Both positive and negative values.
double Motor::radians_per_second_to_pwm(double rps) {
    // If the rad/s is below the anti-jitter treshold, it is simply 0.
    if (rps > 0 && rps < MOTOR_THRESHOLD_RPS) {
        rps = 0;
    }
    if (rps < 0 && rps > -MOTOR_THRESHOLD_RPS) {
        rps = 0;
    }
    
    
    // With our specific motor, full PWM is equal to 1 round per second.
    // Or 2PI radians per second.
    double pwm_speed = rps / (2*PI);
    
    // PWM speeds can only go between [-1, 1]
    if (pwm_speed > 1) { pwm_speed = 1; }
    if (pwm_speed < -1) { pwm_speed = -1; }
    return pwm_speed;
}