James Allen / Mbed 2 deprecated MyMazeSolver

working modified m3pimazesolver

Dependencies:   mbed

m3pimaze.cpp@2:73f7cc4c30c7, 2013-04-06 (annotated)

Committer:
jalle1714
Date:
Sat Apr 06 15:22:01 2013 +0000
Revision:
2:73f7cc4c30c7
Parent:
0:05dee1d3d857 
Added playtune to play a melody at the end of pressed function; Put in a one second pause after resistor pullup to prevent from jumping to pressed function before button pressed

Who changed what in which revision?

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