Revision 0:0d2f5546f51a, committed 2015-05-15

Comitter:

s1200058

Date:

Fri May 15 07:05:06 2015 +0000

Child:

1:b684ee7d282a

Commit message:

2015/05/15

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345.lib	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ADXL345/#45faba962a46

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AigamozuControlPackets.lib	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#4f1516421f02

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5843.lib	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/elrafapadron/code/HMC5843/#fdab96fc6fff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ITG3200/#8967cbe04d96

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MBed_Adafruit-GPS-Library.lib	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library/#7ef0dd12940c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XBee.lib	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/okini3939/code/XBee/#b36422ef864f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agzIDLIST.lib	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agzIDLIST/#0f026cfcf1bd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agz_common.lib	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agz_common/#14e469b0c33e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aigamozuSetting.h	Fri May 15 07:05:06 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
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,338 @@
+/**********************************************/
+//  
+//    
+//
+//  Program name: Aigamozu BASE
+//  Author: Mineta Kizuku
+//  
+//
+/**********************************************/
+
+/**********************************************/
+//更新情報
+//2015/05/11
+//ベースプログラムの作成
+//
+//2015/05/13
+//カルマンフィルタの共分散の値を0.0001以下にならないようにした
+//共分散の値を10進数に変換するようにした
+/**********************************************/
+
+#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"
+
+
+#define SIGMA_MIN 0.0001
+
+//************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);
+
+
+/////////////////////////////////////////
+//
+//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());
+    
+    //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***************************************************
+    
+    //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->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
+                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->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
+        agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
+    }
+    else agz.nowStatus = GPS_UNAVAIL;
+    
+} 
+
+/////////////////////////////////////////
+//
+//Send_Request_to_robot
+//
+/////////////////////////////////////////
+void Send_Request_Robot(int robotnumber){
+    printf("send\n");
+    agz.createRequestCommand(MyID, robotnumber);
+    //Select Destination
+    ZBTxRequest tx64request(robot_Address[robotnumber],agz.packetData,agz.getPacketLength());
+    //Send -> Base
+    xbee.send(tx64request);
+}
+
+
+
+/////////////////////////////////////////
+//
+//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];
+        }
+    }
+    
+    if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
+    if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
+            
+    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 = 1000; //refresh time in ms
+    myGPS.begin(GPS_BAUD_RATE);
+    Timer collect_Timer;
+    const int collect_Time = 2000; //when we collect 4 GPS point, we use 
+    int collect_flag = 0;
+    char collect_state = 'a';
+    
+    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();
+    collect_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( collect_Timer.read_ms() >= collect_Time){
+            collect_Timer.reset();   
+                     
+            Send_Request_Robot(collect_flag);
+            
+            collect_flag++;
+            collect_state++;
+
+            if(collect_flag == 4){
+                collect_state = 'a';
+                collect_flag = 0;
+            }    
+         } 
+        
+        
+        if (refresh_Timer.read_ms() >= refresh_Time) {
+            refresh_Timer.reset();
+            Get_GPS(&myGPS);
+
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri May 15 07:05:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8a40adfe8776
\ No newline at end of file