Giles Barton-Owen / Mbed 2 deprecated MbedRanger

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

Dependencies:   mbed

m3pi/m3pi.cpp@0:342c14fb10c0, 2012-03-14 (annotated)

Committer:
p07gbar
Date:
Wed Mar 14 20:31:09 2012 +0000
Revision:
0:342c14fb10c0
First upload

Who changed what in which revision?

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