Nikolas Goldin / 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.

Dependencies:   btbee

Dependents:   configure_btbee m3pi_btTest Test_SlowDown TurnAround ... more

Fork of m3pi by Chris Styles

m3pi_ng.cpp@14:5589eef1f879, 2014-10-06 (annotated)

Committer:
ngoldin
Date:
Mon Oct 06 10:57:25 2014 +0000
Revision:
14:5589eef1f879
Parent:
12:78fd82b3fb09 
added value ranges for sensors

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