Masterman / m3piExpandedCommandSet

This adds to the existing m3pi mbed library to add driving commands with speeds as integers ranging from -255 for 255, as does the serial slave program for the m3pi. Library for the m3pi robot. This works with a Pololu 3pi robot with the Serial Slave firmware, and exposes an API.

Fork of m3pi by Chris Styles

m3pi.cpp@9:3f9c47a7fc66, 2018-05-27 (annotated)

Committer:
kkillebrew
Date:
Sun May 27 22:06:09 2018 +0000
Revision:
9:3f9c47a7fc66
Parent:
7:9b128cebb3c2 
Overload methods to allow setting wheel speeds in the 0 - 255 range.

Who changed what in which revision?

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