![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Allows the M3Pi to be used as a Sumo robot, using the sharp 100 distance sensors on the front. Hunter strategy
Revision 0:a29bcf098632, committed 2012-06-18
- Comitter:
- jonmarsh
- Date:
- Mon Jun 18 09:27:10 2012 +0000
- Commit message:
- 1st pass - not tested
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SharpDigiDist100/SharpDigiDist100.cpp Mon Jun 18 09:27:10 2012 +0000 @@ -0,0 +1,56 @@ +#include "SharpDigiDist100.h" + +SharpDigiDist100::SharpDigiDist100(PinName pin):intin(pin),pinin(pin) +{ +timer1.reset(); +intin.rise(this,&SharpDigiDist100::onInt); +intin.fall(this,&SharpDigiDist100::onInt); +timer1.start(); +//onInt(); +current = Far; +//last = NA; +onChangeAttached = false; +} + +void SharpDigiDist100::onInt() +{ + timer1.stop(); + last = current; + int timeFromLast = timer1.read_ms(); + if (timeFromLast < 100) + { + current = Far; + timeout.attach(this,&SharpDigiDist100::onInt, 0.3); + } + else + { + if(pinin) + { + current = Mid; + } + else + { + current = Near; + } + } + if(current != last) + { + if(onChangeAttached) + { + onChange(); + } + } + timer1.reset(); + timer1.start(); +} + +int SharpDigiDist100::getDistance() +{ + return current; +} + +void SharpDigiDist100::attachOnChange(void (*ptr) (void)) +{ + onChange = ptr; + onChangeAttached = true; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SharpDigiDist100/SharpDigiDist100.h Mon Jun 18 09:27:10 2012 +0000 @@ -0,0 +1,75 @@ +#ifndef SHARPDIGIDIST100_H +#define SHARPDIGIDIST100_H + +#include "mbed.h" + +/** A class which interfaces with a Sharp Digital Distance sensor (GP2Y0D810) + * + * Example: + * @code + * + * @endcode + * + */ + +class SharpDigiDist100 +{ +public: + +/** Create a sensor input + * + * @param pin The pin the output of the sensor is connected to + */ + +SharpDigiDist100(PinName pin); + +/** The enum which makes up the output + * + */ + +enum Distance +{ + Near = 1, + Mid, + Far +}; + +/** Returns the distace as an enum + * + * @return The distance code: 1 is near, 2 is middle distance and 3 is far + */ + +int getDistance(); + +/** Attaches a function which is called on distance change + * + * @param A pointer to a function with params/returns: void func(void) + */ + +void attachOnChange(void (*ptr) (void)); + + +protected: + +InterruptIn intin; + +DigitalIn pinin; + +Timer timer1; + +enum Distance current; + +enum Distance last; + +void onInt(); + +Timeout timeout; + +void (*onChange) (void); + +bool onChangeAttached; + +//DigitalOut Debug; +}; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pi/m3pi.cpp Mon Jun 18 09:27:10 2012 +0000 @@ -0,0 +1,284 @@ +/* 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(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::get_raw_sensors(int* values) { + _ser.putc(SEND_RAW_SENSOR_VALUES); + for (int i = 0; i<5; i++) { + while(_ser.readable() == 0){} + values[i] = _ser.getc(); + while(_ser.readable() == 0){} + values[i] += _ser.getc() << 8; + //values[i] -= raw_white_levels[i]; + } +} + + +void m3pi::get_calibrated_sensors(float* values) { + int temp[5]; + _ser.putc(SEND_CALIBRATED_SENSOR_VALUES); + for (int i = 0; i<5; i++) { + temp[i] = _ser.getc(); + temp[i] += _ser.getc() << 8; + values[i] = float(temp[i]) / 1000; + } +} + +void m3pi::get_white_levels() { + get_raw_sensors(raw_white_levels); +} + +int m3pi::is_line() +{ + int ret = 0; + int edgeCount = 0; + int temp[5]; + get_raw_sensors(temp); + for(int i = 0; i <5 ; i++) + { + if(temp[i] - raw_white_levels[i]>= LINE_THRESHOLD) + { + ret++; + if(i == 0|| i== 4) + { + edgeCount--; + } + else + { + edgeCount++; + } + } + + } + + if(edgeCount >=0 && ret>0) + { + return 1; + } + else if(edgeCount <0 && ret>0) + { + return -1; + } + else + { + 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()); +} + + + + + +#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pi/m3pi.h Mon Jun 18 09:27:10 2012 +0000 @@ -0,0 +1,270 @@ +/* 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 + + +#define LINE_THRESHOLD 100 + + +/** 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 + * + */ + m3pi(); + + + + + /** Create the m3pi object connected to the spesific 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(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); + + /** Get the raw sensor values + * + * @param int* A pointer to a interger array + */ + void get_raw_sensors(int* values); + + /** Get the calibrated sensor values + * + * @param int* A pointer to an float array + */ + void get_calibrated_sensors(float* values); + + /** Records current levels as white, values used to detect a line + * + */ + void get_white_levels(); + + /** Gets the line state + * + * @returns The line status: 0 means no line, -1 means line at back two sensors, 1 means line at front sensors + */ + + int is_line(); + + + /** 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(); + + int raw_white_levels[5]; + +}; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jun 18 09:27:10 2012 +0000 @@ -0,0 +1,180 @@ +/** +Name: mpi hunter +Description: + +Based on work done by Giles Barton-Owen +Cut down for use at Linton Village College + +This program keeps a m3pi inside a ring and looks for the other robot + +**/ + +#include "mbed.h" +#include "SharpDigiDist100.h" +#include "m3pi.h" + +DigitalOut Left[2] = {LED1,LED2}; //Some indicator LEDs for the range finders +DigitalOut Right[2] = {LED4,LED3}; + +SharpDigiDist100 right(p30); // The range finder class initialisations +SharpDigiDist100 left(p11); + +m3pi m3pi; // Initialise the m3pi + +InterruptIn button(p21); // SW1 on the shield, for strategy switching + +Ticker guidance; // The main guidance caller + +Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging + +int previousLine; // A set of variables to sort out the line detection, previousLine is pretty much not used +int isLine; +int lineCatch; + +Timeout liner; // A timeout to stop it ignoring the line + +void CycleMode(); // Function defs +void guideCall(); +void clearLiner(); + + +int main() { + guidance.attach(&guideCall,0.1); // Sets up the control loop + + m3pi.locate(0,0); // Write the name to the screen + m3pi.printf("m3PiRngr"); + + m3pi.get_white_levels(); // Saves the current levels of the sensors to know what is white + + while (1) { + + switch (right.getDistance()) { // Sets up the distance indicator LEDs for the right side + case SharpDigiDist100::Far : + Right[0] = true; + Right[1] = false; + break; + case SharpDigiDist100::Near : + Right[1] = true; + Right[0] = false; + break; + case SharpDigiDist100::Mid : + Right[0] = true; + Right[1] = true; + break; + default: + break; + } + switch (left.getDistance()) { // Sets up the distance indicator LEDs for the left side + case SharpDigiDist100::Far : + Left[0] = true; + Left[1] = false; + break; + case SharpDigiDist100::Near : + Left[1] = true; + Left[0] = false; + break; + case SharpDigiDist100::Mid : + Left[0] = true; + Left[1] = true; + break; + default: + break; + } +} +} +void guideCall() { + + isLine = m3pi.is_line(); // Gets whether the m3pi is on a line, and if so front/back + + if (lineCatch == 0) { // Has it been off a line for long enough? + isLine = isLine; // Yes - then go ahead + } else { + isLine = lineCatch; // No - pretend to still be on that line + + } + float position; + + switch (isLine) { + case 0: // No line, not even recently so go ahead with the strategies + { + bool atRight = false; + bool atLeft = false; + + // Runs forward until something is really close + + if (right.getDistance() == SharpDigiDist100::Mid || right.getDistance() == SharpDigiDist100::Near) { + atRight = true; + } else atRight = false; + if (left.getDistance() == SharpDigiDist100::Mid || left.getDistance() == SharpDigiDist100::Near) { + atLeft = true; + } else atLeft = false; + + if (atRight && atLeft) { + m3pi.stop(); + } else { + if (atRight == true) { + m3pi.left_motor(0.1); + m3pi.right_motor(0.2); + } else { + if (atLeft == true) { + m3pi.left_motor(0.2); + m3pi.right_motor(0.1); + } else { + m3pi.forward(0.3); + } + } + }} + break; + case 1: // Line in front, reverse + if (lineCatch == 0) { + lineCatch = 1; + + liner.attach(&clearLiner, 0.3); + } + + position = m3pi.line_position(); + if (position < 0) { + m3pi.left_motor(-1); + m3pi.right_motor(-0.8); + } else if (position == 0) { + m3pi.backward(1); + } else if (position > 0) { + m3pi.left_motor(-0.8); + m3pi.right_motor(-1); + } + + //m3pi.locate(0,1); + //m3pi.printf("LINE_FWD"); + + + break; + + case -1: // Line behind, forward + + if (lineCatch == 0) { + lineCatch = -1; + liner.attach(&clearLiner, 0.3); + } + + + position = m3pi.line_position(); + if (position < 0) { + m3pi.left_motor(1); + m3pi.right_motor(0.8); + } else if (position == 0) { + m3pi.forward(1); + } else if (position > 0) { + m3pi.left_motor(0.8); + m3pi.right_motor(1); + } + + break; + } + + //previousLine = isLine; +} + +void clearLiner() { // Gets called a bit after a line is detected + lineCatch = 0; +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jun 18 09:27:10 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912