Diff: Navigate_Pololu.cpp

Revision:

2:1255feea1c59

Parent:

1:b6c1de65e591

Child:

3:c5dff598fea4

--- a/Navigate_Pololu.cpp	Sun Jan 31 16:08:34 2016 +0000
+++ b/Navigate_Pololu.cpp	Sun Jan 31 21:02:13 2016 +0000
@@ -3,31 +3,37 @@
 #include <cstring>
 #include "m3pi.h"
 
-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;
-    leftCounts = 0;
-    rightCounts = 0;
+Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth,PinName robotTx, PinName robotRx):leftOpto(leftIn),rightOpto(rightIn),robot(D7,robotTx,robotRx){
+    calibratePos(); //call routine to calibrate wheels
+    X = 0.0f; //set x coordinate to zero
+    Y = 0.0f; //set y coordinate to zero
+    heading = 0.0f; //set heading to zero
+    leftCounts = 0; //reset counter for left wheel
+    rightCounts = 0; //reset counter for right wheel
     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);
+    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
+    rightOpto.rise(this, &Navigate_Pololu::rightWheelInterrupt); //create interrupt for right opto switch
+    printf("DistancePerCount: %f, RadiansPerCounts: %f\n",distancePerCount,radiansPerCount); //print out initial setup variables
 }
 
 void Navigate_Pololu::leftWheelInterrupt(){
-    leftCounts++;
-    UpdatePosition();
+    if(robot.leftDir == 1)
+        leftCounts++;
+    else
+        leftCounts--;
+    UpdatePosition(); //recalculate position
     previousLeftCounts = leftCounts;
 }
 
 void Navigate_Pololu::rightWheelInterrupt(){
-    rightCounts++;
-    UpdatePosition();
+    if(robot.rightDir == 1)
+        rightCounts++;
+    else
+        rightCounts--;
+    UpdatePosition(); //recalculate position
     previousRightCounts = rightCounts;
 }
 
@@ -36,16 +42,16 @@
     deltaRight = rightCounts - previousRightCounts;
     deltaDistance = 0.5f * (float)(deltaLeft + deltaRight) * distancePerCount;
     deltaHeading = (float) (deltaRight - deltaLeft) * radiansPerCount;
-    heading += deltaHeading;
-    deltaX = deltaDistance * (float)cos(heading);
+    heading += deltaHeading; //add deltaHeading to running total
+    deltaX = deltaDistance * (float)cos(heading); //convert from polar to cartesian coordinates
     deltaY = deltaDistance * (float)sin(heading);
-    X += deltaX;
+    X += deltaX; //add to running coordinate totals
     Y += deltaY;
-    if(heading>PI)
+    if(heading>PI) //keep heading between pi and -pi
         heading -= TWO_PI;
     else if (heading <= -PI)
         heading += TWO_PI;
-    printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading);
+    printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading); //print coorindates after update
 }
 
 int Navigate_Pololu::getX(){
@@ -57,21 +63,22 @@
 }
 
 int Navigate_Pololu::getHeading(){
-    return (int)((heading/PI)*180);
+    return (int)((heading/PI)*180); //returns heading in degrees
 }
 
-void Navigate_Pololu::calibratePos(PinName robotTx,PinName robotRx){
-    m3pi robot(D7, robotTx, robotRx);
+void Navigate_Pololu::calibratePos(){
+    //m3pi robot(D7, robotTx, robotRx); //create new m3pi object with D7 unused pin for reset
     wait(1);
+    //align wheels
     while(leftOpto){
         robot.left_motor(0.2);
         wait(0.01);
     }
-        robot.stop();
-        wait(0.1);
+    robot.stop();
+    wait(0.1);
     while(rightOpto){
-            robot.right_motor(0.2);
-            wait(0.01);
-        }
-        robot.stop();
+        robot.right_motor(0.2);
+        wait(0.01);
+    }
+    robot.stop();
 }
\ No newline at end of file