Diff: main.cpp

Revision:

15:cb1337567ad4

Parent:

14:6a45a9f940a8

Child:

16:b5d949136a21

--- a/main.cpp	Thu Apr 11 09:22:35 2013 +0000
+++ b/main.cpp	Fri Apr 26 06:02:41 2013 +0000
@@ -7,7 +7,7 @@
  *
  * @brief
  *
- * This Programm is for a autonomous robot for the competition
+ * This program is for an autonomous robot for the competition
  * at the Hochschule Luzern.
  * We are one of the 32 teams. In the team #1 is:
  * - Bauernfeind Julia <B>WI</B> <a href="mailto:julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a>
@@ -20,13 +20,13 @@
  *
  * 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">a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
+ * 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>
  *
- * The connection to a android smartphone is realise with the library AndroidAccessory from Rich Bayliss.
- * For more information see here: <a href="https://mbed.org/users/richbayliss/code/AndroidAccessory/docs/tip/">a href="https://mbed.org/users/richbayliss/code/AndroidAccessory/docs/tip/</a>
+ * The connection to an android smartphone is realise with the library AndroidAccessory from Rich Bayliss.
+ * For more information see here: <a href="https://mbed.org/users/richbayliss/code/AndroidAccessory/docs/tip/">https://mbed.org/users/richbayliss/code/AndroidAccessory/docs/tip/</a>
  * 
  * The rest of the classes are only based on standard library from mbed.
- * For more information see here: <a href="http://mbed.org/users/mbed_official/code/mbed/">a href="http://mbed.org/users/mbed_official/code/mbed/</a>
+ * For more information see here: <a href="http://mbed.org/users/mbed_official/code/mbed/">http://mbed.org/users/mbed_official/code/mbed/</a>
  */
 
 /**
@@ -123,7 +123,7 @@
      * start the timer for the Logging to the file
      * and start the Task for logging
      **/
-    state.initPlotFile();
+    //state.initPlotFile();
     state.startTimerFromZero();
     state.start();
 
@@ -139,7 +139,7 @@
      * 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(START_X_OFFSET, START_Y_OFFSET, PI/2 );
     robotControl.start();
     
     pc.baud(460800);
@@ -149,118 +149,14 @@
     pc.printf("Android Development Kit: start\r\n");
     USBInit();
     
+    //wait(3);
+    
     adkTerm.start();
 
