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