m3pi.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 
00004 namespace physcom {
00005     
00006 M3pi::M3pi(PinName nrst, PinName tx, PinName rx) :  Stream("m3pi"), _nrst(nrst), _ser(tx, rx)  {
00007     _ser.baud(115200);
00008     reset();
00009     m1_speed = 0;
00010     m2_speed = 0;
00011 }
00012 
00013 M3pi::M3pi() :  Stream("m3pi"), _nrst(p23), _ser(p9, p10)  {
00014     _ser.baud(115200);
00015     reset();
00016     m1_speed = 0;
00017     m2_speed = 0;
00018 }
00019 
00020 
00021 void M3pi::reset () {
00022     _nrst = 0;
00023     wait (0.01);
00024     _nrst = 1;
00025     wait (0.1);
00026     m1_speed = 0;
00027     m2_speed = 0;
00028 }
00029 
00030 void M3pi::stop(){
00031     while(m1_speed > 0 || m2_speed > 0){
00032         if((m1_speed - 0.05) >= 0){
00033             m1_speed -= 0.05;
00034         }else{
00035             m1_speed = 0;
00036         }
00037         if((m2_speed - 0.05) >= 0){
00038             m2_speed -= 0.05;
00039         }else{
00040             m2_speed = 0;
00041         }
00042         
00043         activate_motor(0, m1_speed);
00044         activate_motor(1, m2_speed);  
00045         wait_ms(35);
00046     }   
00047 }  
00048 
00049 void M3pi::activate_motor (int motor, float speed) {
00050     char opcode = 0x0;
00051     if (speed > 0.0) {
00052         if (motor==0){
00053             opcode = M1_FORWARD;
00054             m1_speed = speed;
00055         }
00056         else {
00057             opcode = M2_FORWARD;
00058             m2_speed = speed;
00059         }
00060     } else {
00061         if (motor==0) {
00062             opcode = M1_BACKWARD;
00063             m1_speed = speed;
00064         }
00065         else {
00066             opcode = M2_BACKWARD;
00067             m2_speed = speed;
00068         }
00069     }
00070     unsigned char arg = 0x7f * abs(speed);
00071 
00072     _ser.putc(opcode);
00073     _ser.putc(arg);
00074 }
00075 
00076 float M3pi::battery() {
00077     _ser.putc(SEND_BATTERY_MILLIVOLTS);
00078     char lowbyte = _ser.getc();
00079     char hibyte  = _ser.getc();
00080     float v = ((lowbyte + (hibyte << 8))/1000.0);
00081     return(v);
00082 }
00083 
00084 void M3pi::raw_sensors( int sensors[5]) {
00085     int sensor1 = 0;
00086     int sensor2 = 0;
00087     int sensor3 = 0;
00088     int sensor4 = 0;
00089     int sensor5 = 0;
00090     _ser.putc(SEND_RAW_SENSOR_VALUES);
00091     sensor1 = _ser.getc();
00092     sensor1 += _ser.getc() << 8;
00093     sensor2 = _ser.getc();
00094     sensor2 += _ser.getc() << 8;
00095     sensor3 = _ser.getc();
00096     sensor3 += _ser.getc() << 8;
00097     sensor4 = _ser.getc();
00098     sensor4 += _ser.getc() << 8;
00099     sensor5 = _ser.getc();
00100     sensor5 += _ser.getc() << 8;
00101     
00102     sensors[0] = sensor1;
00103     sensors[1] = sensor2;
00104     sensors[2] = sensor3;
00105     sensors[3] = sensor4;
00106     sensors[4] = sensor5;
00107 }
00108 
00109 void M3pi::calibrated_sensors( int sensors[5]) {
00110     int sensor1 = 0;
00111     int sensor2 = 0;
00112     int sensor3 = 0;
00113     int sensor4 = 0;
00114     int sensor5 = 0;
00115     _ser.putc(SEND_CALIBRATED_SENSOR_VALUES);
00116     sensor1 = _ser.getc();
00117     sensor1 += _ser.getc() << 8;
00118     sensor2 = _ser.getc();
00119     sensor2 += _ser.getc() << 8;
00120     sensor3 = _ser.getc();
00121     sensor3 += _ser.getc() << 8;
00122     sensor4 = _ser.getc();
00123     sensor4 += _ser.getc() << 8;
00124     sensor5 = _ser.getc();
00125     sensor5 += _ser.getc() << 8;
00126     
00127     sensors[0] = sensor1;
00128     sensors[1] = sensor2;
00129     sensors[2] = sensor3;
00130     sensors[3] = sensor4;
00131     sensors[4] = sensor5;
00132 }
00133 
00134 char M3pi::sensor_auto_calibrate() {
00135     _ser.putc(AUTO_CALIBRATE);
00136     return(_ser.getc());
00137 }
00138 
00139 int M3pi::_putc (int c) {
00140     _ser.putc(DO_PRINT);  
00141     _ser.putc(0x1);       
00142     _ser.putc(c);         
00143     wait (0.001);
00144     return(c);
00145 }
00146 
00147 int M3pi::_getc (void) {
00148     char r = 0;
00149     return(r);
00150 }
00151 
00152 } // Namespace physcom