Diff: main.cpp

Revision:

39:1634312cf621

Parent:

38:5cd6d4964f65

Child:

40:656858844814

--- a/main.cpp	Mon Oct 24 08:15:16 2016 +0000
+++ b/main.cpp	Sat Mar 11 14:57:44 2017 +0000
@@ -24,12 +24,26 @@
 #include "aigamozuSetting.h"
 #include "math.h"
 
+#include "HMC5843.h"
+#include "ADXL345.h"
+#include "ITG3200.h"
+#include "setting.h"
+
+//************ID Number*****************
+const char MyID = 'b';
+const char SenderIDc = 'h';
+//************ID Number*****************
+
 /////////////////////////////////////////
 //
 //Pin Setting
 //
 /////////////////////////////////////////
 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
+HMC5843 cmp(p9, p10);      // sda, scl
+ADXL345 acc(p9, p10);      // sda, scl
+ITG3200 gyr(p9, p10);      // sda, scl
+setting set;
 
 
 /////////////////////////////////////////
@@ -56,6 +70,12 @@
 
 
 int change = 0;
+//values x,y,z
+int readings[3];
+//ID Buffer
+char buffer[3];
+
+int flag =0;
 
 /////////////////////////////////////////
 //
@@ -89,6 +109,125 @@
 
 /////////////////////////////////////////
 //
+//Send_Status
+//
+//リクエストがきたとき、自分の位置情報などを返信する
+/////////////////////////////////////////
+void Send_Status(void){
+
+    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.SendDataCommand(MyID,SenderIDc,(uint8_t)set.agzGyr.x,(uint8_t)set.agzGyr.y, (uint8_t)set.agzGyr.z, (uint8_t)set.agzAcc.x, (uint8_t)set.agzAcc.y, (uint8_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",(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 < 18; i++){
+       printf("%d ", agz.packetData[i]);
+       }
+       printf("\n\r");
+       
+   
+    //Select Destination
+    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
+    //Send -> Base
+   xbee.send(tx64request);
+}
+
+/////////////////////////////////////////
+//
+//Update Sensor
+//
+/////////////////////////////////////////
+
+void update_gyro() {
+    set.setGyr((int16_t)gyr.getGyroX(),(int16_t)gyr.getGyroY(),(int16_t)gyr.getGyroZ());
+}
+
+void update_accel() {
+    acc.getOutput(readings);
+    set.setAcc(((int16_t)readings[0]), ((int16_t)readings[1]),((int16_t)readings[2]));
+}
+
+void update_cmp(){
+    cmp.readData(readings);
+    set.setCmp(((int16_t)readings[0]), ((int16_t)readings[1]),((int16_t)readings[2]));
+}
+
+/////////////////////////////////////////
+//
+//Set Up Sensor
+//
+/////////////////////////////////////////
+void setup_cmp() {
+    // Continuous mode, , 10Hz measurement rate.
+    // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
+    cmp.setDefault();
+    // Wait some time(Need at least 5ms)
+    wait(0.1);
+    cmp.getAddress(buffer);
+    pc.printf("cmp Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
+}
+        
+bool setup_acc() {
+    // These are here to test whether any of the initialization fails. It will print the failure
+    if (acc.setPowerControl(0x00)) {
+        pc.printf("acc: didn't intitialize power control\n"); 
+        return false;  
+    }
+    wait(.001);
+    
+    //Full resolution, +/-16g, 4mg/LSB.
+    //if(acc.setDataFormatControl(0x0B)){
+    if(acc.setDataFormatControl(0x09)){  // +/- 4G               
+        pc.printf("didn't set data format\n");
+        return false;  }
+    wait(.001);
+     
+    //200Hz data rate.
+    if(acc.setDataRate(ADXL345_200HZ)){
+        pc.printf("didn't set data rate\n");
+        return false;    }
+    wait(.001);
+     
+    //Measurement mode.
+    if(acc.setPowerControl(MeasurementMode)) {
+        pc.printf("didn't set the power control to measurement\n"); 
+        return false;   } 
+
+    pc.printf("Acc Id=%x \n\r", acc.getDeviceID());
+    pc.printf("%c" ,13,10);
+    return true;
+}
+
+void setup_gyr() {
+    //Set highest bandwidth.
+    gyr.setLpBandwidth(LPFBW_42HZ);
+    pc.printf("Gyro Id=%x \n\r", gyr.getWhoAmI());
+    pc.printf("%c" ,13,10);
+}
+
+/////////////////////////////////////////
+//
 //Main Processing
 //
 /////////////////////////////////////////
@@ -104,9 +243,11 @@
     //GPS setting
     gps_Serial = new Serial(p28,p27);
     Adafruit_GPS myGPS(gps_Serial); 
+    setup_acc();    
+    setup_gyr();
     
     Timer auto_Timer;
-    const int auto_Time = 2000; //refresh time in ms
+    const int auto_Time = 1000; //refresh time in ms
     
     wait_ms(2000);
        
@@ -120,6 +261,12 @@
         
         //Check Xbee Buffer Available
         xbee.readPacket();
+
+        update_accel();
+        update_gyro();
+        Send_Status();
+        //printf("gx:%d,gy:%d,gz:%d,ax:%d,ay:%d,az:%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); 
+        wait_ms(1000);
             
         if (xbee.getResponse().isAvailable()) {
             xbee.getResponse().getZBRxResponse(zbRx);
@@ -137,7 +284,8 @@
                         Plus_Speed(buf);
                         break;
                     }
-                    case STATUS_REQUEST:{                 
+                    case STATUS_REQUEST:{
+                        flag = 1;
                         break;          
                     }
                     case CHANGE_MODE:{