Dependents:   LPC1768_m3pi_LineFollower_PID

Files at this revision

API Documentation at this revision

Comitter:
donde
Date:
Tue Nov 15 02:14:45 2011 +0000
Commit message:

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
diff -r 000000000000 -r 4d063fa5d347 m3pi.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m3pi.cpp	Tue Nov 15 02:14:45 2011 +0000
@@ -0,0 +1,231 @@
+/* m3pi Library
+ *
+ * Copyright (c) 2007-2010 cstyles
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * 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(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) {
+        if (motor==1)
+            opcode = M1_FORWARD;
+        else
+            opcode = M2_FORWARD;
+    } else {
+        if (motor==1)
+            opcode = M1_BACKWARD;
+        else
+            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();
+    char hibyte  = _ser.getc();
+    float v = ((lowbyte + (hibyte << 8))/1000.0);
+    return(v);
+}
+
+float m3pi::line_position() {
+    int pos = 0;
+    _ser.putc(SEND_LINE_POSITION);
+    pos = _ser.getc();
+    pos += _ser.getc() << 8;
+    
+    float fpos = ((float)pos - 2048.0)/2048.0;
+    return(fpos);
+}
+
+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(SET_PID);
+    _ser.putc(max_speed);
+    _ser.putc(a);
+    _ser.putc(b);
+    _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);
+    volt = _ser.getc();
+    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);       
+    for (int i = 0 ; i < length ; i++) {
+        _ser.putc(text[i]); 
+    }
+    return(0);
+}
+
+void m3pi::play (char* music, int length) {
+    // The default 3pi serial slave program
+    // will not accept music strings longer than 100.
+    if (length > 100)
+        length = 100;
+    _ser.putc(DO_PLAY);
+    _ser.putc(length);
+    for (int i = 0 ; i < length ; i++) {
+        _ser.putc(music[i]); 
+    }
+}
+
+int m3pi::_putc (int c) {
+    _ser.putc(DO_PRINT);  
+    _ser.putc(0x1);       
+    _ser.putc(c);         
+    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> },
+        { "backward", rpc_method_caller<m3pi, float, &m3pi::backward> },
+        { "left", rpc_method_caller<m3pi, float, &m3pi::left> },
+        { "right", rpc_method_caller<m3pi, float, &m3pi::right> },
+        { "stop", rpc_method_caller<m3pi, &m3pi::stop> },
+        { "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> },
+        { "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> },
+        { "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
diff -r 000000000000 -r 4d063fa5d347 m3pi.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m3pi.h	Tue Nov 15 02:14:45 2011 +0000
@@ -0,0 +1,244 @@
+/* mbed m3pi Library
+ * Copyright (c) 2007-2010 cstyles
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * 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_TRIMPOT 0xB0
+#define SEND_BATTERY_MILLIVOLTS 0xB1
+#define DO_PLAY 0xB3
+#define PI_CALIBRATE 0xB4
+#define DO_CLEAR 0xB7
+#define DO_PRINT 0xB8
+#define DO_LCD_GOTO_XY 0xB9
+#define LINE_SENSORS_RESET_CALIBRATION 0xB5
+#define SEND_LINE_POSITION 0xB6
+#define AUTO_CALIBRATE 0xBA
+#define SET_PID 0xBB
+#define STOP_PID 0xBC
+#define M1_FORWARD 0xC1
+#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);
+     wait (0.5);
+     pi.backward(0.5);
+     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
+     * @param tx Serial transmit pin. Default is p9
+     * @param rx Serial receive pin. Default is p10
+     */
+    m3pi();
+
+
+    /** Create the m3pi object connected to the default 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
+     *   0.0 means the line is in the middle
+     *   1.0 means the line is on the right
+     */
+    float line_position (void);
+
+
+    /** 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);
+    
+    /** Send a string buffer to the 3pi serial interface for
+     * playing music. See here for details about the string's 
+     * format:
+     *   http://www.pololu.com/docs/0J18/3
+     * @param music A pointer to a char array
+     * @param length The length of the music string
+     */
+    void play(char* music, int length);
+
+#ifdef MBED_RPC
+    virtual const struct rpc_method *get_rpc_methods();
+#endif
+
+private :
+    DigitalOut _nrst;
+    
+    void motor (int motor, float speed);
+    virtual int _putc(int c);
+    virtual int _getc();
+public :
+    Serial _ser;
+};
+
+#endif
\ No newline at end of file