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
m3pi.cpp
00001 /* m3pi Library 00002 * 00003 * Copyright (c) 2007-2010 cstyles 00004 * 00005 * Permission is hereby granted, free of charge, to any person obtaining a copy 00006 * of this software and associated documentation files (the "Software"), to deal 00007 * in the Software without restriction, including without limitation the rights 00008 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00009 * copies of the Software, and to permit persons to whom the Software is 00010 * furnished to do so, subject to the following conditions: 00011 * 00012 * The above copyright notice and this permission notice shall be included in 00013 * all copies or substantial portions of the Software. 00014 * 00015 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00016 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00017 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00018 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00019 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00020 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00021 * THE SOFTWARE. 00022 */ 00023 00024 #include "mbed.h" 00025 #include "m3pi.h" 00026 00027 m3pi::m3pi(PinName nrst, PinName tx, PinName rx) : Stream("m3pi"), _nrst(nrst), _ser(tx, rx) { 00028 _ser.baud(115200); 00029 reset(); 00030 } 00031 00032 m3pi::m3pi() : Stream("m3pi"), _nrst(p23), _ser(p9, p10) { 00033 _ser.baud(115200); 00034 reset(); 00035 } 00036 00037 00038 void m3pi::reset () { 00039 _nrst = 0; 00040 wait (0.01); 00041 _nrst = 1; 00042 wait (0.1); 00043 } 00044 00045 void m3pi::left_motor (float speed) { 00046 motor(0,speed); 00047 } 00048 00049 void m3pi::right_motor (float speed) { 00050 motor(1,speed); 00051 } 00052 00053 void m3pi::forward (float speed) { 00054 motor(0,speed); 00055 motor(1,speed); 00056 } 00057 00058 void m3pi::backward (float speed) { 00059 motor(0,-1.0*speed); 00060 motor(1,-1.0*speed); 00061 } 00062 00063 void m3pi::left (float speed) { 00064 motor(0,speed); 00065 motor(1,-1.0*speed); 00066 } 00067 00068 void m3pi::right (float speed) { 00069 motor(0,-1.0*speed); 00070 motor(1,speed); 00071 } 00072 00073 void m3pi::stop (void) { 00074 motor(0,0.0); 00075 motor(1,0.0); 00076 } 00077 00078 void m3pi::left_motor(int speed) { 00079 if (speed > 0) { 00080 _ser.putc(M1_FORWARD); 00081 _ser.putc(speed); 00082 } else { 00083 _ser.putc(M1_BACKWARD); 00084 _ser.putc(-speed); 00085 } 00086 } 00087 00088 void m3pi::right_motor(int speed) { 00089 if (speed > 0) { 00090 _ser.putc(M2_FORWARD); 00091 _ser.putc(speed); 00092 } else { 00093 _ser.putc(M2_BACKWARD); 00094 _ser.putc(-speed); 00095 } 00096 } 00097 00098 void m3pi::motor (int motor, float speed) { 00099 char opcode = 0x0; 00100 if (speed > 0.0) { 00101 if (motor==1) 00102 opcode = M1_FORWARD; 00103 else 00104 opcode = M2_FORWARD; 00105 } else { 00106 if (motor==1) 00107 opcode = M1_BACKWARD; 00108 else 00109 opcode = M2_BACKWARD; 00110 } 00111 unsigned char arg = 0x7f * abs(speed); 00112 00113 _ser.putc(opcode); 00114 _ser.putc(arg); 00115 } 00116 00117 float m3pi::battery() { 00118 _ser.putc(SEND_BATTERY_MILLIVOLTS); 00119 char lowbyte = _ser.getc(); 00120 char hibyte = _ser.getc(); 00121 float v = ((lowbyte + (hibyte << 8))/1000.0); 00122 return(v); 00123 } 00124 00125 float m3pi::line_position() { 00126 int pos = 0; 00127 _ser.putc(SEND_LINE_POSITION); 00128 pos = _ser.getc(); 00129 pos += _ser.getc() << 8; 00130 00131 float fpos = ((float)pos - 2048.0)/2048.0; 00132 return(fpos); 00133 } 00134 00135 char m3pi::sensor_auto_calibrate() { 00136 _ser.putc(AUTO_CALIBRATE); 00137 return(_ser.getc()); 00138 } 00139 00140 00141 void m3pi::calibrate(void) { 00142 _ser.putc(PI_CALIBRATE); 00143 } 00144 00145 void m3pi::reset_calibration() { 00146 _ser.putc(LINE_SENSORS_RESET_CALIBRATION); 00147 } 00148 00149 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) { 00150 _ser.putc(max_speed); 00151 _ser.putc(a); 00152 _ser.putc(b); 00153 _ser.putc(c); 00154 _ser.putc(d); 00155 } 00156 00157 void m3pi::PID_stop() { 00158 _ser.putc(STOP_PID); 00159 } 00160 00161 float m3pi::pot_voltage(void) { 00162 int volt = 0; 00163 _ser.putc(SEND_TRIMPOT); 00164 volt = _ser.getc(); 00165 volt += _ser.getc() << 8; 00166 return(volt); 00167 } 00168 00169 00170 void m3pi::leds(int val) { 00171 00172 BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13); 00173 _leds = val; 00174 } 00175 00176 00177 void m3pi::locate(int x, int y) { 00178 _ser.putc(DO_LCD_GOTO_XY); 00179 _ser.putc(x); 00180 _ser.putc(y); 00181 } 00182 00183 void m3pi::cls(void) { 00184 _ser.putc(DO_CLEAR); 00185 } 00186 00187 int m3pi::print (char* text, int length) { 00188 _ser.putc(DO_PRINT); 00189 _ser.putc(length); 00190 for (int i = 0 ; i < length ; i++) { 00191 _ser.putc(text[i]); 00192 } 00193 return(0); 00194 } 00195 00196 int m3pi::_putc (int c) { 00197 _ser.putc(DO_PRINT); 00198 _ser.putc(0x1); 00199 _ser.putc(c); 00200 wait (0.001); 00201 return(c); 00202 } 00203 00204 int m3pi::_getc (void) { 00205 char r = 0; 00206 return(r); 00207 } 00208 00209 int m3pi::putc (int c) { 00210 return(_ser.putc(c)); 00211 } 00212 00213 int m3pi::getc (void) { 00214 return(_ser.getc()); 00215 } 00216 00217 00218 00219 00220 00221 #ifdef MBED_RPC 00222 const rpc_method *m3pi::get_rpc_methods() { 00223 static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> }, 00224 { "backward", rpc_method_caller<m3pi, float, &m3pi::backward> }, 00225 { "left", rpc_method_caller<m3pi, float, &m3pi::left> }, 00226 { "right", rpc_method_caller<m3pi, float, &m3pi::right> }, 00227 { "stop", rpc_method_caller<m3pi, &m3pi::stop> }, 00228 { "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> }, 00229 { "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> }, 00230 { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> }, 00231 { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> }, 00232 { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> }, 00233 00234 00235 RPC_METHOD_SUPER(Base) 00236 }; 00237 return rpc_methods; 00238 } 00239 #endif
Generated on Sat Jul 16 2022 09:25:04 by 1.7.2