Mike Panetta
/
FirstTest
Second revision of test code for Polyathalon sensor board.
Revision 0:13a7de7ce046, committed 2011-11-05
- Comitter:
- mpanetta
- Date:
- Sat Nov 05 18:29:39 2011 +0000
- Child:
- 1:1e37eee8e1cf
- Commit message:
- First revision of test code for Polyathalon sensor board.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODDMA.lib Sat Nov 05 18:29:39 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/MODDMA/#cb10aec6feb1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Quaternion/Quaternion.h Sat Nov 05 18:29:39 2011 +0000 @@ -0,0 +1,45 @@ +/** + * @file Quaternion.h + * @author Mike Panetta + * + * Created Oct 29th 2011 + * + * Based on tutorial here: http://www.cprogramming.com/tutorial/3d/quaternions.html + * + */ + +#ifndef _QUATERNION_H_ +#define _QUATERNION_H_ + +#include <math.h> + +class Quaternion +{ + public: + Quaternion(float const & a, + float const & b, + float const & c, + float const & d) : w(a), x(b), y(c), z(d) {} + + float norm(void) + { + return sqrtf(w*w+x*x+y*y+z*z); + } + + float real(void) + { + return w; + } + + Quaternion unreal(void) + { + return Quaternion(0, x, y, z); + } + + private: + + float w, x, y, z; +}; + + +#endif //_QUATERNION_H)_ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pi/m3pi.cpp Sat Nov 05 18:29:39 2011 +0000 @@ -0,0 +1,238 @@ +/* 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); +} + +const m3pi::LineSensors m3pi::get_raw_sensors(void) { + static LineSensors sensors; + + _ser.putc(SEND_RAW_SENSOR_VALUES); + + sensors.sensor1 = _ser.getc(); + sensors.sensor1 |= _ser.getc() << 8; + sensors.sensor2 = _ser.getc(); + sensors.sensor2 |= _ser.getc() << 8; + sensors.sensor3 = _ser.getc(); + sensors.sensor3 |= _ser.getc() << 8; + sensors.sensor4 = _ser.getc(); + sensors.sensor4 |= _ser.getc() << 8; + sensors.sensor5 = _ser.getc(); + sensors.sensor5 |= _ser.getc() << 8; + + return sensors; +} + +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); +} + +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 Sat Nov 05 18:29:39 2011 +0000 @@ -0,0 +1,247 @@ +/* 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 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 + * 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); + + struct LineSensors { + uint16_t sensor1; + uint16_t sensor2; + uint16_t sensor3; + uint16_t sensor4; + uint16_t sensor5; + }; + + const LineSensors get_raw_sensors(void); + + +#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Nov 05 18:29:39 2011 +0000 @@ -0,0 +1,75 @@ +#include "mbed.h" +#include "m3pi.h" +#include "tlc5916.h" + +#define DARK_THRESHOLD 0x1A0 + + + +AnalogIn in1(p15); +AnalogIn in2(p16); +AnalogIn in3(p17); +AnalogIn in4(p18); +AnalogIn in5(p19); +AnalogIn in6(p20); + +DigitalOut led1(LED1); +DigitalOut led2(LED2); + +m3pi base; +TLC5916 tlc5916(p6, p5, p7, p11); + +int main() { + + struct m3pi::LineSensors sensors; + uint16_t val1 = 0; + uint16_t val2 = 0; + uint8_t leds = 0x01; + + base.reset(); + wait(1); + + base.locate(0, 0); + base.printf("AIN: "); + + while (1) { + if (leds == 0x80) + leds = 0x01; + tlc5916.write_reg(leds); + leds = leds << 1; + tlc5916.enable(); + wait_us(25); + led1 = 1; + val1 = in4.read_u16(); + val2 = in5.read_u16(); + led1 = 0; + tlc5916.disable(); + led2 = 1; + base.locate(0, 0); + base.printf("AIN:%4.4x", val1); + base.locate(0, 1); + base.printf("AIN:%4.4x", leds); + led2 = 0; + } + +#if 0 + base.sensor_auto_calibrate(); + + base.locate(0,0); + base.printf("Sens 3:"); + + + while(1) { + + sensors = base.get_raw_sensors(); + base.locate(0,1); + base.printf("0x%4.4x", sensors.sensor3); + if (sensors.sensor3 > DARK_THRESHOLD) { + base.right_motor(0.2); + base.left_motor(-0.2); + } else { + base.forward(0.2); + } + } +#endif +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Nov 05 18:29:39 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tlc5916.cpp Sat Nov 05 18:29:39 2011 +0000 @@ -0,0 +1,26 @@ + +#include "mbed.h" +#include "tlc5916.h" + +void +TLC5916::write_reg(uint8_t byte) +{ + pinLE = 0; + spi.write(byte); + wait_us(1); + pinLE = 1; + wait_us(1); + pinLE = 0; +} + +void +TLC5916::enable(void) +{ + pinnOE = 0; +} + +void +TLC5916::disable(void) +{ + pinnOE = 1; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tlc5916.h Sat Nov 05 18:29:39 2011 +0000 @@ -0,0 +1,28 @@ +#ifndef _TLC5916_H_ +#define _TLC5916_H_ + +class TLC5916 { + +public: + TLC5916(PinName le, PinName sin, PinName clk, PinName nOE) : pinLE(le), pinnOE(nOE), spi(sin, NC, clk) + { + pinLE = 0; + pinnOE = 1; + + spi.format(8,0); + spi.frequency(1000000); + }; + + void write_reg(uint8_t byte); + void enable(void); + void disable(void); + +private: + + DigitalOut pinLE; + DigitalOut pinnOE; + SPI spi; + +}; + +#endif //_TLC5916_H_ \ No newline at end of file