File content as of revision 10:c3617fcf2c58:

#include "m3pi.h"

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


m3pi::m3pi()
{
    _serial = new BufferedSerial(p9,p10);
    _reset = new DigitalOut(p8);
    _last_line_position = 0.0;

    // initialise the arrays
    _bar_graph[0] = ' ';
    for (int i = 0; i < 6; i++) {
        _bar_graph[i+1] = i;
    }
    for (int i = 0; i < 5; i++) {
        _values[i]=0;
    }
}

m3pi::~m3pi()
{
    delete _serial;
    delete _reset;

}

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

void m3pi::init()
{
    _serial->set_baud(115200);
    reset();                        // hard rest of 3pi
    stop();                         // stop motors
    lcd_clear();                    // clear LCD
}

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

void m3pi::scan()
{
    get_calibrated_values(_values);
}

void m3pi::get_signature(char *signature)
{
    char buff[7];
    //_serial->putc(0x81);
    buff[1]=0x81;
    _serial->write(buff,1);
    //_serial->gets(signature,7);
    _serial->read(buff,7);
}

void m3pi::get_raw_values(unsigned int *values)
{
        char buff[1];                       // send command
        
    while (_serial->readable() ) {  // flush buffer
        //_serial->getc();
        _serial->sync();
        
    }

    char vals[10];                     // array to receive 10 byte return message

    //_serial->putc(0x86);
    buff[0]=0x86;
    _serial->write(buff,1);

    for (int i=0; i < 10; i++) {
        //vals[i] =  _serial->getc();
        _serial->read(buff,1);
        vals[i] = buff[0];
    }

    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 buff[1];

    while (_serial->readable() ) {  // flush buffer
        //_serial->getc();
        _serial->sync();
    }

    char vals[10];                      // array to receive 10 byte return message
    //_serial->putc(0x87);                // send command
    buff[0]=0x87;
    _serial->write(buff,1);

    for (int i=0; i < 10; i++) {
        //vals[i] =  _serial->getc();
        _serial->read(buff,1);
        vals[i] = buff[0];
    }

    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()
{
    char buff[1];

    //_serial->putc(0xB0);
    buff[0]=0xB0;
    _serial->write(buff,1);

    char lsb;// = _serial->getc();
    _serial->read(buff,1);
    lsb = buff[0];
    char msb;// = _serial->getc();
        _serial->read(buff,1);
    msb = buff[0];
    // trimpot value in the range 0 - 1023
    float value = ( msb<<8 | lsb ) / 1023.0;
    return value;
}


float m3pi::get_battery_voltage()
{
    char buff[1];
    //_serial->putc(0xB1);
    buff[0]=0xB1;
    _serial->write(buff,1);
    char lsb;// = _serial->getc();
        _serial->read(buff,1);
    lsb = buff[0];
    char msb;// = _serial->getc();
        _serial->read(buff,1);
    msb = buff[0];
    // 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)
{
    char buff[1];

    length = length < 0 ? 0 : length;
    length = length > 100 ? 100 : length;

    //_serial->putc(0xB3);
    buff[0]=0xB3;
    _serial->write(buff,1);

    //_serial->putc(length);
    buff[0]=length;
    _serial->write(buff,1);

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

void m3pi::calibrate()
{
    char buff[1];
    //_serial->putc(0xB4);
    buff[0]=0xB4;
    _serial->write(buff,1);
}

void m3pi::reset_calibration()
{
    char buff[1];
    //_serial->putc(0xB5);
    buff[0]=0xB5;
    _serial->write(buff,1);
}

float m3pi::get_line_position()
{
    char buff[1];
    //_serial->putc(0xB6);
    buff[0]=0xB6;
    _serial->write(buff,1);

    char lsb;// = _serial->getc();
        _serial->read(buff,1);
    lsb = buff[0];
    char msb;// = _serial->getc();
        _serial->read(buff,1);
    msb = buff[0];
    int position = (msb<<8 | lsb);

    return float(position - 2000)/2000.0;
}

void m3pi::lcd_clear()
{
    char buff[1];
    //_serial->putc(0xB7);
    buff[0]=0xB7;
    _serial->write(buff,1);
}

void m3pi::lcd_print(char text[],int length)
{
    char buff[1];

    length = length < 0 ? 0 : length;
    length = length > 8 ? 8 : length;

    //_serial->putc(0xB8);
    buff[0]=0xB8;
    _serial->write(buff,1);

    //_serial->putc(length);
    buff[0]=length;
    _serial->write(buff,1);

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

void m3pi::lcd_goto_xy(int x, int y)
{
    char buff[1];
    //_serial->putc(0xB9);
        buff[0]=0xB9;
    _serial->write(buff,1);
    //_serial->putc(x);
        buff[0]=x;
    _serial->write(buff,1);
   //_serial->putc(y);
        buff[0]=y;
    _serial->write(buff,1);
}

void m3pi::auto_calibrate()
{
    char buff[1];
    //_serial->putc(0xBA);
        buff[0]=0xBA;
    _serial->write(buff,1);

    while(1) {  // wait for serial response
        if (_serial->readable()) {
            break;
        }
    }
}

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

void m3pi::left_motor(float speed)
{
    char buff[1];
    // 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);
            buff[0]=0xC1;
    _serial->write(buff,1);
        char s = char(127.0*speed);
        //_serial->putc(s);
            buff[0]=s;
    _serial->write(buff,1);
    } else { // backward - speed is negative
        //_serial->putc(0xC2);
            buff[0]=0xC2;
    _serial->write(buff,1);
        char s = char(-127.0*speed);
        //_serial->putc(s);
            buff[0]=s;
    _serial->write(buff,1);
    }

}

void m3pi::right_motor(float speed)
{
    char buff[1];
    // 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);
            buff[0]=0xC5;
    _serial->write(buff,1);
        char s = char(127.0*speed);
        //_serial->putc(s);
            buff[0]=s;
    _serial->write(buff,1);
    } else { // backward - speed is negative
        //_serial->putc(0xC6);
            buff[0]=0xC6;
    _serial->write(buff,1);
        char s = char(-127.0*speed);
        //_serial->putc(s);
            buff[0]=s;
    _serial->write(buff,1);
    }

}

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

////////////////////////////////////////////////////////////////////////////////

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)
{
    char buffer[7]; // including NULL terminator
    
    //_serial->putc(0x81);
        buffer[0]=0x81;
    _serial->write(buffer,1);
    
    
    //_serial->gets(buffer,7);
    _serial->read(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'
    lcd_goto_xy(1,y);

    char sensor_values[5];

    // loop through sensor
    for (int sensor = 0 ; sensor < 5 ; sensor++) {
        // get the value and put it in the correct bin
        // (7 bins in the range 0 to 1000
        char value = char(values[sensor]/(1000.0/7.0));
        // use the bin to select the bar graph icon to display
        sensor_values[sensor] = _bar_graph[value];
    }

    lcd_print(sensor_values,5);

}

void m3pi::display_data()
{
    display_sensor_values(_values,1);

    char buffer[8]= {0};
    sprintf(buffer,"% .3f",_last_line_position);
    lcd_goto_xy(0,0);
    lcd_print(buffer,6);

}

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;

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

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

float m3pi::read_line()
{
    return calc_line_position(_values);
}



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

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