Revision 19:b2f76b0fe4c8, committed 2013-05-03

Comitter:

chrigelburri

Date:

Fri May 03 13:34:34 2013 +0000

Parent:

18:306d362d692b

Child:

20:01b233b0e606

Commit message:

asdf

Changed in this revision

--- a/MicroBridge/androidADB.cpp	Fri May 03 08:35:29 2013 +0000
+++ b/MicroBridge/androidADB.cpp	Fri May 03 13:34:34 2013 +0000
@@ -1,5 +1,6 @@
 #include "androidADB.h"
 
+
 Connection * connection;
 
 /** @brief Desired position in meters for x-coordinate, given by android */
@@ -14,6 +15,20 @@
 /** @brief Indicates if a ADB connection to a android phone is established */
 boolean androidConnected;
 
+float getDesiredX()
+{
+    return androidx;
+}
+
+float getDesiredY()
+{
+    return androidy;
+}
+
+float getDesiredTheta()
+{
+    return androidt;
+}
 void Tokenize(const string& str,
               vector<string>& tokens,
               const string& delimiters /*= " "*/)
@@ -33,7 +48,7 @@
     }
 }
 
-void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
+extern void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
 {
 
     if (event == ADB_CONNECTION_OPEN) {
@@ -41,7 +56,7 @@
         //pc.printf("Android Connected\n");
     } else if (event == ADB_CONNECTION_CLOSE) {
         androidConnected = false;
-        //pc.printf("Android Disonnected\n");
+        printf("Android Disonnected\n");
     }
 
     if (event == ADB_CONNECTION_RECEIVE) {
@@ -73,9 +88,9 @@
         androidy = ::atof(tokens.at(1).c_str());
         androidt = ::atof(tokens.at(2).c_str());
 
-        //pc.printf("Android x(%d): %f\n\r\n",length,x);
-        //pc.printf("Android y(%d): %f\n\r\n",length,y);
-        //pc.printf("Android t(%d): %f\n\r\n",length,t);
+        printf("Android x(%d): %f\n\r\n",length,androidx);
+        printf("Android y(%d): %f\n\r\n",length,androidy);
+        printf("Android t(%d): %f\n\r\n",length,androidt);
     } else {
         //pc.printf("Android sayys(%d): %s\n\r\n",length,str);
     }
@@ -85,20 +100,16 @@
 void connect()
 {
     ADB::poll();
-    //pc.printf("connecting...");
+    printf("connecting...");
     char c = 'c';
     connection->write(sizeof(c), (unsigned char*)&c);
     //wait(0.5);
 }
 
-/*int main()
+void init()
 {
 
-    pc.baud(460800);
-
-    pc.printf("********************* MicroBridge 4568 ********************************\n\r");
-
-    // Initialise the ADB subsystem.
+// Initialise the ADB subsystem.
     ADB::init();
 
     // Open an ADB stream on tcp port 4568. Auto-reconnect
@@ -110,6 +121,39 @@
         wait(0.5);
     }
 
+}
+
+void write2Android(char str [32])
+{
+    connection->write(sizeof(str),(unsigned char*)&str);
+}
+
+void writeActualPosition(float x, float y, float t)
+{
+
+    while(!(androidConnected)) {
+        connect();
+        wait(0.5);
+    }
+
+    char str[32];
+    sprintf( str, "%f;%f;%f;", x, y, t);
+
+    //pc.printf("Sending: %s\n\r",str);
+    connection->write(sizeof(str),(unsigned char*)&str);
+
+}
+
+/*
+int main()
+{
+
+    pc.baud(460800);
+
+    pc.printf("********************* MicroBridge 4568 ********************************\n\r");
+
+
+
     pc.printf("connection isOpen\n");
 
     float flt = 0.0;
@@ -132,4 +176,5 @@
         wait(1);
 
     }
-}*/
+}
+*/
\ No newline at end of file

--- a/MicroBridge/androidADB.h	Fri May 03 08:35:29 2013 +0000
+++ b/MicroBridge/androidADB.h	Fri May 03 13:34:34 2013 +0000
@@ -3,7 +3,7 @@
 
 #include "mbed.h"
 #include "Adb.h"
-#include "defines.h"
+//#include "androidADB.cpp"
 
 #include <string>
 #include <sstream>
@@ -25,4 +25,14 @@
               
 void connect();
 
+float getDesiredTheta();
+float getDesiredX();
+float getDesiredY();
+
+void init();
+
+void write2Android(char str [32]);
+
+void writeActualPosition(float x, float y, float t);
+
 #endif
\ No newline at end of file

--- a/main.cpp	Fri May 03 08:35:29 2013 +0000
+++ b/main.cpp	Fri May 03 13:34:34 2013 +0000
@@ -140,8 +140,8 @@
     /**
      * 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(0, 0, PI/2 );
+    //robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
     robotControl.start();
     /*
     pc.baud(460800);
@@ -229,7 +229,39 @@
     /**
      * Close the File PLOTS.txt to read the file after with the computer and draw a diagramm
      */
+     
+     
+    pc.baud(460800);
 
+    pc.printf("********************* MicroBridge 4568 ********************************\n\r");
+    
+    init();
+    
+    // Initialise the ADB subsystem.
+   
+    pc.printf("connection isOpen\n");
+
+    float flt = 0.0f;
+    float flt2 = 0.2f;
+    float flt3 = 1.2f;
+
+    while(1) {
+
+        ADB::poll();
+
+        flt = flt - 0.1f;
+        flt2 = flt2 + 0.2f;
+        flt3 = flt3 - 0.05f;
+        
+        robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(),  getDesiredTheta());
+        
+        writeActualPosition(robotControl.getxActualPosition(),robotControl.getyActualPosition(), robotControl.getActualTheta());
+
+        //connection->write(sizeof(str),(unsigned char*)&str);
+        wait(1);
+
+    } 
+     
    
      
     state.savePlotFile(s);