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
Diff: m3pi.cpp
- Revision:
- 2:26bf14f4dc84
- Parent:
- 1:5523d6d1feec
- Child:
- 3:5015bc2d1cf8
--- a/m3pi.cpp Wed Apr 05 10:25:42 2017 +0000 +++ b/m3pi.cpp Fri Apr 07 18:22:35 2017 +0000 @@ -9,6 +9,9 @@ _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() @@ -40,6 +43,9 @@ _button->mode(PullUp); // turn pull-up on } +/////////////////////////////// serial slave commands //////////////////////////////// + + void m3pi::get_signature(char *signature) { _serial->putc(0x81); @@ -76,7 +82,7 @@ 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() @@ -123,7 +129,7 @@ lcd_goto_xy(0,1); lcd_print(" line ",8); - wait(1.0); + wait(0.5); lcd_clear(); lcd_goto_xy(0,0); @@ -135,7 +141,7 @@ // loop while waiting for button to be press } - wait(1.0); + wait(0.5); lcd_clear(); lcd_goto_xy(0,0); @@ -149,7 +155,8 @@ Timer timer; timer.start(); - while (timer.read() < 10.0) { + while (timer.read() < 5.0) { + write_leds(led_val++); if (led_val > 255) { @@ -175,7 +182,7 @@ lcd_clear(); write_leds(0); - wait(1.0); + wait(0.5); } void m3pi::reset_calibration() @@ -184,22 +191,15 @@ wait_ms(50); } -int m3pi::get_line_position() +float 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; + return float(position - 2000)/2000.0; } void m3pi::lcd_clear() @@ -242,6 +242,9 @@ write_leds(0); // LEDs off } +/////////////////////////////// motor methods //////////////////////////////// + + void m3pi::left_motor(float speed) { // check within bounds @@ -314,7 +317,6 @@ void m3pi::spin_right(float speed) { - speed = speed > 1.0 ? 1.0 : speed; speed = speed < 0.0 ? 0.0 : speed; @@ -324,7 +326,6 @@ void m3pi::spin_left(float speed) { - speed = speed > 1.0 ? 1.0 : speed; speed = speed < 0.0 ? 0.0 : speed; @@ -332,6 +333,9 @@ right_motor(speed); } +//////////////////////////////////////////////////////////////////////////////// + + int m3pi::read_button() { return _button->read(); @@ -358,6 +362,74 @@ 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()