File content as of revision 6:732aa91eb555:

#include "robot.h"

DigitalOut nsleep(p7);
DigitalIn nfault_l(p8,PullUp);
DigitalIn nfault_r(p11,PullUp);
AnalogIn motor_vprop_l(p19);
AnalogIn motor_vprop_r(p20);
PwmOut enable_l(p21);
PwmOut enable_r(p22);
DigitalOut hb1_l(p23);
DigitalOut hb1_r(p24);
DigitalOut hb2_l(p29);
DigitalOut hb2_r(p30);
float left_motor_speed = 0;
float right_motor_speed = 0;
int motor_pwm_period = 0;

float Motors::get_left_motor_speed()
{
    return left_motor_speed;
}

float Motors::get_right_motor_speed()
{
    return right_motor_speed;
}

void Motors::forwards(float speed)
{
    set_left_motor_speed(speed);
    set_right_motor_speed(speed);
}

void Motors::backwards(float speed)
{
    set_left_motor_speed(-speed);
    set_right_motor_speed(-speed);
}

void Motors::turn(float speed)
{
    set_left_motor_speed(speed);
    set_right_motor_speed(-speed);
}

float Motors::get_current_left()
{
    float current = motor_vprop_l.read();
    //Vprop = 5x voltage on sense pins - sense pins have 0.25 ohm resistance - current is 0.8 actual voltage measured [or 2.64 x scaled value]
    return current * 2.64f;
}

float Motors::get_current_right()
{
    float current = motor_vprop_r.read();
    return current * 2.64f;
}

float Motors::get_adjusted_speed(float speed_in)
{
    float ret_speed = speed_in;
    if(USE_STALL_OFFSET == 1) {
        ret_speed += STALL_OFFSET;
        ret_speed /= (STALL_OFFSET + 1.0f);
    }
    return ret_speed;
}

void Motors::sleep()
{
    nsleep = 0;
    left_motor_speed = 0;
    right_motor_speed = 0;
}

void Motors::wake_up()
{
    nsleep = 1;
}

void Motors::coast_left()
{
    enable_l = 0;
    left_motor_speed = 0;
}

void Motors::brake_left()
{
    hb1_l = 1;
    hb2_l = 1;
    enable_l = 1;
    left_motor_speed = 0;
}

void Motors::set_left_motor_speed(float speed)
{
    if(speed == 0) {
        left_motor_speed = 0;
        enable_l = 0;
    } else {
        if(speed < 0) {
            hb1_l = 1;
            hb2_l = 0;
            left_motor_speed = -get_adjusted_speed(-speed);
            enable_l = -left_motor_speed;
        } else {
            hb1_l = 0;
            hb2_l = 1;
            left_motor_speed = get_adjusted_speed(speed);
            enable_l = left_motor_speed;
        }
    }
}

void Motors::coast_right()
{
    enable_r = 0;
    right_motor_speed = 0;
}

void Motors::brake_right()
{
    hb1_r = 1;
    hb2_r = 1;
    enable_r = 1;
    right_motor_speed = 0;
}

void Motors::coast()
{
    coast_left();
    coast_right();
}

void Motors::brake()
{
    brake_left();
    brake_right();
}

void Motors::set_right_motor_speed(float speed)
{
    if(speed == 0) {
        right_motor_speed = 0;
        enable_r = 0;
    } else {
        if(speed < 0) {
            hb1_r = 1;
            hb2_r = 0;
            right_motor_speed = -get_adjusted_speed(-speed);
            enable_r = -right_motor_speed;
        } else {
            hb1_r = 0;
            hb2_r = 1;
            right_motor_speed = get_adjusted_speed(speed);
            enable_r = right_motor_speed;
        }
    }
}

void Motors::set_pwm_period(int period_in)
{
    motor_pwm_period = period_in;
    enable_l.period_us(motor_pwm_period);
    enable_r.period_us(motor_pwm_period);
}

void Motors::init()
{
    if(motor_pwm_period == 0) motor_pwm_period = MOTOR_PWM_PERIOD_US;
    enable_l.period_us(motor_pwm_period);
    enable_r.period_us(motor_pwm_period);
    enable_l = 0;
    enable_r = 0;
    wake_up();
}