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: main.cpp

Revision:
22:bfec16575c91
Parent:
21:48248c5b8992
Child:
23:4d8173c5183b
Child:
24:08241be546ba
--- a/main.cpp	Fri May 17 06:39:44 2013 +0000
+++ b/main.cpp	Sun May 19 07:35:16 2013 +0000
@@ -114,14 +114,25 @@
 int main()
 {
 
+    int garagenumber = 1;
+    float x = -2.954f + 0.308 * garagenumber;
+    float ypre = 1.30f;
+    float ygoal = 0.80f;
+
     /**
-     * Initialze the filt PLOTS.txt,
+     * Check at first the Battery voltage. Starts when the voltages greater as the min is.
      * start the timer for the Logging to the file
      * and start the Task for logging
      **/
+    state.start();
+    state.initPlotFile();
+    state.closePlotFile();
+    while(s.voltageBattery < BAT_MIN) {}
+    state.stop();
+    wait(0.5);
+    state.start();
     state.initPlotFile();
     state.startTimerFromZero();
-    state.start();
 
     /**
      * Clear all Errors of the ESCON Module, with a disabled to enable event
@@ -134,99 +145,95 @@
     /**
      * Set the startposition and start the Task for controlling the roboter.
      */
-    robotControl.setAllToZero(0, 0, PI/2 );
-    //robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
+    // robotControl.setAllToZero(0, 0, PI/2 );
+    robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
     robotControl.start();
-
-    //  Epä Parkour fahrä
-    /*
-           robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
-           wait(0.1);
+    robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
+    wait(0.1);
+    state.savePlotFile(s);
 
-           robotControl.setDesiredPositionAndAngle(-1.50f, 1.50f,  3*PI/4);
-           while(!(robotControl.getDistanceError() <= 0.9)) {
-               state.savePlotFile(s);
-           };
+    /**
+     * Sets the desired Points.
+     */
+    robotControl.setDesiredPositionAndAngle(-1.35f, 1.85f,  3*PI/8);
+    while(!(robotControl.getDistanceError() <= 0.7)) {
+        state.savePlotFile(s);
+    };
 
-           robotControl.setDesiredPositionAndAngle(-1.20f, 2.1f,  PI/4);
-           while(!(robotControl.getDistanceError() <= 0.7)) {
-               state.savePlotFile(s);
-           };
-
-           robotControl.setDesiredPositionAndAngle(-0.5f, 3.0f,  PI/2); //0.75
-           while(!(robotControl.getDistanceError() <= 0.7)) {
-               state.savePlotFile(s);
-           };
+    robotControl.setDesiredPositionAndAngle(-0.75f, 2.85f,  3*PI/8);
+    while(!(robotControl.getDistanceError() <= 0.7)) {
+        state.savePlotFile(s);
+    };
 
-           robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f,  3*PI/4);
-           while(!(robotControl.getDistanceError() <= 0.55)) {
-               state.savePlotFile(s);
-           };
+    robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f,  3*PI/4);
+    while(!(robotControl.getDistanceError() <= 0.55)) {
+        state.savePlotFile(s);
+    };
 
-
-           robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f,  PI);
-           while(!(robotControl.getDistanceError() <= 0.05)) {
-               state.savePlotFile(s);
-           };
+    robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f,  PI);
+    while(!(robotControl.getDistanceError() <= 0.05)) {
+        state.savePlotFile(s);
+    };
 
-           robotControl.setDesiredPositionAndAngle(-2.6f, 3.0f,  -PI/2);
-           while(!(robotControl.getDistanceError() <= 1.0)) {
-               state.savePlotFile(s);
-           };
-
-                   robotControl.setDesiredPositionAndAngle(-2.0f, 2.0f,  -PI/2);
-           while(!(robotControl.getDistanceError() <= 1.0)) {
-               state.savePlotFile(s);
-           };
+    robotControl.setDesiredPositionAndAngle(-2.1f, 3.0f,  -PI/2);
+    while(!(robotControl.getDistanceError() <= 1.0)) {
+        state.savePlotFile(s);
+    };
 
-           robotControl.setDesiredPositionAndAngle(-2.65f, 1.30f,  -PI/2);
-           while(!(robotControl.getDistanceError() <= 0.4)) {
-               state.savePlotFile(s);
-           };
-           robotControl.setDesiredPositionAndAngle(-2.65f, 0.80f,  -PI/2);
-           while(!(s.millis >= 32000)) {
-               state.savePlotFile(s);
-           }
-    */
+    robotControl.setDesiredPositionAndAngle(-2.0f, 1.8f,  -PI/2);
+    while(!(robotControl.getDistanceError() <= 1.0)) {
+        state.savePlotFile(s);
+    };
+
+    robotControl.setDesiredPositionAndAngle(x, ypre,  -PI/2);
+    while(!(robotControl.getDistanceError() <= 0.4)) {
+        state.savePlotFile(s);
+    };
+    robotControl.setDesiredPositionAndAngle(x, ygoal, -PI/2);
+    while(!(s.millis >= 19000)) {
+        state.savePlotFile(s);
+    }
 
 
 
 
-    pc.baud(460800);
-    pc.printf("********************* MicroBridge 4568 ********************************\n\r");
 
-    init();
+    /*
+        pc.baud(460800);
+        pc.printf("********************* MicroBridge 4568 ********************************\n\r");
 
-    // Initialise the ADB subsystem.
-
-    pc.printf("connection isOpen\n");
+        init();
 
-    while(getDesiredTheta() < (4 * PI)) {
+        // Initialise the ADB subsystem.
 
-        ADB::poll();
+        pc.printf("connection isOpen\n");
 
-        if (getDesiredTheta() > (2 * PI)) {
-            //robotControl.setAllToZero(0, 0, PI/2 );
-            robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
-            robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
-        }
-        else
-        {
-        robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta());
+        while(getDesiredTheta() < (4 * PI)) {
+
+            ADB::poll();
 
-         
-        state.savePlotFile(s);
-        //void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b)
-        }
-        
-        writeActualPosition(robotControl.getxActualPosition(),robotControl.getyActualPosition(), robotControl.getActualTheta(), s.state&STATE_UNDER, s.state&STATE_LEFT, s.state&STATE_RIGHT, s.voltageBattery);
-        //connection->write(sizeof(str),(unsigned char*)&str);
-        
-        wait(0.25);
-
-    }
+            if (getDesiredTheta() > (2 * PI)) {
+                //robotControl.setAllToZero(0, 0, PI/2 );
+                robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
+                robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
+            }
+            else
+            {
+            robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta());
 
 
+            state.savePlotFile(s);
+            //void writeActualPosition(float x, float y, float t, int state_u, int state_l, int state_r, float volt_b)
+            }
+
+            writeActualPosition(robotControl.getxActualPosition(),robotControl.getyActualPosition(), robotControl.getActualTheta(), s.state&STATE_UNDER, s.state&STATE_LEFT, s.state&STATE_RIGHT, s.voltageBattery);
+            //connection->write(sizeof(str),(unsigned char*)&str);
+
+            wait(0.25);
+
+        }
+
+    */
     /**
      * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm
      */