Diff: main.cpp

Revision:

4:3a97923ff2d4

Parent:

3:92ba0254af87

Child:

5:48a258f6335e

--- a/main.cpp	Thu Mar 07 09:47:07 2013 +0000
+++ b/main.cpp	Fri Mar 08 09:54:34 2013 +0000
@@ -10,17 +10,17 @@
  * @section DESCRIPTION
  *
  * This Programm is for a autonomous robot for the competition
- * at the Hochschule Luzern. 
+ * at the Hochschule Luzern.
  * We are one of the 32 teams. In the team #1 is:
- * - Bauernfeind Julia <B>WI</B> <a href="julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a> 
- * - Büttler Pirmin <B>WI</B> <a href="pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a> 
- * - Amberg Reto <B>I</B> <a href="reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a> 
- * - Galliker Arno <B>I</B> <a href="arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a> 
- * - Amrein Marcel <B>M</B> <a href="marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a> 
- * - Flühler Ramon <B>M</B> <a href="ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a> 
+ * - Bauernfeind Julia <B>WI</B> <a href="julia.bauernfeind@stud.hslu.ch">julia.bauernfeind@stud.hslu.ch</a>
+ * - Büttler Pirmin <B>WI</B> <a href="pirmin.buetler@stud.hslu.ch">pirmin.buetler@stud.hslu.ch</a>
+ * - Amberg Reto <B>I</B> <a href="reto.amberg@stud.hslu.ch">reto.amberg@stud.hslu.ch</a>
+ * - Galliker Arno <B>I</B> <a href="arno.galliker@stud.hslu.ch">arno.galliker@stud.hslu.ch</a>
+ * - Amrein Marcel <B>M</B> <a href="marcel.amrein@stud.hslu.ch">marcel.amrein@stud.hslu.ch</a>
+ * - Flühler Ramon <B>M</B> <a href="ramon.fluehler@stud.hslu.ch">ramon.fluehler@stud.hslu.ch</a>
  * - Burri Christian <B>ET</B> <a href="christian.burri@stud.hslu.ch">christian.burri@stud.hslu.ch</a>
- *  
- * The postition control is based on polar coordiantes. 
+ *
+ * The postition control is based on polar coordiantes.
  * 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>
  *
  */
@@ -84,7 +84,7 @@
     //  robotControl.start();
     //  compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     //  compass.start();
-    
+
     state.initPlotFile();
 
     robotControl.start();
@@ -92,7 +92,7 @@
     wait(0.01);
     robotControl.setEnable(true);
     wait(0.01);
-    robotControl.setAllToZero(0, 0, PI/2 );
+    robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
 
     leftMotor.setPulses(0);
     rightMotor.setPulses(0);
@@ -100,25 +100,41 @@
     state.startTimerFromZero();
     state.start();
 
-    robotControl.setPositionAngle(0.0f, 1.0f,  17*PI/18);
-    while(!(s.millis >= 15000)) {
+    robotControl.setPositionAngle(-1.20f, 1.50f,  3*PI/4);
+    while(!(robotControl.getDistanceError() <= 0.4)) {
         state.savePlotFile(s);
     };
 
-    robotControl.setPositionAngle(-1.0f, 1.0f,  -PI/2);
-    while(!(s.millis >= 30000)) {
+    robotControl.setPositionAngle(-1.20f, 2.6f,  PI/4);
+    while(!(robotControl.getDistanceError() <= 0.4)) {
+        state.savePlotFile(s);
+    };
+
+    robotControl.setPositionAngle(-0.45f, 3.4f,  PI/2);
+    while(!(robotControl.getDistanceError() <= 0.4)) {
         state.savePlotFile(s);
     };
 
-    robotControl.setPositionAngle(-1.0f, 0.0f,  0.0f);
-    while(!(s.millis >= 45000)) {
+    robotControl.setPositionAngle(-1.0f, 3.6f,  PI);
+    while(!(robotControl.getDistanceError() <= 0.4)) {
         state.savePlotFile(s);
     };
 
-    robotControl.setPositionAngle(0.0f, 1.0f, PI/2);
-    while(!(s.millis >= 63000)) {
+    robotControl.setPositionAngle(-1.5f, 3.9f,  PI);
+    while(!(robotControl.getDistanceError() <= 0.03)) {
         state.savePlotFile(s);
     };
+    
+        robotControl.setPositionAngle(-2.5f, 3.0f,  -PI/2);
+    while(!(robotControl.getDistanceError() <= 0.4)) {
+        state.savePlotFile(s);
+    };
+    
+            robotControl.setPositionAngle(-1.75f, 1.30f,  -PI/2);
+    while(!(robotControl.getDistanceError() <= 0.04)) {
+        state.savePlotFile(s);
+    };
+
 
     state.savePlotFile(s);
     state.closePlotFile();