File content as of revision 0:56320ef879a6:

#include "m3pi.h"

////////////////////////// constructor/destructor //////////////////////////////


m3pi::m3pi()
{
    _serial = new Serial(p9,p10);
    _reset = new DigitalOut(p23);
    _button = new DigitalIn(p21);
    _leds = new BusOut(p20,p19,p18,p17,p16,p15,p14,p13);
}

m3pi::~m3pi()
{
    delete _serial;
    delete _reset;
    delete _button;
    delete _leds;
}

/////////////////////////////// public methods /////////////////////////////////

void m3pi::write_leds(int val)
{
    // check within limits
    val = val > 255 ? 255 : val;
    val = val < 0 ? 0 : val;

    _leds->write(val);
}

void m3pi::init()
{
    _serial->baud(115200);
    reset();                        // hard rest of 3pi
    stop();                         // stop motors
    lcd_clear();                    // clear LCD
    write_leds(0);                  // turn off LEDs
    _button->mode(PullUp);           // turn pull-up on
}

void m3pi::get_signature(char *signature)
{
    _serial->putc(0x81);
    _serial->gets(signature,7);
}

void m3pi::get_raw_values(unsigned int *values)
{
    _serial->putc(0x86);

    for (int i=0; i<5; i++) {
        char lsb = _serial->getc();
        char msb = _serial->getc();
        unsigned int value = (msb<<8 | lsb);
        values[i]=value;
    }
}

void m3pi::get_calibrated_values(unsigned int *values)
{
    _serial->putc(0x87);

    for (int i=0; i<5; i++) {
        char lsb = _serial->getc();
        char msb = _serial->getc();
        unsigned int value = (msb<<8 | lsb);
        values[i]=value;
    }

}

float m3pi::get_trimpot_value()
{
    _serial->putc(0xB0);
    char lsb = _serial->getc();
    char msb = _serial->getc();
    // trimpot value in the range 0 - 1023
    float value = ( msb<<8 | lsb ) / 1023.0;
    return value;
}


float m3pi::get_battery_voltage()
{
    _serial->putc(0xB1);
    char lsb = _serial->getc();
    char msb = _serial->getc();
    // Battery in mV so convert to volts
    float voltage = ( msb<<8 | lsb ) / 1000.0;
    return voltage;
}

void m3pi::play_music(const char notes[],int length)
{
    length = length < 0 ? 0 : length;
    length = length > 100 ? 100 : length;

    _serial->putc(0xB3);
    _serial->putc(length);

    for (int i = 0 ; i < length ; i++) {
        _serial->putc(notes[i]);
    }
}


void m3pi::calibrate()
{
    reset_calibration();

    lcd_goto_xy(0,0);
    lcd_print("Place on",8);
    lcd_goto_xy(0,1);
    lcd_print("  line  ",8);

    wait(1.0);

    lcd_clear();
    lcd_goto_xy(0,0);
    lcd_print(" Press  ",8);
    lcd_goto_xy(0,1);
    lcd_print("to begin",8);

    while( read_button() ) {
        // loop while waiting for button to be press
    }

    wait(1.0);

    lcd_clear();
    lcd_goto_xy(0,0);
    lcd_print("Reading ",8);
    lcd_goto_xy(0,1);
    lcd_print("Sensors ",8);

    spin_right(0.1);

    char led_val = 0;
    Timer timer;
    timer.start();

    while (timer.read() < 10.0) {
        write_leds(led_val++);

        if (led_val > 255) {
            led_val = 0;
        }

        _serial->putc(0xB4);
        wait_ms(5);
    }

    timer.stop();

    write_leds(255);
    stop();

    lcd_clear();
    lcd_goto_xy(0,0);
    lcd_print("Place on",8);
    lcd_goto_xy(0,1);
    lcd_print("  line  ",8);

    while( read_button() ) {
        // loop while waiting for button to be press
    }

    lcd_clear();
    write_leds(0);
    wait(1.0);
}

void m3pi::reset_calibration()
{
    _serial->putc(0xB5);
    wait_ms(50);
}

