Diff: m3pi.cpp

Revision:

0:56320ef879a6

Child:

1:5523d6d1feec

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m3pi.cpp	Tue Apr 04 08:08:16 2017 +0000
@@ -0,0 +1,364 @@
+#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);
+}
\ No newline at end of file