Mami Yokokawa / Mbed 2 deprecated Aigamozu_Robot_March

3/22-23の実験用 Send_Status(): 加速度・ジャイロをパケットで相手に送信 Get_Status():受信したパケットを表示

Dependencies:   ADXL345 AigamozuControlPackets_展示会 Aigamozu_Robot_展示会 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed setting

Fork of Aigamozu_Robot_展示会 by aigamozu

Diff: main.cpp

Revision:
42:69b4cb31aafc
Parent:
41:55539183dbb0
diff -r 55539183dbb0 -r 69b4cb31aafc main.cpp
--- a/main.cpp	Mon Mar 13 17:55:45 2017 +0000
+++ b/main.cpp	Fri Mar 17 09:04:13 2017 +0000
@@ -31,7 +31,7 @@
 
 //************ID Number*****************
 const char MyID = 'b';
-const char SenderIDc = 'h';
+const char SenderIDc = 'e';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -152,6 +152,14 @@
    xbee.send(tx64request);
 }
 
+void Get_Status(uint8_t *packetdata){
+
+    agz.GetData(&packetdata[8],&packetdata[10],&packetdata[12],&packetdata[14],&packetdata[16],&packetdata[18]);
+    printf("%d, %d, %d, %d, %d, %d, %d\n\r", agz.r_gx, agz.r_gy, agz.r_gz, agz.r_ax, agz.r_ay, agz.r_az, packetdata[20]);
+
+}
+
+
 /////////////////////////////////////////
 //
 //Update Sensor
@@ -246,7 +254,7 @@
     setup_gyr();
     
     Timer auto_Timer;
-    const int auto_Time = 500; //refresh time in ms
+    const int auto_Time = 200; //refresh time in ms
     
     wait_ms(2000);
        
@@ -272,6 +280,7 @@
                 xbee.getResponse().getZBRxResponse(zbRx);
                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
                 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
+                uint8_t *buf1 = &buf[11];
                  
                 //Check Command Type 
                 switch(Command_type){
@@ -281,14 +290,21 @@
                         break;
                     }
                     case STATUS_REQUEST:{
-                        flag = 1;
                         break;          
                     }
                     case CHANGE_MODE:{
                         New_Mode(buf);
+                        if(flag == 0){
+                            flag = 1;
+                        }
+                        else{
+                            flag = 0;
+                        }        
                         break;
                     }
+                    //status受信後
                     case RECEIVE_STATUS:{
+                        Get_Status(buf1);
                         break;
                     }
                     default:{
@@ -298,9 +314,10 @@
             }//endifZB_RX_RESPONSE
         }//endifisAvailable
   
+  //200msごとにstatus送信
      if(auto_Timer.read_ms() >= auto_Time){
            auto_Timer.reset();
-           Send_Status();
+           //Send_Status();
         }
         
     }