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:
1:6cd533a712c6
Parent:
0:31f7be68e52d
Child:
2:d8e1613dc38b
--- a/RobotControl/RobotControl.cpp	Thu Feb 07 17:43:19 2013 +0000
+++ b/RobotControl/RobotControl.cpp	Sat Mar 02 09:39:34 2013 +0000
@@ -2,12 +2,12 @@
 
 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;
     this->motorControllerRight = motorControllerRight;
-    this->compass = compass;
+    //  this->compass = compass;
     this->period = period;
 
     /* initialize peripherals */
@@ -26,7 +26,7 @@
 
 }
 
-RobotControl::~RobotControl() 
+RobotControl::~RobotControl()
 {
 
 }
@@ -42,15 +42,14 @@
     return (motorControllerLeft->isEnabled() && motorControllerRight->isEnabled());
 }
 
-void RobotControl::setAcceleration(float acc)
+void RobotControl::setAcceleration(float acceleration)
 {
-    Desired.setAcceleration(acc);
-
+    Desired.setAcceleration(acceleration);
 }
 
-void RobotControl::setThetaAcceleration(float acc)
+void RobotControl::setThetaAcceleration(float acceleration)
 {
-    Desired.setThetaAcceleration(acc);
+    Desired.setThetaAcceleration(acceleration);
 }
 
 void RobotControl::setDesiredSpeed(float speed)
@@ -63,14 +62,14 @@
     this->omega = omega;
 }
 
-void RobotControl::setxPosition(float position)
+void RobotControl::setxPosition(float xposition)
 {
-    Desired.xposition = position;
+    Desired.xposition = xposition;
 }
 
-void RobotControl::setyPosition(float position)
+void RobotControl::setyPosition(float yposition)
 {
-    Desired.yposition = position;
+    Desired.yposition = yposition;
 }
 
 void RobotControl::setTheta(float theta)
@@ -78,6 +77,11 @@
     Desired.theta = theta;
 }
 
+float RobotControl::getTheta()
+{
+    return Desired.theta;
+}
+
 float RobotControl::getDesiredSpeed()
 {
     return speed;
@@ -128,9 +132,24 @@
     return Desired.getTheta()-Actual.getTheta();
 }
 
-void RobotControl::setAllToZero(float xZeroPos, float yZeroPos)
+float RobotControl::getDistanceError()
+{
+    return sqrt( ( getxPositionError() * getxPositionError() ) + (getyPositionError() * getyPositionError() ) );
+}
+
+float RobotControl::getThetaErrorToGoal()
 {
-    Actual.setState(xZeroPos, yZeroPos, 0.0f, 0.0f, 0.0f);
+    return atan2(getyPositionError(),getxPositionError()) - getActualTheta(); // PI ist weg weil auch negative Zahlen zugelassen werden.
+}
+
+float RobotControl::getThetaGoal()
+{
+    return atan2(getyPositionError(),getxPositionError()) - getTheta();
+}
+
+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);
     stateLeft.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
     stateRight.setState(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
@@ -141,9 +160,15 @@
 
 void RobotControl::run()
 {
+    ///// DAs kan glaub raus ab hier
+
     /* motion planning */
     if (isEnabled()) {
+        ///// DAs kan glaub raus bis hier
         Desired.increment(speed, omega, period);
+
+        ///// DAs kan glaub raus vis hier
+
     } else {
         speed = 0.0f;
         omega = 0.0f;
@@ -153,8 +178,8 @@
     /* position calculation */
 
     /* Set the state of speed from Left und Right Wheel*/
-    stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
-    stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI;
+    stateLeft.speed = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI * GEAR;
+    stateRight.speed = - motorControllerRight->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI * GEAR;
 
     /* translational speed of the Robot (average) */
     Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f;
@@ -162,7 +187,7 @@
     /* rotational speed of the Robot  */
     Actual.omega = ( stateRight.speed - stateLeft.speed  ) / WHEEL_DISTANCE;
 
-    /* rotational theta of the Robot */
+    /* rotational theta of the Robot integrate the omega with the time*/
     Actual.theta += Actual.omega * period;
     if(Actual.theta <= -PI) {
         Actual.theta  += 2*  PI;
@@ -171,40 +196,33 @@
     } else {
         //nothing
     }
-    Actual.theta += Actual.omega * period;
-    Actual.thetaCompass = compass->getFilteredAngle();
+
+    /* 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));
 
-       /* translational X and Y Position. integrate the speed with the time */ 
-  //  Actual.xposition += (Actual.speed * period * sin(Actual.theta));
-  //  Actual.yposition += (Actual.speed * period * cos(Actual.theta));
-    
-        /* translational X and Y Position. integrate the speed with the time theta from compass */
-    Actual.xposition += - (Actual.speed * period * sin(Actual.thetaCompass));
-    Actual.yposition += (Actual.speed * period * cos(Actual.thetaCompass));
-    
-    
-    
+
+    //  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));
     
-    /* postition control calculate */
-    
-    rho = sqrt( pow(Actual.xposition,2) + pow(Actual.yposition,2));
-    
-    
-    
-    
+    /* motor control */
+    if ( isEnabled() &&  ( getDistanceError() >= MIN_DISTANCE_ERROR ) )  {
 
-    /* motor control */
-    if (isEnabled()) {
+        /* postition control */
+         
+        speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
+        omega = K2 * getThetaErrorToGoal() + K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
 
-        motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) );
-        motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) );
-
+        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 );
 
     } else {
 
         motorControllerLeft->setVelocity(0.0f);
         motorControllerRight->setVelocity(0.0f);
 
-    }    
+    }
 }