Jon Marsh / Mbed 2 deprecated m3pi_runner

Allows the M3Pi to be used as a Sumo robot, using the sharp 100 distance sensors on the front. Run away strategy

Dependencies:   mbed

m3pi/m3pi.cpp@0:11d0f3e0d1ad, 2012-06-18 (annotated)

Committer:
jonmarsh
Date:
Mon Jun 18 09:38:17 2012 +0000
Revision:
0:11d0f3e0d1ad
Not tested

Who changed what in which revision?

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