Jon Marsh / Mbed 2 deprecated m3pi_hunter

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

Dependencies:   mbed

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

Committer:
jonmarsh
Date:
Mon Jun 18 09:27:10 2012 +0000
Revision:
0:a29bcf098632
1st pass - not tested

Who changed what in which revision?

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