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: StateDefines/State.cpp

Revision:
2:d8e1613dc38b
Parent:
1:6cd533a712c6
Child:
4:3a97923ff2d4
--- a/StateDefines/State.cpp	Sat Mar 02 09:39:34 2013 +0000
+++ b/StateDefines/State.cpp	Sun Mar 03 16:26:47 2013 +0000
@@ -15,13 +15,11 @@
     this->robotControl = robotControl;
     this->motorControllerLeft = motorControllerLeft;
     this->motorControllerRight = motorControllerRight;
-   // this->compass = compass;
+    // this->compass = compass;
     this->battery =battery;
     this->period = period;
-
 }
 
-
 State::~State() {}
 
 void State::initPlotFile(void)
@@ -50,9 +48,12 @@
                 "SetpointAngel[grad]\t"
                 "RightDistanceSensor[m]\t"
                 "CompassAngle[grad]\t"
-                "CompassX-Axis\t"
+                "CompassX-Axis\t" //20
                 "CompassY-Axis\t"
-                "State\n");
+                "State\t"
+                "distanceToGoal[m]\t" //23
+                "angleToGoal[rad]\t"
+                "thetaFromTheGoal[rad]\n");
     }
 }
 
@@ -61,7 +62,7 @@
 {
     char buf[256];
 
-    sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d",
+    sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d\t%f\t%f\t%f",
             s.millis,
             s.voltageBattery,
             s.leftPulses,
@@ -83,13 +84,15 @@
             s.compassAngle,
             s.compassxAxis,
             s.compassyAxis,
-            s.state
+            s.state,
+            s.rho,
+            s.lamda,
+            s.delta
            );
 
     if (logp)
         fprintf(logp, buf);
     fprintf(logp, "\n"); // new line
-
 }
 
 void State::savePlotText(char text[])
@@ -97,11 +100,9 @@
     fprintf(logp, text);
 }
 
-
 void State::closePlotFile(void)
 {
     fclose(logp);
-
 }
 
 float State::readBattery()
@@ -142,7 +143,6 @@
     timer.start();
 }
 
-
 void State::run()
 {
     s->millis = timer.read_ms();
@@ -159,8 +159,11 @@
     s->xAxisError = robotControl->getxPositionError();
     s->yAxisError = robotControl->getyPositionError();
     s->angleError = robotControl->getThetaError() * 180 / PI;
-   // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
+    // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
 
+    s->rho = robotControl->getDistanceError();
+    s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;
+    s->delta = robotControl->getThetaGoal() * 180 / PI;
 
     setBatteryBit();
     setEnableLeftBit();