File content as of revision 9:074ce6197b51:

#include "m3pi.h"

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


m3pi::m3pi()
{
    _serial = new Serial(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->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)
{
    _serial->putc(0x81);
    _serial->gets(signature,7);
}

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

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

    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)
{
    while (_serial->readable() ) {  // flush buffer
     _serial->getc();   
    }
    
    char vals[10];                      // array to receive 10 byte return message
    _serial->putc(0x87);                // send command
        
    for (int i=0; i < 10; i++) {
        vals[i] =  _serial->getc();   
    }
    
    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()
{
    _serial->putc(0xB4);
}

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

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()
{
    _serial->putc(0xBA);
    
    while(1) {  // wait for serial response
        if (_serial->readable()) {
            break;
        }
    }
}

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

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

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'
    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);
    wait_ms(100);
    _reset->write(0);
    wait_ms(100);
}