File content as of revision 2:26bf14f4dc84:

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

    _last_line_position = 0.0;

}

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
}

/////////////////////////////// serial slave commands ////////////////////////////////


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

void m3pi::get_raw_values(unsigned int *values)
{
    char vals[10];                     // array to receive 10 byte return message
    _serial->putc(0x86);                // send command

    int n=0;
    while ( _serial->readable() ) {     // keep looping while data on rx line
        vals[n] = _serial->getc();      // read into array
        n++;                            // increment index
    }

    for(int i=0; i<5; i++) {            // construct the 2-byte values
        values[i] = (vals[2*i+1] << 8) | vals[2*i];
    }
}

void m3pi::get_calibrated_values(unsigned int *values)
{
    char vals[10];                      // array to receive 10 byte return message
    _serial->putc(0x87);                // send command

    int n=0;
    while ( _serial->readable() ) {     // keep looping while data on rx line
        vals[n] = _serial->getc();      // read into array
        n++;                            // increment index
    }

    for(int i=0; i<5; i++) {            // construct the 2-byte values
        values[i] = (vals[2*i+1] << 8) | vals[2*i];
    }

}

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(0.5);

    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(0.5);

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

    spin_right(0.2);

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

    while (timer.read() < 5.0) {

        write_leds(led_val++);

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

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

    timer.stop();

    write_leds(255);
    stop();

    lcd_clear();
    lcd_goto_xy(0,0);
    lcd_print("  Done  ",8);

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

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

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

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

    char lsb = _serial->getc();
    char msb = _serial->getc();
    int position = (msb<<8 | lsb);

    return float(position - 2000)/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
}

/////////////////////////////// motor methods ////////////////////////////////


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

void m3pi::display_sensor_values(unsigned int values[],int y)
{
    // initialise array to ASCII '0'
    char bin[5]= {'0','0','0','0','0'};

    // loop through and if above threshold then sent to ASCII '1'
    for (int i=0; i<5; i++) {
        if (values[i] > 500) {
            bin[i] = '1';
        }
    }

    lcd_goto_xy(2,y);
    lcd_print(bin,5);

}

unsigned int m3pi::get_sensor_array_value(unsigned int values[])
{

    unsigned int value = 0;

    // loop through each bit, starting from PC4
    for (int i = 4; i >= 0; i--) {

        unsigned int weight = pow(2.0,4-i);

        // check if over threshold
        if (values[i] > 500) {
            // add equivalent binary weight to value
            value += weight;
        }

    }

    return value;
}

float m3pi::calc_line_position(unsigned int values[])
{
    // calculate weighted average
    unsigned int value =
        (0*values[0]+1e3*values[1]+2e3*values[2]+3e3*values[3]+4e3*values[4])/
        (values[0]+values[1]+values[2]+values[3]+values[4]);

    // scale to between -1.0 and 1.0
    float position = (int(value) - 2000)/2000.0;

    float is_on_line = false;
    write_leds(0x0);

    // loop through and check if any sensor reading is above the threshold
    for (int i = 0; i<5; i++) {
        if (values[i] > 500) {
            is_on_line = true;
            write_leds(0xFF);
        }
    }

    // update last line position if over line
    if (is_on_line) {
        _last_line_position = position;
    }

    // if not on line then the last line position will have the last value when over line
    return _last_line_position;
}

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

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