![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is work in progress - maze solving with m3pi robot. Not complete
Revision 1:3ac7462953df, committed 2011-03-03
- Comitter:
- jonmarsh
- Date:
- Thu Mar 03 10:18:49 2011 +0000
- Parent:
- 0:6e52eae6412a
- Commit message:
- With renamed m3pimaze library
Changed in this revision
--- a/m3pijon/m3pijon.cpp Thu Mar 03 09:58:58 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,232 +0,0 @@ -/* 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 "m3pijon.h" - -m3pi::m3pi(PinName nrst, PinName tx, PinName rx) : Stream("m3pi"), _nrst(nrst), _ser(tx, rx), _leds(p20,p19,p18,p17,p16,p15,p14,p13) { - _leds = 0; - _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(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) { - _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); -} - -int m3pi::playtune (char* text, int length) { - _ser.putc(DO_PLAY); - _ser.putc(length); - for (int i = 0 ; i < length ; i++) { - _ser.putc(text[i]); - } - return(0); -} -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()); -} - -void m3pi::readsensor (int *sensor){ - - _ser.putc(SEND_CALIBRATED_SENSOR_VALUES); - sensor[0] = _ser.getc(); - sensor[0] += _ser.getc() << 8; - sensor[1] = _ser.getc(); - sensor[1] += _ser.getc() << 8; - sensor[2] = _ser.getc(); - sensor[2] += _ser.getc() << 8; - sensor[3] = _ser.getc(); - sensor[3] += _ser.getc() << 8; - sensor[4] = _ser.getc(); - sensor[4] += _ser.getc() << 8; - - return; -} -#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
--- a/m3pijon/m3pijon.h Thu Mar 03 09:58:58 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,248 +0,0 @@ -/* 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_CALIBRATED_SENSOR_VALUES 0x87 -#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 "m3pimusic.h" - - m3pi pi(p8,p9,p10); - - 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 p8 - * @param tx Serial transmit pin. Default is p9 - * @param rx Serial receive pin. Default is p10 - */ - 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. - * - */ - float line_position (void); - - - /** Calibrate the sensors. This turns the robot left then right, loking 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); - - /** Play a tune - */ - int playtune(char* text, int length); - - /** - * Read the calibrated value of a sensor - * @param sensor Pointer to array sensor [0-4] - * @param values returned in the range 0-1000 where 1000=completely dark - */ - void readsensor(int *sensor); - -#ifdef MBED_RPC - virtual const struct rpc_method *get_rpc_methods(); -#endif - -private : - - DigitalOut _nrst; - Serial _ser; - BusOut _leds; - - void motor (int motor, float speed); - virtual int _putc(int c); - virtual int _getc(); - -}; - -#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pimaze.lib Thu Mar 03 10:18:49 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/jonmarsh/code/m3pimaze/#e1dac380452c
--- a/main.cpp Thu Mar 03 09:58:58 2011 +0000 +++ b/main.cpp Thu Mar 03 10:18:49 2011 +0000 @@ -1,5 +1,5 @@ #include "mbed.h" -#include "m3pijon.h" +#include "m3pimaze.h" BusOut leds(LED1,LED2,LED3,LED4); m3pi m3pi(p23,p9,p10);