Library for Pololu m3pi line-following robot. Implements the serial slave commands. for MBED OS V6
m3pi.cpp
- Committer:
- eencae
- Date:
- 2017-04-07
- Revision:
- 2:26bf14f4dc84
- Parent:
- 1:5523d6d1feec
- Child:
- 3:5015bc2d1cf8
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); }