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

Files at this revision

API Documentation at this revision

Revision 5:48a258f6335e, committed 2013-03-21

Comitter:
chrigelburri
Date:
Thu Mar 21 08:56:53 2013 +0000
Parent:
4:3a97923ff2d4
Child:
6:48eeb41188dd
Commit message:
vor dem Gr?nden m?nd?li;

Changed in this revision

RobotControl/RobotControl.cpp Show annotated file Show diff for this revision Revisions of this file
RobotControl/RobotControl.h Show annotated file Show diff for this revision Revisions of this file
StateDefines/State.cpp Show annotated file Show diff for this revision Revisions of this file
StateDefines/defines.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/RobotControl/RobotControl.cpp	Fri Mar 08 09:54:34 2013 +0000
+++ b/RobotControl/RobotControl.cpp	Thu Mar 21 08:56:53 2013 +0000
@@ -61,26 +61,41 @@
     this->omega = omega;
 }
 
-void RobotControl::setxPosition(float xposition)
+void RobotControl::setDesiredxPosition(float xposition)
 {
     Desired.xposition = xposition;
 }
 
-void RobotControl::setyPosition(float yposition)
+void RobotControl::setDesiredyPosition(float yposition)
 {
     Desired.yposition = yposition;
 }
 
-void RobotControl::setTheta(float theta)
+void RobotControl::setDesiredTheta(float theta)
 {
     Desired.theta = theta;
 }
 
-void RobotControl::setPositionAngle(float xposition, float yposition, float theta)
+float RobotControl::getDesiredxPosition()
+{
+    return Desired.xposition;
+}
+
+float RobotControl::getDesiredyPosition()
 {
-    setxPosition(xposition);
-    setyPosition(yposition);
-    setTheta(theta);
+    return Desired.yposition;
+}
+
+float RobotControl::getDesiredTheta()
+{
+    return Desired.theta;
+}
+
+void RobotControl::setDesiredPositionAndAngle(float xposition, float yposition, float theta)
+{
+    setDesiredxPosition(xposition);
+    setDesiredyPosition(yposition);
+    setDesiredTheta(theta);
 }
 
 float RobotControl::getTheta()
@@ -243,8 +258,8 @@
         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 );
