Diff: main.cpp

Revision:

27:a13ede88e75f

Parent:

26:a201dcd4e618

--- a/main.cpp	Mon May 20 09:41:47 2013 +0000
+++ b/main.cpp	Mon May 20 11:51:33 2013 +0000
@@ -39,16 +39,6 @@
 #include "RobotControl.h"
 #include "androidADB.h"
 
-/**
- * @name LED
- * @{
- */
-
-/**
-* @brief The Array of the four LED on the mbed board.
-*/
-PwmOut led[4] = { LED1, LED2, LED3, LED4 };
-/*! @} */
 
 /**
  * @name Hallsensor
@@ -115,7 +105,6 @@
 
 /*! @} */
 
-
 // @todo PC USB communications DAs wird danach gelöscht
 Serial pc(USBTX, USBRX);
 
@@ -125,11 +114,6 @@
 int main()
 {
 
-    int garagenumber = 2;
-    float x = -2.954f + 0.308 * garagenumber;
-    float ypre = 1.30f;
-    float ygoal = 0.60f;
-
     /**
      * Check at first the Battery voltage. Starts when the voltages greater as the min is.
      * start the timer for the Logging to the file
@@ -139,29 +123,15 @@
     state.initPlotFile();
     state.closePlotFile();
     while(s.voltageBattery < BAT_MIN) {
-        for (float f = 0.1f; f < 6.3f; f += 0.1f) {
-            for(int i = 0; i <= 3; i  ++) {
-                led[i] = state.dim( i, f );
-            }
-            wait_ms(20);
-        }
-        wait(0.05);
-        for (float f = 0.1f; f < 6.3f; f += 0.1f) {
-            for(int i = 0; i <= 3; i  ++) {
-                led[i] = state.dim( 3-i, f );
-            }
-            wait_ms(20);
-        }
-        wait(0.05);
+        state.ledArray(20.0f);
     }
-    for(int i = 0; i <= 3; i  ++) {
-        led[i] = state.dim( 0, 0.0f );
-    }
+
     state.stop();
     wait(0.5);
     state.start();
     state.initPlotFile();
     state.startTimerFromZero();
+    robotControl.start();
 
     /**
      * Clear all Errors of the ESCON Module, with a disabled to enable event
@@ -170,58 +140,55 @@
     wait(0.01);
     robotControl.setEnable(true);
     wait(0.1);
+    robotControl.setEnable(false);
 
-    /**
-     * 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.start();
-    robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
-    
-    wait(0.02);
-    state.savePlotFile(s);
+    // Verbindung zum putty kann spöter gelöscht werden......
+    pc.baud(460800);
+    pc.printf("********************* MicroBridge 4568 ********************************\n\r");
+
+
+    // Initialise the ADB subsystem.
+    init();
+    setDesiredTheta(400.0f);
+    pc.printf("connection isOpen\n");
+
+    while(getDesiredTheta() < (4 * PI)) {
+
+        ADB::poll();
+        if (getDesiredTheta() > (2 * PI)) {
+            // Runs at first till the Startbutton has pressed
+            state.startTimerFromZero();
+            robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
+            robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
+            robotControl.setEnable(false);
+        } else {
+            robotControl.setEnable(true);
+            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);
+
+    }
 
     /**
-     * Sets the desired Points.
+     * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm
      */
-    robotControl.setDesiredPositionAndAngle(-1.35f, 1.85f,  3*PI/8);
-    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);
-    };
-//QR
-    robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f,  PI);
-    while(!(robotControl.getDistanceError() <= 0.075)) {
-        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(!(robotControl.getDistanceError() <= 0.2)) {
-        state.savePlotFile(s);
-    }
-
-
-/// STOP Setze actueler Wert
-    robotControl.setDesiredPositionAndAngle(robotControl.getxActualPosition(), robotControl.getyActualPosition(), robotControl.getActualTheta());
+    robotControl.setDesiredPositionAndAngle(robotControl.getxActualPosition(),
+                                            robotControl.getyActualPosition(),
+                                            robotControl.getActualTheta());
     robotControl.stop();
     state.savePlotFile(s);
     leftMotor.setVelocity(0.0f);
@@ -230,44 +197,6 @@
         state.savePlotFile(s);
     };
 
-
-
-    /*
-        pc.baud(460800);
-        pc.printf("********************* MicroBridge 4568 ********************************\n\r");
-
-        init();
-
-        // Initialise the ADB subsystem.
-
-        pc.printf("connection isOpen\n");
-
-        while(getDesiredTheta() < (4 * PI)) {
-
-            ADB::poll();
-
-            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
      */
@@ -281,20 +210,7 @@
     * Fast PWM sample for the end
     */
     while(1) {
-        for (float f = 0.1f; f < 6.3f; f += 0.1f) {
-            for(int i = 0; i <= 3; i  ++) {
-                led[i] = state.dim( i, f );
-            }
-            wait_ms(5);
-        }
-        wait(0.1);
-        for (float f = 0.1f; f < 6.3f; f += 0.1f) {
-            for(int i = 0; i <= 3; i  ++) {
-                led[i] = state.dim( 3-i, f );
-            }
-            wait_ms(5);
-        }
-        wait(0.05);
+        state.ledArray(20.0f);
     }
 
 }