-
-    // robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
-    //  robotControl.setDesiredPositionAndAngle(0, 0, PI/2);
-    // wait(0.1);
-
-    //////////////////////////////////////////
-/*
-    robotControl.setDesiredPositionAndAngle(0.0f, 1.0f,  PI);
-    while(!(robotControl.getDistanceError() <= 0.1)) {
-        state.savePlotFile(s);
-    };
-
-    robotControl.setDesiredPositionAndAngle(-1.00f, 1.0f,  -PI/2);
-    while(!(robotControl.getDistanceError() <= 0.1)) {
-        state.savePlotFile(s);
-    };
-
-    robotControl.setDesiredPositionAndAngle(0.0f, -0.8f,  PI/2);
-    while(!(robotControl.getDistanceError() <= 0.05)) {
-        state.savePlotFile(s);
-    };
-
-    robotControl.setDesiredPositionAndAngle(0.0f, -0.2f,  PI/2);
-    while(!(robotControl.getDistanceError() <= 0.05)) {
-        state.savePlotFile(s);
-    };
-
-    robotControl.setDesiredPositionAndAngle(0.0, 0.0f,  PI/2);
-    while(!(s.millis >= 32000)) {
-        state.savePlotFile(s);
-    };
-
-*/
-
-
-
-    ///////////////////////stay
-    /*
-            robotControl.setDesiredPositionAndAngle(0.0, 0.0f,  PI/2);
-        while(!(s.millis >= 25000)) {
-            state.savePlotFile(s);
-        };
-        */
-    //////////////////////////stay
-
-
-
-
-
-
-
-    ////////////////////////////////////////////////////////////////////////
-
-    /*
-
-        //Zum Umfang einstellen 2m fahren
-        robotControl.setDesiredPositionAndAngle(0.0f, 0.5f,  PI/2);
-        while(!(robotControl.getDistanceError() <= 0.2)) {
-            state.savePlotFile(s);
-        };
-
-            robotControl.setDesiredPositionAndAngle(0.0f, 1.0f,  PI/2);
-        while(!(robotControl.getDistanceError() <= 0.2)) {
-            state.savePlotFile(s);
-        };
-                robotControl.setDesiredPositionAndAngle(0.0f, 1.5f,  PI/2);
-        while(!(robotControl.getDistanceError() <= 0.2)) {
-            state.savePlotFile(s);
-        };
-
-
-        robotControl.setDesiredPositionAndAngle(0.0f, 2.0f,  PI/2);
-        while(!(s.millis >= 30000)) {
-            state.savePlotFile(s);
-        };
-
-    */
-
-
-    ///////////////oder//////////////////e
-
-
-    // Zum radabstand einstellen
-
-    /*
-        robotControl.setDesiredPositionAndAngle(-0.7f, 1.0f,  PI);
-        while(!(robotControl.getDistanceError() <= 0.05)) {
-            state.savePlotFile(s);
-        };
-
-        robotControl.setDesiredPositionAndAngle(-0.8f, 1.0f,  PI);
-        while(!(robotControl.getDistanceError() <= 0.05)) {
-            state.savePlotFile(s);
-        };
-
-
-        robotControl.setDesiredPositionAndAngle(-1.0f, 1.0f,  PI);
-        while(!(s.millis >= 30000)) {
-            state.savePlotFile(s);
-        }
-
-    */
-
-
-////////////////////////////////////////////////////////
-
-
+  /*
 
     //  Epä Parkour fahrä
-    /*
+    
         robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
         wait(0.1);
 
@@ -274,7 +170,7 @@
             state.savePlotFile(s);
         };
 
-        robotControl.setDesiredPositionAndAngle(-0.75f, 3.0f,  PI/2);
+        robotControl.setDesiredPositionAndAngle(-0.55f, 3.0f,  PI/2); //0.75
         while(!(robotControl.getDistanceError() <= 0.7)) {
             state.savePlotFile(s);
         };
@@ -294,27 +190,36 @@
         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(-1.74f, 1.30f,  -PI/2);
+        robotControl.setDesiredPositionAndAngle(-2.65f, 1.30f,  -PI/2);
         while(!(robotControl.getDistanceError() <= 0.1)) {
             state.savePlotFile(s);
         };
-        robotControl.setDesiredPositionAndAngle(-1.75f, 0.80f,  -PI/2);
+        robotControl.setDesiredPositionAndAngle(-2.65f, 0.80f,  -PI/2);
         while(!(s.millis >= 32000)) {
             state.savePlotFile(s);
         }
 
     */
-       
+     
         while (1) {
             USBLoop();
+            
+            robotControl.setDesiredPositionAndAngle(adkTerm.getDesiredX(), adkTerm.getDesiredY(),  adkTerm.getDesiredTheta());
+            
+            //pc.printf(".");
 
-            pc.printf("From Android x: %f   y: %f  t: %f",adkTerm.getDesiredX(), adkTerm.getDesiredY(),adkTerm.getDesiredTheta());
-            robotControl.setDesiredPositionAndAngle(adkTerm.getDesiredX(), adkTerm.getDesiredY(),  adkTerm.getDesiredTheta());
+            /*pc.printf("From Android x: %f   y: %f  t: %f",adkTerm.getDesiredX(), adkTerm.getDesiredY(),adkTerm.getDesiredTheta());
+            
             
             pc.printf( "To Android: x: %f   y: %f   t: %f; \n \n", robotControl.getxActualPosition(),
              robotControl.getyActualPosition(),
-             robotControl.getActualTheta());
+             robotControl.getActualTheta());*/
         }
 
 
@@ -322,10 +227,9 @@
     /**
      * Close the File PLOTS.txt to read the file after with the computer and draw a diagramm
      */
-     /*
     state.savePlotFile(s);
     state.closePlotFile();
     state.stop();
     robotControl.setEnable(false);
-    */
+    
 }