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@9:deb9547909c3, 2013-05-13 (annotated)

Committer:
ngoldin
Date:
Mon May 13 10:54:18 2013 +0000
Revision:
9:deb9547909c3
Child:
10:f89d2a3a9ed2
added read out calibrated sensors function

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 9:deb9547909c3 41 char * m3pi::signature () {
ngoldin 9:deb9547909c3 42 char* sig = new char[6]; //must be initialised with new, else it is destroyed when the function exits
ngoldin 9:deb9547909c3 43 _ser.putc(SEND_SIGNATURE);
ngoldin 9:deb9547909c3 44 for (int i=0; i<6;i++){
ngoldin 9:deb9547909c3 45 sig[i]=_ser.getc();
ngoldin 9:deb9547909c3 46 }
ngoldin 9:deb9547909c3 47 return sig;
ngoldin 9:deb9547909c3 48 }
ngoldin 9:deb9547909c3 49
ngoldin 9:deb9547909c3 50 int * m3pi::raw_sensor () {
ngoldin 9:deb9547909c3 51 _ser.putc(SEND_RAW_SENSOR_VALUES);
ngoldin 9:deb9547909c3 52 int* raw = new int[5]; //must be initialised with new, else it is destroyed when the function exits
ngoldin 9:deb9547909c3 53 for (int i=0; i<5; i++){
ngoldin 9:deb9547909c3 54 char lowbyte = _ser.getc();
ngoldin 9:deb9547909c3 55 char hibyte = _ser.getc();
ngoldin 9:deb9547909c3 56 int s = (lowbyte + (hibyte << 8));
ngoldin 9:deb9547909c3 57 raw[i] = s;
ngoldin 9:deb9547909c3 58 }
ngoldin 9:deb9547909c3 59 return(raw);
ngoldin 9:deb9547909c3 60 }
ngoldin 9:deb9547909c3 61
ngoldin 9:deb9547909c3 62 int * m3pi::calibrated_sensor () {
ngoldin 9:deb9547909c3 63 _ser.putc(SEND_CALIBRATED_SENSOR_VALUES);
ngoldin 9:deb9547909c3 64 int* cal = new int[5]; //must be initialised with new, else it is destroyed when the function exits
ngoldin 9:deb9547909c3 65 for (int i=0; i<5; i++){
ngoldin 9:deb9547909c3 66 char lowbyte = _ser.getc();
ngoldin 9:deb9547909c3 67 char hibyte = _ser.getc();
ngoldin 9:deb9547909c3 68 int s = (lowbyte + (hibyte << 8));
ngoldin 9:deb9547909c3 69 cal[i] = s;
ngoldin 9:deb9547909c3 70 }
ngoldin 9:deb9547909c3 71 return(cal);
ngoldin 9:deb9547909c3 72 }
ngoldin 9:deb9547909c3 73
ngoldin 9:deb9547909c3 74 void m3pi::playtune (char* text, int length) {
ngoldin 9:deb9547909c3 75 _ser.putc(DO_PLAY);
ngoldin 9:deb9547909c3 76 _ser.putc(length);
ngoldin 9:deb9547909c3 77 for (int i = 0 ; i < length ; i++) {
ngoldin 9:deb9547909c3 78 _ser.putc(text[i]);
ngoldin 9:deb9547909c3 79 }
ngoldin 9:deb9547909c3 80 }
ngoldin 9:deb9547909c3 81
ngoldin 9:deb9547909c3 82 void m3pi::reset () {
ngoldin 9:deb9547909c3 83 _nrst = 0;
ngoldin 9:deb9547909c3 84 wait (0.01);
ngoldin 9:deb9547909c3 85 _nrst = 1;
ngoldin 9:deb9547909c3 86 wait (0.1);
ngoldin 9:deb9547909c3 87 }
ngoldin 9:deb9547909c3 88
ngoldin 9:deb9547909c3 89 void m3pi::left_motor (float speed) {
ngoldin 9:deb9547909c3 90 motor(0,speed);
ngoldin 9:deb9547909c3 91 }
ngoldin 9:deb9547909c3 92
ngoldin 9:deb9547909c3 93 void m3pi::right_motor (float speed) {
ngoldin 9:deb9547909c3 94 motor(1,speed);
ngoldin 9:deb9547909c3 95 }
ngoldin 9:deb9547909c3 96
ngoldin 9:deb9547909c3 97 void m3pi::forward (float speed) {
ngoldin 9:deb9547909c3 98 motor(0,speed);
ngoldin 9:deb9547909c3 99 motor(1,speed);
ngoldin 9:deb9547909c3 100 }
ngoldin 9:deb9547909c3 101
ngoldin 9:deb9547909c3 102 void m3pi::backward (float speed) {
ngoldin 9:deb9547909c3 103 motor(0,-1.0*speed);
ngoldin 9:deb9547909c3 104 motor(1,-1.0*speed);
ngoldin 9:deb9547909c3 105 }
ngoldin 9:deb9547909c3 106
ngoldin 9:deb9547909c3 107 void m3pi::left (float speed) {
ngoldin 9:deb9547909c3 108 motor(0,speed);
ngoldin 9:deb9547909c3 109 motor(1,-1.0*speed);
ngoldin 9:deb9547909c3 110 }
ngoldin 9:deb9547909c3 111
ngoldin 9:deb9547909c3 112 void m3pi::right (float speed) {
ngoldin 9:deb9547909c3 113 motor(0,-1.0*speed);
ngoldin 9:deb9547909c3 114 motor(1,speed);
ngoldin 9:deb9547909c3 115 }
ngoldin 9:deb9547909c3 116
ngoldin 9:deb9547909c3 117 void m3pi::stop (void) {
ngoldin 9:deb9547909c3 118 motor(0,0.0);
ngoldin 9:deb9547909c3 119 motor(1,0.0);
ngoldin 9:deb9547909c3 120 }
ngoldin 9:deb9547909c3 121
ngoldin 9:deb9547909c3 122 void m3pi::motor (int motor, float speed) {
ngoldin 9:deb9547909c3 123 char opcode = 0x0;
ngoldin 9:deb9547909c3 124 if (speed > 0.0) {
ngoldin 9:deb9547909c3 125 if (motor==1)
ngoldin 9:deb9547909c3 126 opcode = M1_FORWARD;
ngoldin 9:deb9547909c3 127 else
ngoldin 9:deb9547909c3 128 opcode = M2_FORWARD;
ngoldin 9:deb9547909c3 129 } else {
ngoldin 9:deb9547909c3 130 if (motor==1)
ngoldin 9:deb9547909c3 131 opcode = M1_BACKWARD;
ngoldin 9:deb9547909c3 132 else
ngoldin 9:deb9547909c3 133 opcode = M2_BACKWARD;
ngoldin 9:deb9547909c3 134 }
ngoldin 9:deb9547909c3 135 unsigned char arg = 0x7f * fabs(speed);
ngoldin 9:deb9547909c3 136
ngoldin 9:deb9547909c3 137 _ser.putc(opcode);
ngoldin 9:deb9547909c3 138 _ser.putc(arg);
ngoldin 9:deb9547909c3 139 }
ngoldin 9:deb9547909c3 140
ngoldin 9:deb9547909c3 141 float m3pi::battery() {
ngoldin 9:deb9547909c3 142 _ser.putc(SEND_BATTERY_MILLIVOLTS);
ngoldin 9:deb9547909c3 143 char lowbyte = _ser.getc();
ngoldin 9:deb9547909c3 144 char hibyte = _ser.getc();
ngoldin 9:deb9547909c3 145 float v = ((lowbyte + (hibyte << 8))/1000.0);
ngoldin 9:deb9547909c3 146 return(v);
ngoldin 9:deb9547909c3 147 }
ngoldin 9:deb9547909c3 148
ngoldin 9:deb9547909c3 149 float m3pi::line_position() {
ngoldin 9:deb9547909c3 150 int pos = 0;
ngoldin 9:deb9547909c3 151 _ser.putc(SEND_LINE_POSITION);
ngoldin 9:deb9547909c3 152 pos = _ser.getc();
ngoldin 9:deb9547909c3 153 pos += _ser.getc() << 8;
ngoldin 9:deb9547909c3 154
ngoldin 9:deb9547909c3 155 float fpos = ((float)pos - 2048.0)/2048.0;
ngoldin 9:deb9547909c3 156 return(fpos);
ngoldin 9:deb9547909c3 157 }
ngoldin 9:deb9547909c3 158
ngoldin 9:deb9547909c3 159 char m3pi::sensor_auto_calibrate() {
ngoldin 9:deb9547909c3 160 _ser.putc(AUTO_CALIBRATE);
ngoldin 9:deb9547909c3 161 return(_ser.getc());
ngoldin 9:deb9547909c3 162 }
ngoldin 9:deb9547909c3 163
ngoldin 9:deb9547909c3 164
ngoldin 9:deb9547909c3 165 void m3pi::calibrate(void) {
ngoldin 9:deb9547909c3 166 _ser.putc(PI_CALIBRATE);
ngoldin 9:deb9547909c3 167 }
ngoldin 9:deb9547909c3 168
ngoldin 9:deb9547909c3 169 void m3pi::reset_calibration() {
ngoldin 9:deb9547909c3 170 _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
ngoldin 9:deb9547909c3 171 }
ngoldin 9:deb9547909c3 172
ngoldin 9:deb9547909c3 173 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) {
ngoldin 9:deb9547909c3 174 _ser.putc(max_speed);
ngoldin 9:deb9547909c3 175 _ser.putc(a);
ngoldin 9:deb9547909c3 176 _ser.putc(b);
ngoldin 9:deb9547909c3 177 _ser.putc(c);
ngoldin 9:deb9547909c3 178 _ser.putc(d);
ngoldin 9:deb9547909c3 179 }
ngoldin 9:deb9547909c3 180
ngoldin 9:deb9547909c3 181 void m3pi::PID_stop() {
ngoldin 9:deb9547909c3 182 _ser.putc(STOP_PID);
ngoldin 9:deb9547909c3 183 }
ngoldin 9:deb9547909c3 184
ngoldin 9:deb9547909c3 185 float m3pi::pot_voltage(void) {
ngoldin 9:deb9547909c3 186 int volt = 0;
ngoldin 9:deb9547909c3 187 _ser.putc(SEND_TRIMPOT);
ngoldin 9:deb9547909c3 188 volt = _ser.getc();
ngoldin 9:deb9547909c3 189 volt += _ser.getc() << 8;
ngoldin 9:deb9547909c3 190 return(volt);
ngoldin 9:deb9547909c3 191 }
ngoldin 9:deb9547909c3 192
ngoldin 9:deb9547909c3 193
ngoldin 9:deb9547909c3 194 void m3pi::leds(int val) {
ngoldin 9:deb9547909c3 195
ngoldin 9:deb9547909c3 196 BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
ngoldin 9:deb9547909c3 197 _leds = val;
ngoldin 9:deb9547909c3 198 }
ngoldin 9:deb9547909c3 199
ngoldin 9:deb9547909c3 200
ngoldin 9:deb9547909c3 201 void m3pi::locate(int x, int y) {
ngoldin 9:deb9547909c3 202 _ser.putc(DO_LCD_GOTO_XY);
ngoldin 9:deb9547909c3 203 _ser.putc(x);
ngoldin 9:deb9547909c3 204 _ser.putc(y);
ngoldin 9:deb9547909c3 205 }
ngoldin 9:deb9547909c3 206
ngoldin 9:deb9547909c3 207 void m3pi::cls(void) {
ngoldin 9:deb9547909c3 208 _ser.putc(DO_CLEAR);
ngoldin 9:deb9547909c3 209 }
ngoldin 9:deb9547909c3 210
ngoldin 9:deb9547909c3 211 int m3pi::print (char* text, int length) {
ngoldin 9:deb9547909c3 212 _ser.putc(DO_PRINT);
ngoldin 9:deb9547909c3 213 _ser.putc(length);
ngoldin 9:deb9547909c3 214 for (int i = 0 ; i < length ; i++) {
ngoldin 9:deb9547909c3 215 _ser.putc(text[i]);
ngoldin 9:deb9547909c3 216 }
ngoldin 9:deb9547909c3 217 return(0);
ngoldin 9:deb9547909c3 218 }
ngoldin 9:deb9547909c3 219
ngoldin 9:deb9547909c3 220 int m3pi::_putc (int c) {
ngoldin 9:deb9547909c3 221 _ser.putc(DO_PRINT);
ngoldin 9:deb9547909c3 222 _ser.putc(0x1);
ngoldin 9:deb9547909c3 223 _ser.putc(c);
ngoldin 9:deb9547909c3 224 wait (0.001);
ngoldin 9:deb9547909c3 225 return(c);
ngoldin 9:deb9547909c3 226 }
ngoldin 9:deb9547909c3 227
ngoldin 9:deb9547909c3 228 int m3pi::_getc (void) {
ngoldin 9:deb9547909c3 229 char r = 0;
ngoldin 9:deb9547909c3 230 return(r);
ngoldin 9:deb9547909c3 231 }
ngoldin 9:deb9547909c3 232
ngoldin 9:deb9547909c3 233 int m3pi::putc (int c) {
ngoldin 9:deb9547909c3 234 return(_ser.putc(c));
ngoldin 9:deb9547909c3 235 }
ngoldin 9:deb9547909c3 236
ngoldin 9:deb9547909c3 237 int m3pi::getc (void) {
ngoldin 9:deb9547909c3 238 return(_ser.getc());
ngoldin 9:deb9547909c3 239 }
ngoldin 9:deb9547909c3 240
ngoldin 9:deb9547909c3 241
ngoldin 9:deb9547909c3 242
ngoldin 9:deb9547909c3 243
ngoldin 9:deb9547909c3 244
ngoldin 9:deb9547909c3 245 #ifdef MBED_RPC
ngoldin 9:deb9547909c3 246 const rpc_method *m3pi::get_rpc_methods() {
ngoldin 9:deb9547909c3 247 static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> },
ngoldin 9:deb9547909c3 248 { "backward", rpc_method_caller<m3pi, float, &m3pi::backward> },
ngoldin 9:deb9547909c3 249 { "left", rpc_method_caller<m3pi, float, &m3pi::left> },
ngoldin 9:deb9547909c3 250 { "right", rpc_method_caller<m3pi, float, &m3pi::right> },
ngoldin 9:deb9547909c3 251 { "stop", rpc_method_caller<m3pi, &m3pi::stop> },
ngoldin 9:deb9547909c3 252 { "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> },
ngoldin 9:deb9547909c3 253 { "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> },
ngoldin 9:deb9547909c3 254 { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> },
ngoldin 9:deb9547909c3 255 { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> },
ngoldin 9:deb9547909c3 256 { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> },
ngoldin 9:deb9547909c3 257
ngoldin 9:deb9547909c3 258
ngoldin 9:deb9547909c3 259 RPC_METHOD_SUPER(Base)
ngoldin 9:deb9547909c3 260 };
ngoldin 9:deb9547909c3 261 return rpc_methods;
ngoldin 9:deb9547909c3 262 }
ngoldin 9:deb9547909c3 263 #endif