Diff: main.cpp

Revision:

2:886fac7f4399

Parent:

1:b2b950b916ce

Child:

3:1ac506a96fd6

diff -r b2b950b916ce -r 886fac7f4399 main.cpp
--- a/main.cpp	Wed May 13 04:09:16 2015 +0000
+++ b/main.cpp	Wed May 13 06:04:47 2015 +0000
@@ -85,10 +85,23 @@
 XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)};
 XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
 
+/////////////////////////////////////////
+//
+//Plus Speed
+//
+//MNUAL_MODEの時にスピードを変える
+/////////////////////////////////////////
+void Plus_Speed(uint8_t *packetdata){
+       
+    if(agz.nowMode == MANUAL_MODE){ 
+        agz.changeSpeed(packetdata);
+    }
+    
+}
 
 /////////////////////////////////////////
 //
-//Send_Status
+//Send Status
 //
 //リクエストがきたとき、自分の位置情報などを返信する
 /////////////////////////////////////////
@@ -158,6 +171,18 @@
     else agz.nowStatus = GPS_UNAVAIL;
     
 } 
+
+/////////////////////////////////////////
+//
+//New Mode
+//
+/////////////////////////////////////////
+
+void New_Mode(uint8_t *packetdata){
+  
+  agz.changeMode(packetdata); 
+  
+}
   
 /////////////////////////////////////////
 //
@@ -192,6 +217,31 @@
     }
 }
 
+void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
+    
+    //マネージャからデータが来たとき
+    if(SenderIDc == '0'){
+        printf("get manager Status Kalman\n");
+    }
+    //他のロボットからデータが来たとき
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+        printf("get other robots Status Kalman\n");
+    }
+    //基地局からデータが来たとき
+    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+        printf("Get Base data Kalman\n");
+        int id = SenderIDc - 'a';
+        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
+        
+        //debug
+        for(int i = 0;i < 4;i++){
+        printf("BASE:%d\n",i);
+        printf("latitudeK:%f,longitudeK:%f\n",
+            agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i));
+        }
+    }
+}
+
 /////////////////////////////////////////
 //
 //Send_Request_to_base
@@ -332,14 +382,26 @@
                 //Check Command Type 
                 switch(Command_type){
                     //Get Request command
+                    case MANUAL:{
+                        Plus_Speed(buf1);
+                        break;
+                    }
                     case STATUS_REQUEST:{
                         Send_Status(SenderIDc);                     
                         break;          
                     }
+                    case CHANGE_MODE:{
+                        New_Mode(buf1);
+                        break;
+                    }
                     case RECEIVE_STATUS:{
                         Get_Status(SenderIDc,buf1);
                         break;
                     }
+                    case RECEIVE_KALMAN:{
+                        Get_Status_Kalman(SenderIDc, buf1);
+                        break;
+                    }
                     default:{
                         break;
                     }