Diff: Navigate_Pololu.cpp

Revision:

1:b6c1de65e591

Parent:

0:33c364521d16

Child:

2:1255feea1c59

--- a/Navigate_Pololu.cpp	Sat Jan 30 20:26:14 2016 +0000
+++ b/Navigate_Pololu.cpp	Sun Jan 31 16:08:34 2016 +0000
@@ -1,7 +1,10 @@
 #include "Navigate_Pololu.h"
+#include "mbed.h"
 #include <cstring>
+#include "m3pi.h"
 
-Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth):leftOpto(leftIn),rightOpto(rightIn){
+Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth,PinName robotTx, PinName robotRx):leftOpto(leftIn),rightOpto(rightIn){
+    calibratePos(robotTx, robotRx); //call routine to calibrate wheels
     X = 0.0f;
     Y = 0.0f;
     heading = 0.0f;
@@ -45,27 +48,30 @@
     printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading);
 }
 
-uint8_t* Navigate_Pololu::getX(){
-    //static_assert(sizeof(float) == 2);
-    int f = 0; // whatever value
-    uint8_t bytes[4];
-    std::memcpy(bytes, &X,4);
-    return bytes;
+int Navigate_Pololu::getX(){
+    return (int)X;
+}
+
+int Navigate_Pololu::getY(){
+    return (int)Y;
+}
+
+int Navigate_Pololu::getHeading(){
+    return (int)((heading/PI)*180);
 }
 
-uint8_t* Navigate_Pololu::getY(){
-    //static_assert(sizeof(float) == 2);
-    float f = 0; // whatever value
-    uint8_t bytes[4];
-    std::memcpy(bytes, &Y,4);
-    return bytes;
-}
-
-uint8_t* Navigate_Pololu::getHeading(){
-    //static_assert(sizeof(float) == 2);
-    float f = 0; // whatever value
-    uint8_t bytes[4];
-    std::memcpy(bytes, &heading,4);
-    return bytes;
-}
-
+void Navigate_Pololu::calibratePos(PinName robotTx,PinName robotRx){
+    m3pi robot(D7, robotTx, robotRx);
+    wait(1);
+    while(leftOpto){
+        robot.left_motor(0.2);
+        wait(0.01);
+    }
+        robot.stop();
+        wait(0.1);
+    while(rightOpto){
+            robot.right_motor(0.2);
+            wait(0.01);
+        }
+        robot.stop();
+}
\ No newline at end of file