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