Christian Burri / Mbed 2 deprecated autonomousRobotAndroid

This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> 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: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>

Dependencies:   mbed

Fork of autonomousRobotAndroid by Christian Burri

Diff: RobotControl/RobotControl.cpp

Revision:
6:48eeb41188dd
Parent:
5:48a258f6335e
Child:
8:696c2f9dfc62
diff -r 48a258f6335e -r 48eeb41188dd RobotControl/RobotControl.cpp
--- a/RobotControl/RobotControl.cpp	Thu Mar 21 08:56:53 2013 +0000
+++ b/RobotControl/RobotControl.cpp	Sat Mar 23 13:52:48 2013 +0000
@@ -2,7 +2,10 @@
 
 using namespace std;
 
-RobotControl::RobotControl(MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ float period) : Task(period)
+RobotControl::RobotControl(MaxonESCON* motorControllerLeft,
+                           MaxonESCON* motorControllerRight,
+                           /*HMC6352* compass,*/
+                           float period) : Task(period)
 {
     /* get peripherals */
     this->motorControllerLeft = motorControllerLeft;
@@ -91,7 +94,9 @@
     return Desired.theta;
 }
 
-void RobotControl::setDesiredPositionAndAngle(float xposition, float yposition, float theta)
+void RobotControl::setDesiredPositionAndAngle(float xposition,
+        float yposition,
+        float theta)
 {
     setDesiredxPosition(xposition);
     setDesiredyPosition(yposition);
@@ -161,35 +166,16 @@
 float RobotControl::getThetaErrorToGoal()
 {
     return PiRange(atan2(getyPositionError(),getxPositionError()) - getActualTheta());
-    /*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 PiRange(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)
+void RobotControl::setAllToZero(float xZeroPos,
+                                float yZeroPos,
+                                float theta)
 {
     Actual.setState(xZeroPos, yZeroPos, theta, 0.0f, 0.0f);
     Desired.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
@@ -219,8 +205,10 @@
     /* position calculation */
 
     /* Set the state of speed from Left und Right Wheel*/
-    stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI * GEAR;
-    stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI * GEAR;
+    stateLeft.speed = motorControllerLeft->getActualSpeed() *
+                      2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
+    stateRight.speed = - motorControllerRight->getActualSpeed() *
+                       2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR;
 
     /* translational speed of the Robot (average) */
     Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f;
@@ -231,15 +219,7 @@
     /* rotational theta of the Robot integrate the omega with the time*/
     Actual.theta += Actual.omega * period;
     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));
@@ -256,10 +236,13 @@
 
         speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
         omega = K2 * getThetaErrorToGoal() +
-                K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
+                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) );
+        motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) /
+                                          (2 * WHEEL_RADIUS_LEFT * PI * GEAR) );
+        motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) /
+                                          (2 * WHEEL_RADIUS_RIGHT * PI * GEAR) );
 
     } else {
 
@@ -279,4 +262,3 @@
         return theta;
     }
 }
-