Diff: Navigate_Pololu.cpp

Revision:

3:c5dff598fea4

Parent:

2:1255feea1c59

--- a/Navigate_Pololu.cpp	Sun Jan 31 21:02:13 2016 +0000
+++ b/Navigate_Pololu.cpp	Fri Mar 04 18:46:26 2016 +0000
@@ -12,6 +12,7 @@
     rightCounts = 0; //reset counter for right wheel
     previousLeftCounts = 0;
     previousRightCounts = 0;
+    //take out pi on distance per count and change to two_pi on raidanspercount
     distancePerCount = (PI * wheelDiameter) / (float)countsPerRevolution; //set variable
     radiansPerCount = PI * (wheelDiameter/trackWidth)/countsPerRevolution; //set variable
     leftOpto.rise(this, &Navigate_Pololu::leftWheelInterrupt); //create interrupt for left opto switch
@@ -20,20 +21,22 @@
 }
 
 void Navigate_Pololu::leftWheelInterrupt(){
-    if(robot.leftDir == 1)
+    if(robot.leftDir == 1)//check direction of left motor and increment or decrement count accordingly
         leftCounts++;
     else
         leftCounts--;
     UpdatePosition(); //recalculate position
+    //printf("Left: %d\n",leftCounts); //testing use
     previousLeftCounts = leftCounts;
 }
 
 void Navigate_Pololu::rightWheelInterrupt(){
-    if(robot.rightDir == 1)
+    if(robot.rightDir == 1)//check direction of right motor and increment or decrement count accordingly
         rightCounts++;
     else
         rightCounts--;
     UpdatePosition(); //recalculate position
+    //printf("Right: %d\n",rightCounts); //testing use
     previousRightCounts = rightCounts;
 }
 
@@ -51,7 +54,7 @@
         heading -= TWO_PI;
     else if (heading <= -PI)
         heading += TWO_PI;
-    printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading); //print coorindates after update
+    //printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading); //print coorindates after update
 }
 
 int Navigate_Pololu::getX(){
@@ -81,4 +84,9 @@
         wait(0.01);
     }
     robot.stop();
+}
+
+void Navigate_Pololu::resetCoordinates(){
+    X = 0.0f; //reset x coordinate
+    Y = 0.0f; //reset y coordinate
 }
\ No newline at end of file