Diff: main.cpp

Revision:

44:02873ac29052

Parent:

43:a05f2571151f

--- a/main.cpp	Fri Mar 17 09:07:45 2017 +0000
+++ b/main.cpp	Fri Mar 17 10:12:27 2017 +0000
@@ -31,8 +31,8 @@
 #include "setting.h"
 
 //************ID Number*****************
-const char MyID = 'b';
-const char SenderIDc = 'e';
+const char MyID = 'e';
+const char SenderIDc = 'J';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -129,17 +129,7 @@
     //send normal data
     //Create GPS Infomation Packet
     agz.SendDataCommand(MyID,SenderIDc,(int16_t)set.agzGyr.x,(int16_t)set.agzGyr.y, (int16_t)set.agzGyr.z, (int16_t)set.agzAcc.x,(int16_t)set.agzAcc.y,(int16_t)set.agzAcc.z, (uint8_t)flag);
-    
- /*   //debug***************************************************
-    printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
-            agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-            agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-            agz.get_agzCov_lati(),agz.get_agzCov_longi()
-            );
-    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
-        printf("\n");
-    //debug end***************************************************
-   */
+
    //debug
    printf("%d %d %d %d %d %d\n\r",(int16_t)set.agzGyr.x,(int16_t)set.agzGyr.y, (int16_t)set.agzGyr.z, (int16_t)set.agzAcc.x, (int16_t)set.agzAcc.y, (int16_t)set.agzAcc.z); 
 for(int i = 0; i < 24; i++){
@@ -160,6 +150,28 @@
 
 }
 
+void Send_Mannual(char c){
+
+    XBeeAddress64 send_Address;
+    if(SenderIDc == '0'){
+        send_Address = manager_Address;
+    }
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+        send_Address = robot_Address[SenderIDc - 'A'];
+    }
+    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+        send_Address = base_Address[SenderIDc - 'a'];
+    }
+    //send normal data
+    //Create GPS Infomation Packet
+    agz.CreateManualCommand(MyID,SenderIDc,c);   
+    //Select Destination
+    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
+    //Send -> Base
+   xbee.send(tx64request);
+}
+
+
 
 /////////////////////////////////////////
 //
@@ -317,7 +329,14 @@
   
   //200msごとにstatus送信
      if(auto_Timer.read_ms() >= auto_Time){
-           auto_Timer.reset();
+        
+        char c;
+        auto_Timer.reset();
+        if(pc.readable()) {
+            c = pc.getc();
+            Send_Mannual(c);
+            pc.printf("send mannual command\n\r");
+        }
            //Send_Status();
         }