valerio rizzo / Mbed 2 deprecated m3pi_object-avoider

Line follower and object avoider

Dependencies:   mbed SHARPIR

m3pi/m3pi.cpp@0:df7d60d7a6ee, 2012-04-12 (annotated)

Committer:
nepol77
Date:
Thu Apr 12 14:37:54 2012 +0000
Revision:
0:df7d60d7a6ee
Hi guys, i will put the pictures on very soon!! Thanks for looking

Who changed what in which revision?

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