Jon Marsh / Mbed 2 deprecated m3pi_attack

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

Dependencies:   mbed

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

Committer:
jonmarsh
Date:
Mon Jun 18 09:54:54 2012 +0000
Revision:
0:dda4072af5ff
not tested

Who changed what in which revision?

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