File content as of revision 0:8a2dd255c508:

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

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

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

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

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

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

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

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