Diff: RobotControl/RobotControl.cpp

Revision:

3:92ba0254af87

Parent:

2:d8e1613dc38b

Child:

5:48a258f6335e

diff -r d8e1613dc38b -r 92ba0254af87 RobotControl/RobotControl.cpp
--- a/RobotControl/RobotControl.cpp	Sun Mar 03 16:26:47 2013 +0000
+++ b/RobotControl/RobotControl.cpp	Thu Mar 07 09:47:07 2013 +0000
@@ -145,7 +145,8 @@
 
 float RobotControl::getThetaErrorToGoal()
 {
-    float temp;
+    return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta());
+    /*float temp;
     temp = atan2(getyPositionError(),getxPositionError()) - getActualTheta();
 
     if(temp <= -PI) {
@@ -155,14 +156,14 @@
     } else {
         //nothing
     }
-    return temp;
+    return temp;*/
 }
 
 float RobotControl::getThetaGoal()
 {
-    float temp;
-    temp = atan2(getyPositionError(),getxPositionError()) - getTheta();
+    return PiRange(atan2(getyPositionError(),getxPositionError()) - getTheta());
 
+    /*
     if(temp <= -PI) {
         temp  += 2*  PI;
     } else if (temp > PI) {
@@ -170,7 +171,7 @@
     } else {
         //nothing
     }
-    return temp;
+    return temp;*/
 }
 
 void RobotControl::setAllToZero(float xZeroPos, float yZeroPos, float theta)
@@ -214,14 +215,16 @@
 
     /* rotational theta of the Robot integrate the omega with the time*/
     Actual.theta += Actual.omega * period;
-    if(Actual.theta <= -PI) {
-        Actual.theta  += 2*  PI;
-    } else if (Actual.theta > PI) {
-        Actual.theta  -= 2*  PI;
-    } else {
-        //nothing
-    }
-
+    Actual.theta = PiRange(Actual.theta);
+    /*
+        if(Actual.theta <= -PI) {
+            Actual.theta  += 2*  PI;
+        } else if (Actual.theta > PI) {
+            Actual.theta  -= 2*  PI;
+        } else {
+            //nothing
+        }
+    */
     /* translational X and Y Position. integrate the speed with the time */
     Actual.xposition += (Actual.speed * period * cos(Actual.theta));
     Actual.yposition += (Actual.speed * period * sin(Actual.theta));
@@ -250,3 +253,15 @@
 
     }
 }
+
+float RobotControl::PiRange(float theta)
+{
+    if(theta <= -PI) {
+        return theta  += 2*PI;
+    } else if (theta > PI) {
+        return theta  -= 2*PI;
+    } else {
+        return theta;
+    }
+}
+