Mike Panetta / m3pi

Modifid library for m3pi

Dependents:   BeaconAvoid

m3pi.cpp@0:1ce8bf1f4452, 2012-09-09 (annotated)

Committer:
mpanetta
Date:
Sun Sep 09 22:28:04 2012 +0000
Revision:
0:1ce8bf1f4452
[mbed] converted /BeaconAvoid/m3pi

Who changed what in which revision?

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