int m3pi::get_line_position()
{
    _serial->putc(0xB6);

    // order is different from claimed in User Guide?
    char lsb = _serial->getc();
    char msb = _serial->getc();
    int position = (msb<<8 | lsb);

    return position - 2000;
}

float m3pi::get_normalised_line_position()
{
    int position = get_line_position();
    return float(position)/2000.0;
}

void m3pi::lcd_clear()
{
    _serial->putc(0xB7);
}

void m3pi::lcd_print(char text[],int length)
{
    length = length < 0 ? 0 : length;
    length = length > 8 ? 8 : length;

    _serial->putc(0xB8);
    _serial->putc(length);

    for (int i = 0 ; i < length ; i++) {
        _serial->putc(text[i]);
    }
}

void m3pi::lcd_goto_xy(int x, int y)
{
    _serial->putc(0xB9);
    _serial->putc(x);
    _serial->putc(y);

}

void m3pi::auto_calibrate()
{
    reset_calibration();            // clear previous calibration

    _serial->putc(0xBA);
    write_leds(0xFF);  // LEDs on
    while(1) {
        if (_serial->readable()) {
            break;
        }
    }
    write_leds(0);    // LEDs off
}

void m3pi::left_motor(float speed)
{
    // check within bounds
    speed = speed > 1.0 ? 1.0 : speed;
    speed = speed < -1.0 ? -1.0 : speed;

    if (speed > 0.0) {  // forward
        _serial->putc(0xC1);
        char s = char(127.0*speed);
        _serial->putc(s);
    } else { // backward - speed is negative
        _serial->putc(0xC2);
        char s = char(-127.0*speed);
        _serial->putc(s);
    }

}

void m3pi::right_motor(float speed)
{
    // check within bounds
    speed = speed > 1.0 ? 1.0 : speed;
    speed = speed < -1.0 ? -1.0 : speed;

    if (speed > 0.0) {  // forward
        _serial->putc(0xC5);
        char s = char(127.0*speed);
        _serial->putc(s);
    } else { // backward - speed is negative
        _serial->putc(0xC6);
        char s = char(-127.0*speed);
        _serial->putc(s);
    }

}

// speeds from -1.0 to 1.0 (0 is stop)
void m3pi::motors(float left_speed,float right_speed)
{
    left_motor(left_speed);
    right_motor(right_speed);
}


void m3pi::stop()
{
    left_motor(0.0);
    right_motor(0.0);
}

// speed in range 0.0 to 1.0
void m3pi::forward(float speed)
{
    speed = speed > 1.0 ? 1.0 : speed;
    speed = speed < 0.0 ? 0.0 : speed;

    left_motor(speed);
    right_motor(speed);
}

// speed in range 0 to 1.0
void m3pi::reverse(float speed)
{
    speed = speed > 1.0 ? 1.0 : speed;
    speed = speed < 0.0 ? 0.0 : speed;

    left_motor(-speed);
    right_motor(-speed);
}

void m3pi::spin_right(float speed)
{

    speed = speed > 1.0 ? 1.0 : speed;
    speed = speed < 0.0 ? 0.0 : speed;

    left_motor(speed);
    right_motor(-speed);
}

void m3pi::spin_left(float speed)
{

    speed = speed > 1.0 ? 1.0 : speed;
    speed = speed < 0.0 ? 0.0 : speed;

    left_motor(-speed);
    right_motor(speed);
}

int m3pi::read_button()
{
    return _button->read();
}

void m3pi::display_battery_voltage(int x,int y)
{
    float voltage = get_battery_voltage();

    char buffer[8];
    sprintf(buffer,"%3.1f V",voltage);

    lcd_goto_xy(x,y);
    lcd_print(buffer,5);
}

void m3pi::display_signature(int x,int y)
{
    _serial->putc(0x81);
    char buffer[7];  // including NULL terminator
    _serial->gets(buffer,7);

    lcd_goto_xy(x,y);
    lcd_print(buffer,6);
}

/////////////////////////////// private methods ////////////////////////////////

void m3pi::reset()
{
    // pulse the reset line (active-high)
    _reset->write(0);
    wait_ms(100);
    _reset->write(1);
    wait_ms(100);
}