Thomas Glatzel / m3pi_ng

Library for the m3pi robot. This works with a Pololu 3pi robot with the Serial Slave firmware, and exposes and API. This is a fork of cstyles m3pi library.

Dependents:   newlib PID Robot WarehouseBot1 ... more

Fork of m3pi_ng by Nikolas Goldin

m3pi_ng.cpp@10:f89d2a3a9ed2, 2013-05-14 (annotated)

Committer:
ngoldin
Date:
Tue May 14 11:05:33 2013 +0000
Revision:
10:f89d2a3a9ed2
Parent:
9:deb9547909c3
Child:
11:2761054a8926
Fixed my additional methods. Constructing the array inside the function led to memory overrun in the long run. Now, they expect to be given a pointer to somewhere to write.

Who changed what in which revision?

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