Diff: main.cpp

Revision:

17:f0a973f17917

Parent:

16:b5d949136a21

--- a/main.cpp	Fri Apr 26 06:36:57 2013 +0000
+++ b/main.cpp	Fri May 03 06:30:46 2013 +0000
@@ -24,7 +24,7 @@
  *
  * 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/">http://mbed.org/users/mbed_official/code/mbed/</a>
  */
@@ -36,7 +36,8 @@
 #include "defines.h"
 #include "State.h"
 #include "RobotControl.h"
-//#include "android.h"
+
+#include "androidADB.h"
 
 /**
  * @name Hallsensor
@@ -141,95 +142,35 @@
     //robotControl.setAllToZero(0, 0, PI/2 );
     robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
     robotControl.start();
-    /*
-    pc.baud(460800);
-    pc.printf("Welcome to adkTerm\n\n\n\n\n\n\r");
-    pc.printf("here we go... \n");
-    adkTerm.setupDevice();
-    pc.printf("Android Development Kit: start\r\n");
-    USBInit();
-    
-    //wait(3);
-    
-    adkTerm.start();
 
-  */
-
-    //  Epä Parkour fahrä
-    
-        robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
-        wait(0.1);
+    init();
 
-        robotControl.setDesiredPositionAndAngle(-1.50f, 1.50f,  3*PI/4);
-        while(!(robotControl.getDistanceError() <= 0.9)) {
-            state.savePlotFile(s);
-        };
-
-        robotControl.setDesiredPositionAndAngle(-1.20f, 2.1f,  PI/4);
-        while(!(robotControl.getDistanceError() <= 0.7)) {
-            state.savePlotFile(s);
-        };
+    //pc.printf("connection isOpen\n");
 
-        robotControl.setDesiredPositionAndAngle(-0.5f, 3.0f,  PI/2); //0.75
-        while(!(robotControl.getDistanceError() <= 0.7)) {
-            state.savePlotFile(s);
-        };
+    while(1) {
 
-        robotControl.setDesiredPositionAndAngle(-1.0f, 3.75f,  3*PI/4);
-        while(!(robotControl.getDistanceError() <= 0.55)) {
-            state.savePlotFile(s);
-        };
-
+        ADB::poll();
 
-        robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f,  PI);
-        while(!(robotControl.getDistanceError() <= 0.05)) {
-            state.savePlotFile(s);
-        };
+        //robotControl.setDesiredPositionAndAngle(androidx, androidy, androidt);
+        robotControl.setDesiredPositionAndAngle(androidx, 1.4, 1.3);
 
-        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);
-        };
+        char str[32];
 
-        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);
-        }
+        // to mbed
+        sprintf( str, "%f;%f;%f;", robotControl.getxActualPosition(), robotControl.getyActualPosition(), robotControl.getDesiredTheta());
+        //pc.printf("Sending: %s\n\r",str);
+        connection->write(sizeof(str),(unsigned char*)&str);
 
-    
-     /*
-        while (1) {
-            USBLoop();
-            
-            robotControl.setDesiredPositionAndAngle(adkTerm.getDesiredX(), adkTerm.getDesiredY(),  adkTerm.getDesiredTheta());
-            
-            //pc.printf(".");
+        wait(1);
 
-            /*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());
-        }*/
-
-
+    }
 
     /**
-     * Close the File PLOTS.txt to read the file after with the computer and draw a diagramm
-     */
+    * 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);
-    
+
 }