Library for the m3pi robot. This works with a Pololu 3pi robot with the Serial Slave firmware, and exposes and API.
Fork of m3pi by
Revision 9:e0904a2c1bc0, committed 2016-11-23
- Comitter:
- jpxu
- Date:
- Wed Nov 23 09:00:13 2016 +0000
- Parent:
- 8:4b7d6ea9b35b
- Commit message:
- labyrinth
Changed in this revision
m3pi.cpp | Show annotated file Show diff for this revision Revisions of this file |
m3pi.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/m3pi.cpp Thu May 12 13:26:37 2011 +0000 +++ b/m3pi.cpp Wed Nov 23 09:00:13 2016 +0000 @@ -20,61 +20,61 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ - + #include "mbed.h" #include "m3pi.h" - + m3pi::m3pi(PinName nrst, PinName tx, PinName rx) : Stream("m3pi"), _nrst(nrst), _ser(tx, rx) { _ser.baud(115200); reset(); } - + m3pi::m3pi() : Stream("m3pi"), _nrst(p23), _ser(p9, p10) { _ser.baud(115200); reset(); } - - + + void m3pi::reset () { _nrst = 0; wait (0.01); _nrst = 1; wait (0.1); } - + void m3pi::left_motor (float speed) { + motor(1,speed); +} + +void m3pi::right_motor (float speed) { motor(0,speed); } - -void m3pi::right_motor (float speed) { - motor(1,speed); -} - + void m3pi::forward (float speed) { motor(0,speed); motor(1,speed); } - + void m3pi::backward (float speed) { motor(0,-1.0*speed); motor(1,-1.0*speed); } - + void m3pi::left (float speed) { motor(0,speed); motor(1,-1.0*speed); } - + void m3pi::right (float speed) { motor(0,-1.0*speed); motor(1,speed); } - + void m3pi::stop (void) { motor(0,0.0); motor(1,0.0); } - + void m3pi::motor (int motor, float speed) { char opcode = 0x0; if (speed > 0.0) { @@ -89,11 +89,11 @@ opcode = M2_BACKWARD; } unsigned char arg = 0x7f * abs(speed); - + _ser.putc(opcode); _ser.putc(arg); } - + float m3pi::battery() { _ser.putc(SEND_BATTERY_MILLIVOLTS); char lowbyte = _ser.getc(); @@ -101,7 +101,7 @@ float v = ((lowbyte + (hibyte << 8))/1000.0); return(v); } - + float m3pi::line_position() { int pos = 0; _ser.putc(SEND_LINE_POSITION); @@ -111,21 +111,30 @@ float fpos = ((float)pos - 2048.0)/2048.0; return(fpos); } - + +void m3pi::calibrated_sensors(unsigned short ltab[5]) { + unsigned i; + _ser.putc(SEND_CALIB_SENSOR_VALUES); + for(i=0;i<5;i++){ + ltab[i] = (unsigned short) _ser.getc(); + ltab[i] += _ser.getc() << 8; + } +} + char m3pi::sensor_auto_calibrate() { _ser.putc(AUTO_CALIBRATE); return(_ser.getc()); } - - + + void m3pi::calibrate(void) { _ser.putc(PI_CALIBRATE); } - + void m3pi::reset_calibration() { _ser.putc(LINE_SENSORS_RESET_CALIBRATION); } - + void m3pi::PID_start(int max_speed, int a, int b, int c, int d) { _ser.putc(max_speed); _ser.putc(a); @@ -133,11 +142,11 @@ _ser.putc(c); _ser.putc(d); } - + void m3pi::PID_stop() { _ser.putc(STOP_PID); } - + float m3pi::pot_voltage(void) { int volt = 0; _ser.putc(SEND_TRIMPOT); @@ -145,25 +154,25 @@ volt += _ser.getc() << 8; return(volt); } - - + + void m3pi::leds(int val) { - + BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13); _leds = val; } - - + + void m3pi::locate(int x, int y) { _ser.putc(DO_LCD_GOTO_XY); _ser.putc(x); _ser.putc(y); } - + void m3pi::cls(void) { _ser.putc(DO_CLEAR); } - + int m3pi::print (char* text, int length) { _ser.putc(DO_PRINT); _ser.putc(length); @@ -172,7 +181,7 @@ } return(0); } - + int m3pi::_putc (int c) { _ser.putc(DO_PRINT); _ser.putc(0x1); @@ -180,24 +189,24 @@ wait (0.001); return(c); } - + int m3pi::_getc (void) { char r = 0; return(r); } - + int m3pi::putc (int c) { return(_ser.putc(c)); } - + int m3pi::getc (void) { return(_ser.getc()); } - - - - - + + + + + #ifdef MBED_RPC const rpc_method *m3pi::get_rpc_methods() { static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> }, @@ -210,10 +219,10 @@ { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> }, { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> }, { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> }, - - + + RPC_METHOD_SUPER(Base) }; return rpc_methods; } -#endif +#endif \ No newline at end of file
--- a/m3pi.h Thu May 12 13:26:37 2011 +0000 +++ b/m3pi.h Wed Nov 23 09:00:13 2016 +0000 @@ -19,19 +19,20 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ - + #ifndef M3PI_H #define M3PI_H - + #include "mbed.h" #include "platform.h" - + #ifdef MBED_RPC #include "rpc.h" #endif - + #define SEND_SIGNATURE 0x81 #define SEND_RAW_SENSOR_VALUES 0x86 +#define SEND_CALIB_SENSOR_VALUES 0x87 #define SEND_TRIMPOT 0xB0 #define SEND_BATTERY_MILLIVOLTS 0xB1 #define DO_PLAY 0xB3 @@ -48,24 +49,24 @@ #define M1_BACKWARD 0xC2 #define M2_FORWARD 0xC5 #define M2_BACKWARD 0xC6 - - - + + + /** m3pi control class * * Example: * @code * // Drive the m3pi forward, turn left, back, turn right, at half speed for half a second - + #include "mbed.h" #include "m3pi.h" - + m3pi pi; - + int main() { - + wait(0.5); - + pi.forward(0.5); wait (0.5); pi.left(0.5); @@ -74,17 +75,17 @@ wait (0.5); pi.right(0.5); wait (0.5); - + pi.stop(); - + } * @endcode */ class m3pi : public Stream { - + // Public functions public: - + /** Create the m3pi object connected to the default pins * * @param nrst GPIO pin used for reset. Default is p23 @@ -92,70 +93,70 @@ * @param rx Serial receive pin. Default is p10 */ m3pi(); - - + + /** Create the m3pi object connected to specific pins * */ m3pi(PinName nrst, PinName tx, PinName rx); - - - + + + /** Force a hardware reset of the 3pi */ void reset (void); - + /** Directly control the speed and direction of the left motor * * @param speed A normalised number -1.0 - 1.0 represents the full range. */ void left_motor (float speed); - + /** Directly control the speed and direction of the right motor * * @param speed A normalised number -1.0 - 1.0 represents the full range. */ void right_motor (float speed); - + /** Drive both motors forward as the same speed * * @param speed A normalised number 0 - 1.0 represents the full range. */ void forward (float speed); - + /** Drive both motors backward as the same speed * * @param speed A normalised number 0 - 1.0 represents the full range. */ void backward (float speed); - + /** Drive left motor backwards and right motor forwards at the same speed to turn on the spot * * @param speed A normalised number 0 - 1.0 represents the full range. */ void left (float speed); - + /** Drive left motor forward and right motor backwards at the same speed to turn on the spot * @param speed A normalised number 0 - 1.0 represents the full range. */ void right (float speed); - + /** Stop both motors * */ void stop (void); - + /** Read the voltage of the potentiometer on the 3pi * @returns voltage as a float * */ float pot_voltage(void); - + /** Read the battery voltage on the 3pi * @returns battery voltage as a float */ float battery(void); - + /** Read the position of the detected line * @returns position as A normalised number -1.0 - 1.0 represents the full range. * -1.0 means line is on the left, or the line has been lost @@ -163,74 +164,79 @@ * 1.0 means the line is on the right */ float line_position (void); - - + /** lecture capteurs calibres + * @returns tableau val capteurs de gauche à droite + * 0 blanc (reflexion max) + * 1000 noir (pas de reflexion) + */ + void calibrated_sensors(unsigned short ltab[5]); + /** Calibrate the sensors. This turns the robot left then right, looking for a line * */ char sensor_auto_calibrate (void); - + /** Set calibration manually to the current settings. * */ void calibrate(void); - + /** Clear the current calibration settings * */ void reset_calibration (void); - + void PID_start(int max_speed, int a, int b, int c, int d); - + void PID_stop(); - + /** Write to the 8 LEDs * * @param leds An 8 bit value to put on the LEDs */ void leds(int val); - + /** Locate the cursor on the 8x2 LCD * * @param x The horizontal position, from 0 to 7 * @param y The vertical position, from 0 to 1 */ void locate(int x, int y); - + /** Clear the LCD * */ void cls(void); - + /** Send a character directly to the 3pi serial interface * @param c The character to send to the 3pi */ int putc(int c); - + /** Receive a character directly to the 3pi serial interface * @returns c The character received from the 3pi */ int getc(); - + /** Send a string buffer to the 3pi serial interface * @param text A pointer to a char array * @param int The character to send to the 3pi */ int print(char* text, int length); - + #ifdef MBED_RPC virtual const struct rpc_method *get_rpc_methods(); #endif - + private : - + DigitalOut _nrst; Serial _ser; void motor (int motor, float speed); virtual int _putc(int c); virtual int _getc(); - + }; - + #endif \ No newline at end of file