2015/05/12

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed

Files at this revision

API Documentation at this revision

Comitter:
kityann
Date:
Tue May 12 01:03:31 2015 +0000
Commit message:
test

Changed in this revision

ADXL345.lib Show annotated file Show diff for this revision Revisions of this file
AigamozuControlPackets.lib Show annotated file Show diff for this revision Revisions of this file
HMC5843.lib Show annotated file Show diff for this revision Revisions of this file
ITG3200.lib Show annotated file Show diff for this revision Revisions of this file
Kalman/Kalman.cpp Show annotated file Show diff for this revision Revisions of this file
Kalman/Kalman.h Show annotated file Show diff for this revision Revisions of this file
MBed_Adafruit-GPS-Library.lib Show annotated file Show diff for this revision Revisions of this file
XBee.lib Show annotated file Show diff for this revision Revisions of this file
agzIDLIST.lib Show annotated file Show diff for this revision Revisions of this file
agz_common.lib Show annotated file Show diff for this revision Revisions of this file
aigamozuSetting.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r eee5e3d906ce ADXL345.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345.lib	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ADXL345/#45faba962a46
diff -r 000000000000 -r eee5e3d906ce AigamozuControlPackets.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AigamozuControlPackets.lib	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#a6fa8cc96d94
diff -r 000000000000 -r eee5e3d906ce HMC5843.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5843.lib	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/elrafapadron/code/HMC5843/#fdab96fc6fff
diff -r 000000000000 -r eee5e3d906ce ITG3200.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ITG3200/#8967cbe04d96
diff -r 000000000000 -r eee5e3d906ce Kalman/Kalman.cpp
diff -r 000000000000 -r eee5e3d906ce Kalman/Kalman.h
diff -r 000000000000 -r eee5e3d906ce MBed_Adafruit-GPS-Library.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MBed_Adafruit-GPS-Library.lib	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library/#7ef0dd12940c
diff -r 000000000000 -r eee5e3d906ce XBee.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XBee.lib	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/okini3939/code/XBee/#b36422ef864f
diff -r 000000000000 -r eee5e3d906ce agzIDLIST.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agzIDLIST.lib	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agzIDLIST/#425a574537fd
diff -r 000000000000 -r eee5e3d906ce agz_common.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agz_common.lib	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agz_common/#14e469b0c33e
diff -r 000000000000 -r eee5e3d906ce aigamozuSetting.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aigamozuSetting.h	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,6 @@
+//Set Baud Rate
+#define GPS_BAUD_RATE 9600
+#define XBEE_BAUD_RATE 57600
+#define PC_BAUD_RATE 57600
+#define IMU_BAUD_RATE 400000
+
diff -r 000000000000 -r eee5e3d906ce main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,302 @@
+/**********************************************/
+//  
+//    
+//
+//  Program name: Aigamozu BASE
+//  Author: Mineta Kizuku
+//  
+//
+/**********************************************/
+
+/**********************************************/
+//更新情報
+//2015/05/11
+//ベースプログラムの作成
+//
+//
+//
+/**********************************************/
+
+#include "mbed.h"
+#include "XBee.h"
+#include "MBed_Adafruit_GPS.h"
+#include "AigamozuControlPackets.h"
+#include "agzIDLIST.h"
+#include "aigamozuSetting.h"
+#include "agz_common.h"
+#include "Kalman.h"
+
+//************ID Number*****************
+const char MyID = 'D';
+//************ID Number*****************
+
+/////////////////////////////////////////
+//
+//Pin Setting
+//
+/////////////////////////////////////////
+VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
+
+
+/////////////////////////////////////////
+//
+//Connection Setting
+//
+/////////////////////////////////////////
+
+//Serial Connect Setting: PC <--> mbed
+Serial pc(USBTX, USBRX);    
+
+//Serial Connect Setting: GPS <--> mbed
+Serial * gps_Serial;
+
+//Serial Connect Setting: XBEE <--> mbed
+XBee xbee(p13,p14);
+ZBRxResponse zbRx = ZBRxResponse();
+
+//set up GPS module
+
+//set up AigamozuControlPackets library
+AigamozuControlPackets agz(agz_motorShield);
+
+
+/////////////////////////////////////////
+//
+//For Kalman data
+//
+/////////////////////////////////////////
+double sigmaGPS[2][2] = {{250,0},{0,250}};
+double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
+double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
+double y[2],x[2][2]={0};
+void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
+
+/////////////////////////////////////////
+//
+//Address List
+//
+/////////////////////////////////////////
+XBeeAddress64 base_Address[BaseNumber] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
+                                 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
+                                 XBeeAddress64(BASE5_32H,BASE5_32L)};
+XBeeAddress64 robot_Address[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)};
+XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
+
+
+/////////////////////////////////////////
+//
+//Send_Status
+//
+//リクエストがきたとき、自分の位置情報などを返信する
+/////////////////////////////////////////
+void Send_Status(char SenderIDc){
+    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.createReceiveStatusCommand(MyID,SenderIDc,agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                    agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                    agz.get_agzCov_lati(),agz.get_agzCov_longi());
+    //Select Destination
+    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
+    //Send -> Base
+    xbee.send(tx64request);
+}
+
+/////////////////////////////////////////
+//
+//Get GPS function
+//
+/////////////////////////////////////////
+
+void Get_GPS(Adafruit_GPS *myGPS){
+    static bool flag = true;
+    
+    if (myGPS->fix) {
+        agz.nowStatus = GPS_AVAIL;
+        
+        if(flag){//初期値設定
+            if(myGPS->longitudeH>=138 && myGPS->longitudeH<=141 && myGPS->latitudeH>=36 && myGPS->latitudeH<=38){
+                flag = false;
+                x[0][0]=(double)myGPS->latitudeL;                        
+                x[0][1]=(double)myGPS->longitudeL;   
+            } 
+        }
+        
+        if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
+            return;
+        }
+        //Kalman Filter
+        Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
+                
+        agz.reNewRobotPoint(myGPS->longitudeH,myGPS->longitudeL,myGPS->latitudeH,myGPS->latitudeL);
+        agz.reNewRobotPointKalman(myGPS->longitudeKH,myGPS->longitudeKL,myGPS->latitudeKH,myGPS->latitudeKL);
+    }
+    else agz.nowStatus = GPS_UNAVAIL;
+    
+} 
+
+
+/////////////////////////////////////////
+//
+//Kalman Processing
+//
+/////////////////////////////////////////
+    
+void get_K(){
+  double temp[2][2]={
+    {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
+    {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
+  };
+  double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
+  K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
+  K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
+}
+
+
+void get_x(){
+  x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
+  x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
+}
+
+
+void get_sigma(){
+    double temp[2][2];
+    for(int i=0;i<2;i++) {
+        for(int j=0;j<2;j++) {
+            for(int k=0;k<2;k++) {
+                temp[i][j]+=K[1][i][k]*sigma[0][k][j];
+            }
+        }
+   }
+    for(int i = 0;i < 2;i++){
+        for(int j = 0;j < 2;j++){
+            sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
+        }
+    }   
+}
+
+void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
+    y[0] = Latitude;
+    y[1] = Longitude;
+    //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
+    get_K();
+    //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
+    get_x();
+    //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
+    get_sigma();
+    
+    
+    //kousinn
+    for(int i = 0;i < 2;i++){
+        for(int j = 0;j < 2;j++){
+            K[0][i][j]=K[1][i][j];
+            x[0][i]=x[1][i];
+            sigma[0][i][j]=sigma[1][i][j];
+        }
+    }
+            
+    myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
+    myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
+    myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
+    myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
+    
+    agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
+}
+
+
+/////////////////////////////////////////
+//
+//Main Processing
+//
+/////////////////////////////////////////
+int main() {
+    //start up time
+    wait(3);
+    //set pc frequency to 57600bps 
+    pc.baud(PC_BAUD_RATE); 
+    //set xbee frequency to 57600bps
+    xbee.begin(XBEE_BAUD_RATE);    
+        
+
+    //GPS setting
+    gps_Serial = new Serial(p28,p27);
+    Adafruit_GPS myGPS(gps_Serial); 
+    Timer refresh_Timer;
+    const int refresh_Time = 2000; //refresh time in ms
+    myGPS.begin(GPS_BAUD_RATE); 
+    
+    char SenderIDc;
+    //GPS Send Command
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
+    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
+    myGPS.sendCommand(PGCMD_ANTENNA);
+    
+    wait(2);
+       
+    //interrupt start
+    refresh_Timer.start();
+
+    printf("start\n");
+    
+    while (true) {
+        
+        //Check Xbee Buffer Available
+        xbee.readPacket();
+            
+        if (xbee.getResponse().isAvailable()) {
+            xbee.getResponse().getZBRxResponse(zbRx);
+            uint8_t *buf = zbRx.getFrameData();
+                
+            if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
+                xbee.getResponse().getZBRxResponse(zbRx);
+                uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
+                uint8_t *buf1 = &buf[11];//データの部分のみを格納する
+                SenderIDc = buf1[5];//送信元のIDを取得する
+                char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
+                 
+                //Check Command Type 
+                switch(Command_type){
+                    //Get Request command
+                    case STATUS_REQUEST:{
+                        Send_Status(SenderIDc);                            
+                        break;          
+                    }
+                    default:{
+                        break;
+                    }
+                }//endswitch
+            }//endifZB_RX_RESPONSE
+        }//endifisAvailable
+                       
+        myGPS.read();
+        //recive gps module
+        //check if we recieved a new message from GPS, if so, attempt to parse it,
+        if ( myGPS.newNMEAreceived() ) {
+            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+                continue;   
+            }       
+        }
+        
+        
+        if (refresh_Timer.read_ms() >= refresh_Time) {
+            refresh_Timer.reset();
+            Get_GPS(&myGPS);
+            
+            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()
+                    );
+            
+        }
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r eee5e3d906ce mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 12 01:03:31 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8a40adfe8776
\ No newline at end of file