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::motor (int motor, float speed) {
00079     char opcode = 0x0;
00080     if (speed > 0.0) {
00081         if (motor==1)
00082             opcode = M1_FORWARD;
00083         else
00084             opcode = M2_FORWARD;
00085     } else {
00086         if (motor==1)
00087             opcode = M1_BACKWARD;
00088         else
00089             opcode = M2_BACKWARD;
00090     }
00091     unsigned char arg = 0x7f * abs(speed);
00092 
00093     _ser.putc(opcode);
00094     _ser.putc(arg);
00095 }
00096 
00097 float m3pi::battery() {
00098     _ser.putc(SEND_BATTERY_MILLIVOLTS);
00099     char lowbyte = _ser.getc();
00100     char hibyte  = _ser.getc();
00101     float v = ((lowbyte + (hibyte << 8))/1000.0);
00102     return(v);
00103 }
00104 
00105 float m3pi::line_position() {
00106     int pos = 0;
00107     _ser.putc(SEND_LINE_POSITION);
00108     pos = _ser.getc();
00109     pos += _ser.getc() << 8;
00110 
00111     float fpos = ((float)pos - 2048.0)/2048.0;
00112     return(fpos);
00113 }
00114 
00115 char m3pi::sensor_auto_calibrate() {
00116     _ser.putc(AUTO_CALIBRATE);
00117     return(_ser.getc());
00118 }
00119 
00120 
00121 void m3pi::calibrate(void) {
00122     _ser.putc(PI_CALIBRATE);
00123 }
00124 
00125 void m3pi::reset_calibration() {
00126     _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
00127 }
00128 
00129 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) {
00130     _ser.putc(max_speed);
00131     _ser.putc(a);
00132     _ser.putc(b);
00133     _ser.putc(c);
00134     _ser.putc(d);
00135 }
00136 
00137 void m3pi::PID_stop() {
00138     _ser.putc(STOP_PID);
00139 }
00140 
00141 float m3pi::pot_voltage(void) {
00142     int volt = 0;
00143     _ser.putc(SEND_TRIMPOT);
00144     volt = _ser.getc();
00145     volt += _ser.getc() << 8;
00146     return(volt);
00147 }
00148 
00149 
00150 void m3pi::leds(int val) {
00151 
00152     BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
00153     _leds = val;
00154 }
00155 
00156 
00157 void m3pi::locate(int x, int y) {
00158     _ser.putc(DO_LCD_GOTO_XY);
00159     _ser.putc(x);
00160     _ser.putc(y);
00161 }
00162 
00163 void m3pi::cls(void) {
00164     _ser.putc(DO_CLEAR);
00165 }
00166 
00167 int m3pi::print (char* text, int length) {
00168     _ser.putc(DO_PRINT);
00169     _ser.putc(length);
00170     for (int i = 0 ; i < length ; i++) {
00171         _ser.putc(text[i]);
00172     }
00173     return(0);
00174 }
00175 
00176 void m3pi::get_raw_sensors(int* values) {
00177     _ser.putc(SEND_RAW_SENSOR_VALUES);
00178     for (int i = 0; i<5; i++) {
00179         while(_ser.readable() == 0){}
00180         values[i] = _ser.getc();
00181         while(_ser.readable() == 0){}
00182         values[i] += _ser.getc() << 8;
00183         //values[i] -= raw_white_levels[i];
00184     }
00185 }
00186 
00187 
00188 void m3pi::get_calibrated_sensors(float* values) {
00189     int temp[5];
00190     _ser.putc(SEND_CALIBRATED_SENSOR_VALUES);
00191     for (int i = 0; i<5; i++) {
00192         temp[i] = _ser.getc();
00193         temp[i] += _ser.getc() << 8;
00194         values[i] = float(temp[i]) / 1000;
00195     }
00196 }
00197 
00198 void m3pi::get_white_levels() {
00199     get_raw_sensors(raw_white_levels);
00200 }
00201 
00202 int m3pi::is_line()
00203 {
00204     int ret = 0;
00205     int edgeCount = 0;
00206     int temp[5];
00207     get_raw_sensors(temp);
00208     for(int i = 0; i <5 ; i++)
00209     {
00210         if(temp[i] - raw_white_levels[i]>= LINE_THRESHOLD)
00211         {
00212         ret++;
00213         if(i == 0|| i== 4)
00214         {
00215             edgeCount--;
00216         }
00217         else
00218         {
00219             edgeCount++;
00220         }
00221         }
00222         
00223     }
00224     
00225     if(edgeCount >=0 && ret>0)
00226     {
00227        return 1;
00228     }
00229     else if(edgeCount <0 && ret>0)
00230     {
00231         return -1;    
00232     }
00233     else
00234     {
00235         return 0;
00236     }
00237     
00238     
00239 }
00240 
00241 int m3pi::_putc (int c) {
00242     _ser.putc(DO_PRINT);
00243     _ser.putc(0x1);
00244     _ser.putc(c);
00245     wait (0.001);
00246     return(c);
00247 }
00248 
00249 int m3pi::_getc (void) {
00250     char r = 0;
00251     return(r);
00252 }
00253 
00254 int m3pi::putc (int c) {
00255     return(_ser.putc(c));
00256 }
00257 
00258 int m3pi::getc (void) {
00259     return(_ser.getc());
00260 }
00261 
00262 
00263 
00264 
00265 
00266 #ifdef MBED_RPC
00267 const rpc_method *m3pi::get_rpc_methods() {
00268     static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> },
00269         { "backward", rpc_method_caller<m3pi, float, &m3pi::backward> },
00270         { "left", rpc_method_caller<m3pi, float, &m3pi::left> },
00271         { "right", rpc_method_caller<m3pi, float, &m3pi::right> },
00272         { "stop", rpc_method_caller<m3pi, &m3pi::stop> },
00273         { "left_motor", rpc_method_caller<m3pi, float, &m3pi::left_motor> },
00274         { "right_motor", rpc_method_caller<m3pi, float, &m3pi::right_motor> },
00275         { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> },
00276         { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> },
00277         { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> },
00278 
00279 
00280         RPC_METHOD_SUPER(Base)
00281     };
00282     return rpc_methods;
00283 }
00284 #endif