ba
Fork of m3pi by
Embed:
(wiki syntax)
Show/hide line numbers
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 #define SEND_CALIBRATED_SENSOR_VALUES 0x87 00028 00029 m3pi::m3pi(PinName nrst, PinName tx, PinName rx) : Stream("m3pi"), _nrst(nrst), _ser(tx, rx) { 00030 _ser.baud(115200); 00031 reset(); 00032 } 00033 00034 m3pi::m3pi() : Stream("m3pi"), _nrst(p23), _ser(p9, p10) { 00035 _ser.baud(115200); 00036 reset(); 00037 } 00038 00039 00040 void m3pi::reset () { 00041 _nrst = 0; 00042 wait (0.01); 00043 _nrst = 1; 00044 wait (0.1); 00045 } 00046 00047 void m3pi::left_motor (float speed) { 00048 motor(0,speed); 00049 } 00050 00051 void m3pi::right_motor (float speed) { 00052 motor(1,speed); 00053 } 00054 00055 void m3pi::forward (float speed) { 00056 motor(0,speed); 00057 motor(1,speed); 00058 } 00059 00060 void m3pi::backward (float speed) { 00061 motor(0,-1.0*speed); 00062 motor(1,-1.0*speed); 00063 } 00064 00065 void m3pi::left (float speed) { 00066 motor(0,speed); 00067 motor(1,-1.0*speed); 00068 } 00069 00070 void m3pi::right (float speed) { 00071 motor(0,-1.0*speed); 00072 motor(1,speed); 00073 } 00074 00075 void m3pi::stop (void) { 00076 motor(0,0.0); 00077 motor(1,0.0); 00078 } 00079 00080 void m3pi::motor (int motor, float speed) { 00081 char opcode = 0x0; 00082 if (speed > 0.0) { 00083 if (motor==1) 00084 opcode = M1_FORWARD; 00085 else 00086 opcode = M2_FORWARD; 00087 } else { 00088 if (motor==1) 00089 opcode = M1_BACKWARD; 00090 else 00091 opcode = M2_BACKWARD; 00092 } 00093 unsigned char arg = 0x7f * abs(speed); 00094 00095 _ser.putc(opcode); 00096 _ser.putc(arg); 00097 } 00098 00099 float m3pi::battery() { 00100 _ser.putc(SEND_BATTERY_MILLIVOLTS); 00101 char lowbyte = _ser.getc(); 00102 char hibyte = _ser.getc(); 00103 float v = ((lowbyte + (hibyte << 8))/1000.0); 00104 return(v); 00105 } 00106 00107 float m3pi::line_position() { 00108 int pos = 0; 00109 _ser.putc(SEND_LINE_POSITION); 00110 pos = _ser.getc(); 00111 pos += _ser.getc() << 8; 00112 00113 float fpos = ((float)pos - 2048.0)/2048.0; 00114 return(fpos); 00115 } 00116 00117 char m3pi::sensor_auto_calibrate() { 00118 _ser.putc(AUTO_CALIBRATE); 00119 return(_ser.getc()); 00120 } 00121 00122 00123 void m3pi::calibrate(void) { 00124 _ser.putc(PI_CALIBRATE); 00125 } 00126 00127 void m3pi::reset_calibration() { 00128 _ser.putc(LINE_SENSORS_RESET_CALIBRATION); 00129 } 00130 00131 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) { 00132 _ser.putc(max_speed); 00133 _ser.putc(a); 00134 _ser.putc(b); 00135 _ser.putc(c); 00136 _ser.putc(d); 00137 } 00138 00139 void m3pi::PID_stop() { 00140 _ser.putc(STOP_PID); 00141 } 00142 00143 float m3pi::pot_voltage(void) { 00144 int volt = 0; 00145 _ser.putc(SEND_TRIMPOT); 00146 volt = _ser.getc(); 00147 volt += _ser.getc() << 8; 00148 return(volt); 00149 } 00150 00151 00152 void m3pi::leds(int val) { 00153 00154 BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13); 00155 _leds = val; 00156 } 00157 00158 00159 void m3pi::locate(int x, int y) { 00160 _ser.putc(DO_LCD_GOTO_XY); 00161 _ser.putc(x); 00162 _ser.putc(y); 00163 } 00164 00165 void m3pi::cls(void) { 00166 _ser.putc(DO_CLEAR); 00167 } 00168 00169 int m3pi::print (char* text, int length) { 00170 _ser.putc(DO_PRINT); 00171 _ser.putc(length); 00172 for (int i = 0 ; i < length ; i++) { 00173 _ser.putc(text[i]); 00174 } 00175 return(0); 00176 } 00177 00178 int m3pi::_putc (int c) { 00179 _ser.putc(DO_PRINT); 00180 _ser.putc(0x1); 00181 _ser.putc(c); 00182 wait (0.001); 00183 return(c); 00184 } 00185 00186 int m3pi::_getc (void) { 00187 char r = 0; 00188 return(r); 00189 } 00190 00191 int m3pi::putc (int c) { 00192 return(_ser.putc(c)); 00193 } 00194 00195 int m3pi::getc (void) { 00196 return(_ser.getc()); 00197 } 00198 00199 void m3pi::readsensor (int *sensor){ 00200 00201 _ser.putc(SEND_CALIBRATED_SENSOR_VALUES); 00202 sensor[0] = _ser.getc(); 00203 sensor[0] += _ser.getc() << 8; 00204 sensor[1] = _ser.getc(); 00205 sensor[1] += _ser.getc() << 8; 00206 sensor[2] = _ser.getc(); 00207 sensor[2] += _ser.getc() << 8; 00208 sensor[3] = _ser.getc(); 00209 sensor[3] += _ser.getc() << 8; 00210 sensor[4] = _ser.getc(); 00211 sensor[4] += _ser.getc() << 8; 00212 00213 return; 00214 } 00215 00216 00217 00218 00219 00220 #ifdef MBED_RPC 00221 const rpc_method *m3pi::get_rpc_methods() { 00222 static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> }, 00223 { "backward", rpc_method_caller<m3pi, float, &m3pi::backward> }, 00224 { "left", rpc_method_caller<m3pi, float, &m3pi::left> }, 00225 { "right", rpc_method_caller<m3pi, float, &m3pi::right> }, 00226 { "stop", rpc_method_caller<m3pi, &m3pi::stop> }, 00227 { "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> }, 00228 { "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> }, 00229 { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> }, 00230 { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> }, 00231 { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> }, 00232 00233 00234 RPC_METHOD_SUPER(Base) 00235 }; 00236 return rpc_methods; 00237 } 00238 #endif
Generated on Tue Jul 19 2022 01:26:21 by 1.7.2