Library for Pololu m3pi line-following robot. Implements the serial slave commands.
Dependents: 3pi_Example_2 3pi_Lab1_Task2_Example1 3pi_Lab2_Task1_Example1 3pi_Line_Follow ... more
m3pi.cpp
- Committer:
- eencae
- Date:
- 2018-03-19
- Revision:
- 9:074ce6197b51
- Parent:
- 8:92167bd3eb44
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); }