Christian Burri / Mbed 2 deprecated autonomous Robot Android

This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf

Dependencies:   mbed

Fork of autonomous Robot Android by Christian Burri

Diff: RobotControl/RobotControl.cpp

Revision:
2:d8e1613dc38b
Parent:
1:6cd533a712c6
Child:
3:92ba0254af87
--- a/RobotControl/RobotControl.cpp	Sat Mar 02 09:39:34 2013 +0000
+++ b/RobotControl/RobotControl.cpp	Sun Mar 03 16:26:47 2013 +0000
@@ -23,7 +23,6 @@
 
     Desired.setAcceleration(ACCELERATION);
     Desired.setThetaAcceleration(THETA_ACCELERATION);
-
 }
 
 RobotControl::~RobotControl()
@@ -77,6 +76,13 @@
     Desired.theta = theta;
 }
 
+void RobotControl::setPositionAngle(float xposition, float yposition, float theta)
+{
+    setxPosition(xposition);
+    setyPosition(yposition);
+    setTheta(theta);
+}
+
 float RobotControl::getTheta()
 {
     return Desired.theta;
@@ -139,12 +145,32 @@
 
 float RobotControl::getThetaErrorToGoal()
 {
-    return atan2(getyPositionError(),getxPositionError()) - getActualTheta(); // PI ist weg weil auch negative Zahlen zugelassen werden.
+    float temp;
+    temp = atan2(getyPositionError(),getxPositionError()) - getActualTheta();
+
+    if(temp <= -PI) {
+        temp  += 2*  PI;
+    } else if (temp > PI) {
+        temp  -= 2*  PI;
+    } else {
+        //nothing
+    }
+    return temp;
 }
 
 float RobotControl::getThetaGoal()
 {
-    return atan2(getyPositionError(),getxPositionError()) - getTheta();
+    float temp;
+    temp = atan2(getyPositionError(),getxPositionError()) - getTheta();
+
+    if(temp <= -PI) {
+        temp  += 2*  PI;
+    } else if (temp > PI) {
+        temp  -= 2*  PI;
+    } else {
+        //nothing
+    }
+    return temp;
 }
 
 void RobotControl::setAllToZero(float xZeroPos, float yZeroPos, float theta)
@@ -157,17 +183,16 @@
     omega = 0.0f;
 }
 
-
 void RobotControl::run()
 {
     ///// DAs kan glaub raus ab hier
-
+/////////////////////////////////////////////////////////7
     /* motion planning */
     if (isEnabled()) {
         ///// DAs kan glaub raus bis hier
         Desired.increment(speed, omega, period);
 
-        ///// DAs kan glaub raus vis hier
+        ///// DAs kan glaub raus bis hier
 
     } else {
         speed = 0.0f;
@@ -201,19 +226,19 @@
     Actual.xposition += (Actual.speed * period * cos(Actual.theta));
     Actual.yposition += (Actual.speed * period * sin(Actual.theta));
 
-
     //  Actual.thetaCompass = compass->getFilteredAngle();
     /* translational X and Y Position. integrate the speed with the time theta from compass */
     //  Actual.xposition += - (Actual.speed * period * cos(Actual.thetaCompass));
     //  Actual.yposition += (Actual.speed * period * sin(Actual.thetaCompass));
-    
+
     /* motor control */
     if ( isEnabled() &&  ( getDistanceError() >= MIN_DISTANCE_ERROR ) )  {
 
         /* postition control */
-         
+
         speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
-        omega = K2 * getThetaErrorToGoal() + K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
+        omega = K2 * getThetaErrorToGoal() +
+                K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
 
         motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) * GEAR );
         motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) * GEAR );
@@ -225,4 +250,3 @@
 
     }
 }
-