Ryan Mulligan / m3pi

m3pi library with fixes for left and right motor specific functions and additional functionality like play

m3pi.cpp@1:66b56d54336e, 2011-06-02 (annotated)

Committer:
ryantm
Date:
Thu Jun 02 19:15:43 2011 +0000
Revision:
1:66b56d54336e
Parent:
0:a6a3ac2e729d 
improves play comment

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryantm 0:a6a3ac2e729d 1 /* m3pi Library
ryantm 0:a6a3ac2e729d 2 *
ryantm 0:a6a3ac2e729d 3 * Copyright (c) 2007-2010 cstyles
ryantm 0:a6a3ac2e729d 4 *
ryantm 0:a6a3ac2e729d 5 * Permission is hereby granted, free of charge, to any person obtaining a copy
ryantm 0:a6a3ac2e729d 6 * of this software and associated documentation files (the "Software"), to deal
ryantm 0:a6a3ac2e729d 7 * in the Software without restriction, including without limitation the rights
ryantm 0:a6a3ac2e729d 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
ryantm 0:a6a3ac2e729d 9 * copies of the Software, and to permit persons to whom the Software is
ryantm 0:a6a3ac2e729d 10 * furnished to do so, subject to the following conditions:
ryantm 0:a6a3ac2e729d 11 *
ryantm 0:a6a3ac2e729d 12 * The above copyright notice and this permission notice shall be included in
ryantm 0:a6a3ac2e729d 13 * all copies or substantial portions of the Software.
ryantm 0:a6a3ac2e729d 14 *
ryantm 0:a6a3ac2e729d 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
ryantm 0:a6a3ac2e729d 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
ryantm 0:a6a3ac2e729d 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
ryantm 0:a6a3ac2e729d 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
ryantm 0:a6a3ac2e729d 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
ryantm 0:a6a3ac2e729d 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
ryantm 0:a6a3ac2e729d 21 * THE SOFTWARE.
ryantm 0:a6a3ac2e729d 22 */
ryantm 0:a6a3ac2e729d 23
ryantm 0:a6a3ac2e729d 24 #include "mbed.h"
ryantm 0:a6a3ac2e729d 25 #include "m3pi.h"
ryantm 0:a6a3ac2e729d 26
ryantm 0:a6a3ac2e729d 27 m3pi::m3pi(PinName nrst, PinName tx, PinName rx) : Stream("m3pi"), _nrst(nrst), _ser(tx, rx) {
ryantm 0:a6a3ac2e729d 28 _ser.baud(115200);
ryantm 0:a6a3ac2e729d 29 reset();
ryantm 0:a6a3ac2e729d 30 }
ryantm 0:a6a3ac2e729d 31
ryantm 0:a6a3ac2e729d 32 m3pi::m3pi() : Stream("m3pi"), _nrst(p23), _ser(p9, p10) {
ryantm 0:a6a3ac2e729d 33 _ser.baud(115200);
ryantm 0:a6a3ac2e729d 34 reset();
ryantm 0:a6a3ac2e729d 35 }
ryantm 0:a6a3ac2e729d 36
ryantm 0:a6a3ac2e729d 37
ryantm 0:a6a3ac2e729d 38 void m3pi::reset () {
ryantm 0:a6a3ac2e729d 39 _nrst = 0;
ryantm 0:a6a3ac2e729d 40 wait (0.01);
ryantm 0:a6a3ac2e729d 41 _nrst = 1;
ryantm 0:a6a3ac2e729d 42 wait (0.1);
ryantm 0:a6a3ac2e729d 43 }
ryantm 0:a6a3ac2e729d 44
ryantm 0:a6a3ac2e729d 45 void m3pi::left_motor (float speed) {
ryantm 0:a6a3ac2e729d 46 motor(0,speed);
ryantm 0:a6a3ac2e729d 47 }
ryantm 0:a6a3ac2e729d 48
ryantm 0:a6a3ac2e729d 49 void m3pi::right_motor (float speed) {
ryantm 0:a6a3ac2e729d 50 motor(1,speed);
ryantm 0:a6a3ac2e729d 51 }
ryantm 0:a6a3ac2e729d 52
ryantm 0:a6a3ac2e729d 53 void m3pi::forward (float speed) {
ryantm 0:a6a3ac2e729d 54 motor(0,speed);
ryantm 0:a6a3ac2e729d 55 motor(1,speed);
ryantm 0:a6a3ac2e729d 56 }
ryantm 0:a6a3ac2e729d 57
ryantm 0:a6a3ac2e729d 58 void m3pi::backward (float speed) {
ryantm 0:a6a3ac2e729d 59 motor(0,-1.0*speed);
ryantm 0:a6a3ac2e729d 60 motor(1,-1.0*speed);
ryantm 0:a6a3ac2e729d 61 }
ryantm 0:a6a3ac2e729d 62
ryantm 0:a6a3ac2e729d 63 void m3pi::left (float speed) {
ryantm 0:a6a3ac2e729d 64 motor(0,speed);
ryantm 0:a6a3ac2e729d 65 motor(1,-1.0*speed);
ryantm 0:a6a3ac2e729d 66 }
ryantm 0:a6a3ac2e729d 67
ryantm 0:a6a3ac2e729d 68 void m3pi::right (float speed) {
ryantm 0:a6a3ac2e729d 69 motor(0,-1.0*speed);
ryantm 0:a6a3ac2e729d 70 motor(1,speed);
ryantm 0:a6a3ac2e729d 71 }
ryantm 0:a6a3ac2e729d 72
ryantm 0:a6a3ac2e729d 73 void m3pi::stop (void) {
ryantm 0:a6a3ac2e729d 74 motor(0,0.0);
ryantm 0:a6a3ac2e729d 75 motor(1,0.0);
ryantm 0:a6a3ac2e729d 76 }
ryantm 0:a6a3ac2e729d 77
ryantm 0:a6a3ac2e729d 78 void m3pi::motor (int motor, float speed) {
ryantm 0:a6a3ac2e729d 79 char opcode = 0x0;
ryantm 0:a6a3ac2e729d 80 if (speed > 0.0) {
ryantm 0:a6a3ac2e729d 81 if (motor==1)
ryantm 0:a6a3ac2e729d 82 opcode = M1_FORWARD;
ryantm 0:a6a3ac2e729d 83 else
ryantm 0:a6a3ac2e729d 84 opcode = M2_FORWARD;
ryantm 0:a6a3ac2e729d 85 } else {
ryantm 0:a6a3ac2e729d 86 if (motor==1)
ryantm 0:a6a3ac2e729d 87 opcode = M1_BACKWARD;
ryantm 0:a6a3ac2e729d 88 else
ryantm 0:a6a3ac2e729d 89 opcode = M2_BACKWARD;
ryantm 0:a6a3ac2e729d 90 }
ryantm 0:a6a3ac2e729d 91 unsigned char arg = 0x7f * abs(speed);
ryantm 0:a6a3ac2e729d 92
ryantm 0:a6a3ac2e729d 93 _ser.putc(opcode);
ryantm 0:a6a3ac2e729d 94 _ser.putc(arg);
ryantm 0:a6a3ac2e729d 95 }
ryantm 0:a6a3ac2e729d 96
ryantm 0:a6a3ac2e729d 97 float m3pi::battery() {
ryantm 0:a6a3ac2e729d 98 _ser.putc(SEND_BATTERY_MILLIVOLTS);
ryantm 0:a6a3ac2e729d 99 char lowbyte = _ser.getc();
ryantm 0:a6a3ac2e729d 100 char hibyte = _ser.getc();
ryantm 0:a6a3ac2e729d 101 float v = ((lowbyte + (hibyte << 8))/1000.0);
ryantm 0:a6a3ac2e729d 102 return(v);
ryantm 0:a6a3ac2e729d 103 }
ryantm 0:a6a3ac2e729d 104
ryantm 0:a6a3ac2e729d 105 float m3pi::line_position() {
ryantm 0:a6a3ac2e729d 106 int pos = 0;
ryantm 0:a6a3ac2e729d 107 _ser.putc(SEND_LINE_POSITION);
ryantm 0:a6a3ac2e729d 108 pos = _ser.getc();
ryantm 0:a6a3ac2e729d 109 pos += _ser.getc() << 8;
ryantm 0:a6a3ac2e729d 110
ryantm 0:a6a3ac2e729d 111 float fpos = ((float)pos - 2048.0)/2048.0;
ryantm 0:a6a3ac2e729d 112 return(fpos);
ryantm 0:a6a3ac2e729d 113 }
ryantm 0:a6a3ac2e729d 114
ryantm 0:a6a3ac2e729d 115 char m3pi::sensor_auto_calibrate() {
ryantm 0:a6a3ac2e729d 116 _ser.putc(AUTO_CALIBRATE);
ryantm 0:a6a3ac2e729d 117 return(_ser.getc());
ryantm 0:a6a3ac2e729d 118 }
ryantm 0:a6a3ac2e729d 119
ryantm 0:a6a3ac2e729d 120
ryantm 0:a6a3ac2e729d 121 void m3pi::calibrate(void) {
ryantm 0:a6a3ac2e729d 122 _ser.putc(PI_CALIBRATE);
ryantm 0:a6a3ac2e729d 123 }
ryantm 0:a6a3ac2e729d 124
ryantm 0:a6a3ac2e729d 125 void m3pi::reset_calibration() {
ryantm 0:a6a3ac2e729d 126 _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
ryantm 0:a6a3ac2e729d 127 }
ryantm 0:a6a3ac2e729d 128
ryantm 0:a6a3ac2e729d 129 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) {
ryantm 0:a6a3ac2e729d 130 _ser.putc(SET_PID);
ryantm 0:a6a3ac2e729d 131 _ser.putc(max_speed);
ryantm 0:a6a3ac2e729d 132 _ser.putc(a);
ryantm 0:a6a3ac2e729d 133 _ser.putc(b);
ryantm 0:a6a3ac2e729d 134 _ser.putc(c);
ryantm 0:a6a3ac2e729d 135 _ser.putc(d);
ryantm 0:a6a3ac2e729d 136 }
ryantm 0:a6a3ac2e729d 137
ryantm 0:a6a3ac2e729d 138 void m3pi::PID_stop() {
ryantm 0:a6a3ac2e729d 139 _ser.putc(STOP_PID);
ryantm 0:a6a3ac2e729d 140 }
ryantm 0:a6a3ac2e729d 141
ryantm 0:a6a3ac2e729d 142 float m3pi::pot_voltage(void) {
ryantm 0:a6a3ac2e729d 143 int volt = 0;
ryantm 0:a6a3ac2e729d 144 _ser.putc(SEND_TRIMPOT);
ryantm 0:a6a3ac2e729d 145 volt = _ser.getc();
ryantm 0:a6a3ac2e729d 146 volt += _ser.getc() << 8;
ryantm 0:a6a3ac2e729d 147 return(volt);
ryantm 0:a6a3ac2e729d 148 }
ryantm 0:a6a3ac2e729d 149
ryantm 0:a6a3ac2e729d 150
ryantm 0:a6a3ac2e729d 151 void m3pi::leds(int val) {
ryantm 0:a6a3ac2e729d 152
ryantm 0:a6a3ac2e729d 153 BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
ryantm 0:a6a3ac2e729d 154 _leds = val;
ryantm 0:a6a3ac2e729d 155 }
ryantm 0:a6a3ac2e729d 156
ryantm 0:a6a3ac2e729d 157
ryantm 0:a6a3ac2e729d 158 void m3pi::locate(int x, int y) {
ryantm 0:a6a3ac2e729d 159 _ser.putc(DO_LCD_GOTO_XY);
ryantm 0:a6a3ac2e729d 160 _ser.putc(x);
ryantm 0:a6a3ac2e729d 161 _ser.putc(y);
ryantm 0:a6a3ac2e729d 162 }
ryantm 0:a6a3ac2e729d 163
ryantm 0:a6a3ac2e729d 164 void m3pi::cls(void) {
ryantm 0:a6a3ac2e729d 165 _ser.putc(DO_CLEAR);
ryantm 0:a6a3ac2e729d 166 }
ryantm 0:a6a3ac2e729d 167
ryantm 0:a6a3ac2e729d 168 int m3pi::print (char* text, int length) {
ryantm 0:a6a3ac2e729d 169 _ser.putc(DO_PRINT);
ryantm 0:a6a3ac2e729d 170 _ser.putc(length);
ryantm 0:a6a3ac2e729d 171 for (int i = 0 ; i < length ; i++) {
ryantm 0:a6a3ac2e729d 172 _ser.putc(text[i]);
ryantm 0:a6a3ac2e729d 173 }
ryantm 0:a6a3ac2e729d 174 return(0);
ryantm 0:a6a3ac2e729d 175 }
ryantm 0:a6a3ac2e729d 176
ryantm 0:a6a3ac2e729d 177 void m3pi::play (char* music, int length) {
ryantm 1:66b56d54336e 178 // The default 3pi serial slave program
ryantm 1:66b56d54336e 179 // will not accept music strings longer than 100.
ryantm 0:a6a3ac2e729d 180 if (length > 100)
ryantm 0:a6a3ac2e729d 181 length = 100;
ryantm 1:66b56d54336e 182 _ser.putc(DO_PLAY);
ryantm 0:a6a3ac2e729d 183 _ser.putc(length);
ryantm 0:a6a3ac2e729d 184 for (int i = 0 ; i < length ; i++) {
ryantm 0:a6a3ac2e729d 185 _ser.putc(music[i]);
ryantm 0:a6a3ac2e729d 186 }
ryantm 0:a6a3ac2e729d 187 }
ryantm 0:a6a3ac2e729d 188
ryantm 0:a6a3ac2e729d 189 int m3pi::_putc (int c) {
ryantm 0:a6a3ac2e729d 190 _ser.putc(DO_PRINT);
ryantm 0:a6a3ac2e729d 191 _ser.putc(0x1);
ryantm 0:a6a3ac2e729d 192 _ser.putc(c);
ryantm 0:a6a3ac2e729d 193 wait (0.001);
ryantm 0:a6a3ac2e729d 194 return(c);
ryantm 0:a6a3ac2e729d 195 }
ryantm 0:a6a3ac2e729d 196
ryantm 0:a6a3ac2e729d 197 int m3pi::_getc (void) {
ryantm 0:a6a3ac2e729d 198 char r = 0;
ryantm 0:a6a3ac2e729d 199 return(r);
ryantm 0:a6a3ac2e729d 200 }
ryantm 0:a6a3ac2e729d 201
ryantm 0:a6a3ac2e729d 202 int m3pi::putc (int c) {
ryantm 0:a6a3ac2e729d 203 return(_ser.putc(c));
ryantm 0:a6a3ac2e729d 204 }
ryantm 0:a6a3ac2e729d 205
ryantm 0:a6a3ac2e729d 206 int m3pi::getc (void) {
ryantm 0:a6a3ac2e729d 207 return(_ser.getc());
ryantm 0:a6a3ac2e729d 208 }
ryantm 0:a6a3ac2e729d 209
ryantm 0:a6a3ac2e729d 210
ryantm 0:a6a3ac2e729d 211
ryantm 0:a6a3ac2e729d 212
ryantm 0:a6a3ac2e729d 213 #ifdef MBED_RPC
ryantm 0:a6a3ac2e729d 214 const rpc_method *m3pi::get_rpc_methods() {
ryantm 0:a6a3ac2e729d 215 static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> },
ryantm 0:a6a3ac2e729d 216 { "backward", rpc_method_caller<m3pi, float, &m3pi::backward> },
ryantm 0:a6a3ac2e729d 217 { "left", rpc_method_caller<m3pi, float, &m3pi::left> },
ryantm 0:a6a3ac2e729d 218 { "right", rpc_method_caller<m3pi, float, &m3pi::right> },
ryantm 0:a6a3ac2e729d 219 { "stop", rpc_method_caller<m3pi, &m3pi::stop> },
ryantm 0:a6a3ac2e729d 220 { "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> },
ryantm 0:a6a3ac2e729d 221 { "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> },
ryantm 0:a6a3ac2e729d 222 { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> },
ryantm 0:a6a3ac2e729d 223 { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> },
ryantm 0:a6a3ac2e729d 224 { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> },
ryantm 0:a6a3ac2e729d 225
ryantm 0:a6a3ac2e729d 226
ryantm 0:a6a3ac2e729d 227 RPC_METHOD_SUPER(Base)
ryantm 0:a6a3ac2e729d 228 };
ryantm 0:a6a3ac2e729d 229 return rpc_methods;
ryantm 0:a6a3ac2e729d 230 }
ryantm 0:a6a3ac2e729d 231 #endif