Diff: main.cpp

Revision:

6:8d80c84e0c09

Parent:

5:f07cbb5a86c3

Child:

7:6a99be179329

--- a/main.cpp	Thu Jul 16 08:19:18 2015 +0000
+++ b/main.cpp	Thu Jul 16 08:38:19 2015 +0000
@@ -1,6 +1,16 @@
 #include "mbed.h"
 #include "pin_config.h"
 
+#include "communication.h"
+#include "protocol.h"
+
+//set frequancy unit in Hz
+#define F_UPDATE 10.0f
+#define TIMER_UPDATE 1.0f/F_UPDATE
+
+//counter not receive from station
+#define TIMEOUT_RESPONE_COMMAND 5
+
 //define pin class
 DigitalOut dirA_LU(INA_L_U);
 DigitalOut dirB_LU(INB_L_U);
@@ -34,8 +44,11 @@
 DigitalOut myled(LED1);
 DigitalIn mybutton(USER_BUTTON);
 
+//communication config
+//serial for debug
 Serial pc(USBTX, USBRX);
-
+//serial for xbee
+COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx
 
 //Function Prototype
 void motor_set(uint8_t id, uint8_t direct);
@@ -46,6 +59,8 @@
 
 void calibration(uint8_t id);
 
+void getCommand();
+
 
 //Globle Variable
 uint16_t max_pos_LU= 10000;
@@ -63,6 +78,10 @@
 
 int16_t MARGIN = 500;
 
+//set foreground
+Ticker Update_command;
+
+
 //Main function
 int main()
 {
@@ -227,6 +246,7 @@
     pc.printf("RUN mode [START]\n");
        }
 
+    Update_command.attach(&getCommand,TIMER_UPDATE);
 
     uint8_t count=0;
 
@@ -627,4 +647,38 @@
 {
 
 }
-*/
\ No newline at end of file
+*/
+
+void getCommand()
+{
+    static uint8_t count =0;
+
+    ANDANTE_PROTOCOL_PACKET packet;
+
+    uint8_t status = pan_a.receiveCommunicatePacket(&packet);
+    myled=0;
+    
+
+
+    if(status == ANDANTE_ERRBIT_NONE) {
+        if(count >2 && count <10) {
+            count--;
+        } else {
+            count=0;
+            
+        }
+        pan_a.sendCommunicatePacket(&packet);
+        //update command
+        //setControl(&packet);
+
+    } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
+        count++;
+    }
+
+    if(count >= TIMEOUT_RESPONE_COMMAND) {
+        //stop robot
+        count++;
+        myled=1;
+     //   setSpeedControl(0.0);
+    }
+}
\ No newline at end of file