Diff: main.cpp

Revision:

47:626572030a78

Parent:

46:f1e87c375e02

Child:

48:303e2ed5e062

--- a/main.cpp	Fri Mar 31 06:27:01 2017 +0000
+++ b/main.cpp	Thu Apr 20 10:24:53 2017 +0000
@@ -24,6 +24,11 @@
 #include "aigamozuSetting.h"
 #include "math.h"
 
+//************ID Number*****************
+const char MyID = 'e';
+const char SenderIDc = 'c';
+//************ID Number*****************
+
 /////////////////////////////////////////
 //
 //Pin Setting
@@ -85,6 +90,57 @@
 
 /////////////////////////////////////////
 //
+//Send_Change_Mode
+//
+//マニュアルモードのコマンドを送る
+/////////////////////////////////////////
+void Send_Change_Mode(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
+    int cmode=0;
+    if(c == 'n'){
+        cmode = 0;
+        pc.printf("stand by mode\n\r");
+    }
+    else if(c == 'm'){
+        cmode = 1;
+        pc.printf("manual mode\n\r");
+    }
+    else if(c == 'b'){
+        cmode = 2;
+        pc.printf("auto mode\n\r");
+    }
+
+    //Create GPS Infomation Packet
+    agz.createChangeModeCommand(MyID,SenderIDc,cmode);
+    
+   //debug
+   printf("RF data:");
+    for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; i++){
+       printf("%x ", agz.packetData[i]);
+    }
+    printf("\n\r");
+   
+    //Select Destination
+    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
+    //Send -> Base
+   xbee.send(tx64request);
+}
+
+
+/////////////////////////////////////////
+//
 //Main Processing
 //
 /////////////////////////////////////////
@@ -102,7 +158,7 @@
   
     //2secごとの更新用Timer
     Timer autoTimer;
-    const int autoTime = 2000; //refresh time in ms
+    const int autoTime = 200; //refresh time in ms
 
     //interrupt start
     autoTimer.start();
@@ -153,11 +209,22 @@
         }//endifisAvailable
   
     //ロボットがオートモードのとき、シーケンス動作する
-     if(agz.nowMode == AUTO_GPS_MODE && autoTimer.read_ms() >= autoTime){
-           autoTimer.reset();
-           agz.autoMove();
+    // if(agz.nowMode == AUTO_GPS_MODE && autoTimer.read_ms() >= autoTime){
+    //       autoTimer.reset();
+    //       agz.autoMove();
+    //    }
+    //
+    
+    if(autoTimer.read_ms() >= autoTime && pc.readable()){
+            autoTimer.reset();
+            char c = pc.getc();
+            
+            if(c == 'm' || c == 'n' || c == 'b'){
+                //m:manual-mode,n:stand-by-mode,b:auto-gps-mode
+                Send_Change_Mode(c);
+                //pc.printf("%c\n\r", c);
+            }
         }
-        
     }
     
 }
\ No newline at end of file