Diff: Navigate_Pololu.cpp

Revision:

0:33c364521d16

Child:

1:b6c1de65e591

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Navigate_Pololu.cpp	Sat Jan 30 20:26:14 2016 +0000
@@ -0,0 +1,71 @@
+#include "Navigate_Pololu.h"
+#include <cstring>
+
+Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth):leftOpto(leftIn),rightOpto(rightIn){
+    X = 0.0f;
+    Y = 0.0f;
+    heading = 0.0f;
+    leftCounts = 0;
+    rightCounts = 0;
+    previousLeftCounts = 0;
+    previousRightCounts = 0;
+    distancePerCount = (PI * wheelDiameter) / (float)countsPerRevolution;
+    radiansPerCount = PI * (wheelDiameter/trackWidth)/countsPerRevolution;
+    leftOpto.rise(this, &Navigate_Pololu::leftWheelInterrupt);
+    rightOpto.rise(this, &Navigate_Pololu::rightWheelInterrupt);
+    printf("DistancePerCount: %f, RadiansPerCounts: %f\n",distancePerCount,radiansPerCount);
+}
+
+void Navigate_Pololu::leftWheelInterrupt(){
+    leftCounts++;
+    UpdatePosition();
+    previousLeftCounts = leftCounts;
+}
+
+void Navigate_Pololu::rightWheelInterrupt(){
+    rightCounts++;
+    UpdatePosition();
+    previousRightCounts = rightCounts;
+}
+
+void Navigate_Pololu::UpdatePosition(){
+    deltaLeft = leftCounts - previousLeftCounts;
+    deltaRight = rightCounts - previousRightCounts;
+    deltaDistance = 0.5f * (float)(deltaLeft + deltaRight) * distancePerCount;
+    deltaHeading = (float) (deltaRight - deltaLeft) * radiansPerCount;
+    heading += deltaHeading;
+    deltaX = deltaDistance * (float)cos(heading);
+    deltaY = deltaDistance * (float)sin(heading);
+    X += deltaX;
+    Y += deltaY;
+    if(heading>PI)
+        heading -= TWO_PI;
+    else if (heading <= -PI)
+        heading += TWO_PI;
+    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;
+}
+
+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;
+}
+