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@12:bfad7a7422fb, 2013-05-17 (annotated)

Committer:
jomkippur
Date:
Fri May 17 11:14:01 2013 +0000
Revision:
12:bfad7a7422fb
Parent:
11:2761054a8926 
store_keeper

Who changed what in which revision?

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