Diff: m3pi.cpp

Revision:

9:e0904a2c1bc0

Parent:

7:9b128cebb3c2

--- a/m3pi.cpp	Thu May 12 13:26:37 2011 +0000
+++ b/m3pi.cpp	Wed Nov 23 09:00:13 2016 +0000
@@ -20,61 +20,61 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
-
+ 
 #include "mbed.h"
 #include "m3pi.h"
-
+ 
 m3pi::m3pi(PinName nrst, PinName tx, PinName rx) :  Stream("m3pi"), _nrst(nrst), _ser(tx, rx)  {
     _ser.baud(115200);
     reset();
 }
-
+ 
 m3pi::m3pi() :  Stream("m3pi"), _nrst(p23), _ser(p9, p10)  {
     _ser.baud(115200);
     reset();
 }
-
-
+ 
+ 
 void m3pi::reset () {
     _nrst = 0;
     wait (0.01);
     _nrst = 1;
     wait (0.1);
 }
-
+ 
 void m3pi::left_motor (float speed) {
+    motor(1,speed);
+}
+ 
+void m3pi::right_motor (float speed) {
     motor(0,speed);
 }
-
-void m3pi::right_motor (float speed) {
-    motor(1,speed);
-}
-
+ 
 void m3pi::forward (float speed) {
     motor(0,speed);
     motor(1,speed);
 }
-
+ 
 void m3pi::backward (float speed) {
     motor(0,-1.0*speed);
     motor(1,-1.0*speed);
 }
-
+ 
 void m3pi::left (float speed) {
     motor(0,speed);
     motor(1,-1.0*speed);
 }
-
+ 
 void m3pi::right (float speed) {
     motor(0,-1.0*speed);
     motor(1,speed);
 }
-
+ 
 void m3pi::stop (void) {
     motor(0,0.0);
     motor(1,0.0);
 }
-
+ 
 void m3pi::motor (int motor, float speed) {
     char opcode = 0x0;
     if (speed > 0.0) {
@@ -89,11 +89,11 @@
             opcode = M2_BACKWARD;
     }
     unsigned char arg = 0x7f * abs(speed);
-
+ 
     _ser.putc(opcode);
     _ser.putc(arg);
 }
-
+ 
 float m3pi::battery() {
     _ser.putc(SEND_BATTERY_MILLIVOLTS);
     char lowbyte = _ser.getc();
@@ -101,7 +101,7 @@
     float v = ((lowbyte + (hibyte << 8))/1000.0);
     return(v);
 }
-
+ 
 float m3pi::line_position() {
     int pos = 0;
     _ser.putc(SEND_LINE_POSITION);
@@ -111,21 +111,30 @@
     float fpos = ((float)pos - 2048.0)/2048.0;
     return(fpos);
 }
-
+ 
+void m3pi::calibrated_sensors(unsigned short ltab[5]) {
+    unsigned i;
+    _ser.putc(SEND_CALIB_SENSOR_VALUES);
+    for(i=0;i<5;i++){
+        ltab[i] = (unsigned short) _ser.getc();
+        ltab[i] += _ser.getc() << 8;
+    }
+}
+ 
 char m3pi::sensor_auto_calibrate() {
     _ser.putc(AUTO_CALIBRATE);
     return(_ser.getc());
 }
-
-
+ 
+ 
 void m3pi::calibrate(void) {
     _ser.putc(PI_CALIBRATE);
 }
-
+ 
 void m3pi::reset_calibration() {
     _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
 }
-
+ 
 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) {
     _ser.putc(max_speed);
     _ser.putc(a);
@@ -133,11 +142,11 @@
     _ser.putc(c);
     _ser.putc(d);
 }
-
+ 
 void m3pi::PID_stop() {
     _ser.putc(STOP_PID);
 }
-
+ 
 float m3pi::pot_voltage(void) {
     int volt = 0;
     _ser.putc(SEND_TRIMPOT);
@@ -145,25 +154,25 @@
     volt += _ser.getc() << 8;
     return(volt);
 }
-
-
+ 
+ 
 void m3pi::leds(int val) {
-
+ 
     BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
     _leds = val;
 }
-
-
+ 
+ 
 void m3pi::locate(int x, int y) {
     _ser.putc(DO_LCD_GOTO_XY);
     _ser.putc(x);
     _ser.putc(y);
 }
-
+ 
 void m3pi::cls(void) {
     _ser.putc(DO_CLEAR);
 }
-
+ 
 int m3pi::print (char* text, int length) {
     _ser.putc(DO_PRINT);  
     _ser.putc(length);       
@@ -172,7 +181,7 @@
     }
     return(0);
 }
-
+ 
 int m3pi::_putc (int c) {
     _ser.putc(DO_PRINT);  
     _ser.putc(0x1);       
@@ -180,24 +189,24 @@
     wait (0.001);
     return(c);
 }
-
+ 
 int m3pi::_getc (void) {
     char r = 0;
     return(r);
 }
-
+ 
 int m3pi::putc (int c) {
     return(_ser.putc(c));
 }
-
+ 
 int m3pi::getc (void) {
     return(_ser.getc());
 }
-
-
-
-
-
+ 
+ 
+ 
+ 
+ 
 #ifdef MBED_RPC
 const rpc_method *m3pi::get_rpc_methods() {
     static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> },
@@ -210,10 +219,10 @@
         { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> },
         { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> },
         { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> },
-
-
+ 
+ 
         RPC_METHOD_SUPER(Base)
     };
     return rpc_methods;
 }
-#endif
+#endif
\ No newline at end of file