File content as of revision 3:8762f6b2ea8d:

#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;

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){
        hb1_l = 0;
        hb2_l = 1;
        left_motor_speed = -get_adjusted_speed(-speed);
        enable_l = -left_motor_speed;
    }else{
        hb1_l = 1;
        hb2_l = 0;
        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){
        hb1_r = 0;
        hb2_r = 1;
        right_motor_speed = -get_adjusted_speed(-speed);
        enable_r = -right_motor_speed;
    }else{
        hb1_r = 1;
        hb2_r = 0;
        right_motor_speed = get_adjusted_speed(speed);
        enable_r = right_motor_speed;   
    }   
}


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