+        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 {
 

--- a/RobotControl/RobotControl.h	Fri Mar 08 09:54:34 2013 +0000
+++ b/RobotControl/RobotControl.h	Thu Mar 21 08:56:53 2013 +0000
@@ -103,19 +103,37 @@
     * Sets the desired X-position of the robot.
     * @param xposition the desired position, given in [m]
     */
-    void        setxPosition(float xposition);
+    void        setDesiredxPosition(float xposition);
 
     /**
     * Sets the desired Y-position of the robot.
     * @param yposition the desired position, given in [m]
     */
-    void        setyPosition(float yposition);
+    void        setDesiredyPosition(float yposition);
 
     /**
     * Sets the desired θ of the robot.
     * @param theta the desired θ, given in [rad]
     */
-    void        setTheta(float theta);
+    void        setDesiredTheta(float theta);
+    
+        /**
+    * Get the desired X-position of the robot.
+    * @return xposition the desired position, given in [m]
+    */
+    float        getDesiredxPosition();
+
+    /**
+    * Get the desired Y-position of the robot.
+    * @return yposition the desired position, given in [m]
+    */
+    float        getDesiredyPosition();
+
+    /**
+    * Get the desired θ of the robot.
+    * @return theta the desired θ, given in [rad]
+    */
+    float        getDesiredTheta();
 
     /**
     * Sets the desired Position and θ.
@@ -123,7 +141,7 @@
     * @param yposition the desired position, given in [m]
     * @param theta the desired θ, given in [rad]
     */
-    void        setPositionAngle(float xposition, float yposition, float theta);
+    void        setDesiredPositionAndAngle(float xposition, float yposition, float theta);
 
     /**
      * Gets the desired θ of the goal point.

--- a/StateDefines/State.cpp	Fri Mar 08 09:54:34 2013 +0000
+++ b/StateDefines/State.cpp	Thu Mar 21 08:56:53 2013 +0000
@@ -43,17 +43,17 @@
                 "X-AxisError[m]\t"
                 "AngleError[grad]\t"
                 "AngleCar[grad]\t"
-                "SetpointX-Axis[m]\t"
+                "SetpointX-Axis[m]\t" //15
                 "SetpointY-Axis[m]\t"
-                "SetpointAngel[grad]\t"
+                "SetpointAngel[grad]\t" //17
                 "RightDistanceSensor[m]\t"
                 "CompassAngle[grad]\t"
                 "CompassX-Axis\t" //20
                 "CompassY-Axis\t"
                 "State\t"
                 "distanceToGoal[m]\t" //23
-                "angleToGoal[rad]\t"
-                "thetaFromTheGoal[rad]\n");
+                "angleToGoal[grad]\t"
+                "thetaFromTheGoal[grad]\n");
     }
 }
 
@@ -159,6 +159,9 @@
     s->xAxisError = robotControl->getxPositionError();
     s->yAxisError = robotControl->getyPositionError();
     s->angleError = robotControl->getThetaError() * 180 / PI;
+    s->setxAxis = robotControl->getDesiredxPosition();
+    s->setyAxis = robotControl->getDesiredyPosition();
+    s->setAngle = robotControl->getDesiredTheta() * 180 / PI;
     // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
 
     s->rho = robotControl->getDistanceError();

--- a/StateDefines/defines.h	Fri Mar 08 09:54:34 2013 +0000
+++ b/StateDefines/defines.h	Thu Mar 21 08:56:53 2013 +0000
@@ -8,7 +8,7 @@
 
 // maxon motor #339282 EC 45 flat 30W and GEAR 
 #define POLE_PAIRS            8u            // Number of of pole pairs
-#define GEAR                  1.0f          // Gear on the motor
+#define GEAR                  1/11.6f       // Gear on the motor 1/11.6f
 #define PULSES_PER_STEP       6u            // Pulses per step Hallsensor have 6 steps
 
 // Physical Dimension of the car
@@ -22,8 +22,8 @@
 #define STATE_RIGHT           8u            // Bit3 = right ESCON in error state     
 
 // ESCON Constands
-#define ESCON_SET_FACTOR      100.0f        // Speed Factor how set in the ESCON
-#define ESCON_GET_FACTOR      100.0f        // Speed Factor how get in the ESCON       
+#define ESCON_SET_FACTOR      1200.0f       // Speed Factor how set in the ESCON
+#define ESCON_GET_FACTOR      1400.0f       // Speed Factor how get in the ESCON       
 
 // Start Defintition
 #define START_X_OFFSET        -0.8f         // Sets the start X-point [m]
@@ -38,7 +38,7 @@
 #define K1                    1.0f * GAIN
 #define K2                    3.0f * GAIN
 #define K3                    2.0f * GAIN
-#define MIN_DISTANCE_ERROR 0.03             // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m]                 
+#define MIN_DISTANCE_ERROR    0.03          // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m]                 
 
 // LiPo Batterie
 #define BAT_MULTIPLICATOR     21.633333333f // R2 / (R1 + R2) = 0.153    R2= 10k , R1 = 1.8k 1/0.153 = 6.555  ---> 3.3 * 6.555 = 21.6333333f

--- a/main.cpp	Fri Mar 08 09:54:34 2013 +0000
+++ b/main.cpp	Thu Mar 21 08:56:53 2013 +0000
@@ -100,37 +100,40 @@
     state.startTimerFromZero();
     state.start();
 
-    robotControl.setPositionAngle(-1.20f, 1.50f,  3*PI/4);
+    robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
+    wait(0.1);
+
+    robotControl.setDesiredPositionAndAngle(-1.20f, 1.50f,  3*PI/4);
     while(!(robotControl.getDistanceError() <= 0.4)) {
         state.savePlotFile(s);
     };
 
-    robotControl.setPositionAngle(-1.20f, 2.6f,  PI/4);
-    while(!(robotControl.getDistanceError() <= 0.4)) {
-        state.savePlotFile(s);
-    };
-
-    robotControl.setPositionAngle(-0.45f, 3.4f,  PI/2);
+    robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f,  PI/4);
     while(!(robotControl.getDistanceError() <= 0.4)) {
         state.savePlotFile(s);
     };
 
-    robotControl.setPositionAngle(-1.0f, 3.6f,  PI);
+    robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f,  3*PI/4);
     while(!(robotControl.getDistanceError() <= 0.4)) {
         state.savePlotFile(s);
     };
 
-    robotControl.setPositionAngle(-1.5f, 3.9f,  PI);
+    robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f,  PI);
+    while(!(robotControl.getDistanceError() <= 0.2)) {
+        state.savePlotFile(s);
+    };
+
+    robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f,  PI);
     while(!(robotControl.getDistanceError() <= 0.03)) {
         state.savePlotFile(s);
     };
-    
-        robotControl.setPositionAngle(-2.5f, 3.0f,  -PI/2);
+
+    robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f,  -PI/2);
     while(!(robotControl.getDistanceError() <= 0.4)) {
         state.savePlotFile(s);
     };
-    
-            robotControl.setPositionAngle(-1.75f, 1.30f,  -PI/2);
+
+    robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f,  -PI/2);
     while(!(robotControl.getDistanceError() <= 0.04)) {
         state.savePlotFile(s);
     };