test

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:
Sat Apr 25 01:06:06 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 68d27eee9a6c ADXL345.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345.lib	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ADXL345/#45faba962a46
diff -r 000000000000 -r 68d27eee9a6c AigamozuControlPackets.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AigamozuControlPackets.lib	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#e441331aa4a1
diff -r 000000000000 -r 68d27eee9a6c HMC5843.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5843.lib	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/elrafapadron/code/HMC5843/#fdab96fc6fff
diff -r 000000000000 -r 68d27eee9a6c ITG3200.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ITG3200/#8967cbe04d96
diff -r 000000000000 -r 68d27eee9a6c Kalman/Kalman.cpp
diff -r 000000000000 -r 68d27eee9a6c Kalman/Kalman.h
diff -r 000000000000 -r 68d27eee9a6c MBed_Adafruit-GPS-Library.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MBed_Adafruit-GPS-Library.lib	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library/#cc9ab73d0624
diff -r 000000000000 -r 68d27eee9a6c XBee.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XBee.lib	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/okini3939/code/XBee/#b36422ef864f
diff -r 000000000000 -r 68d27eee9a6c agzIDLIST.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agzIDLIST.lib	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agzIDLIST/#f9411e7b89a6
diff -r 000000000000 -r 68d27eee9a6c agz_common.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agz_common.lib	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agz_common/#14e469b0c33e
diff -r 000000000000 -r 68d27eee9a6c aigamozuSetting.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aigamozuSetting.h	Sat Apr 25 01:06: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
+
diff -r 000000000000 -r 68d27eee9a6c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,310 @@
+/**********************************************/
+//  
+//    
+//
+//  Program name: Aigamozu Robot Control
+//  Author: Atsunori Maruyama
+//  Ver ->  1.3
+//  Day ->  2014/06/09
+//  
+//
+/**********************************************/
+
+#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"
+
+/////////////////////////////////////////
+//
+//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();
+XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
+
+AGZ_ROBOT robot[4];
+
+/////////////////////////////////////////
+//
+//Pin Setting
+//
+/////////////////////////////////////////
+VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
+
+
+
+/////////////////////////////////////////
+//
+//Kalman Processing
+//
+/////////////////////////////////////////
+
+
+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 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){
+    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();
+}
+
+
+/////////////////////////////////////////
+//
+//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); 
+    
+    Timer collect_Timer;
+    const int collect_Time = 200; //when we collect 4 GPS point, we use 
+    int collect_flag = 0;
+    char collect_state = 'a';
+    XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
+                                        XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
+    XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
+    int id;
+    bool flag = true;
+    
+    //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
+    AigamozuControlPackets agz(agz_motorShield);
+    refresh_Timer.start();
+    
+    printf("test\n");
+        
+    
+    while (true) {
+        
+        //Check Xbee Buffer Available
+        xbee.readPacket();
+            if (xbee.getResponse().isAvailable()) {
+                if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
+                xbee.getResponse().getZBRxResponse(zbRx);
+                uint8_t *buf = zbRx.getFrameData();
+                uint8_t *buf1 = &buf[11]; 
+                 
+                 
+                 //Check Command Type 
+                 switch(agz.checkCommnadType(buf)){
+                    
+                    //CommandType -> ChanegeMode
+                    case CHANGE_MODE :{
+                        agz.changeMode(buf);                                        
+                        break;
+                        }
+
+                    //CommandType -> Manual
+                    case MANUAL:{  
+                        //Check now Mode
+                        if(agz.nowMode == MANUAL_MODE){ 
+                            agz.changeSpeed(buf);
+                            }
+                        break;
+                        }
+                    
+                    //CommandType -> Send Status
+                    case STATUS_REQUEST:{
+                            //send normal data
+                            //Create GPS Infomation Packet
+                            agz.createReceiveStatusCommand('A','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+                            //Select Destination
+                            ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
+                            //Send -> Base
+                            xbee.send(tx64request);
+                            
+                            //send Kalman data
+                            agz.createReceiveStatusCommandwithKalman('A','a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
+                            //Select Destination
+                            ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
+                            //Send -> Base
+                            xbee.send(tx64request);
+                            break;         
+          
+                        }
+                        
+                    case RECEIVE_STATUS:{
+                            
+                        //printf("Receive Status\n");      
+                           
+                        id = buf1[5] - 'A';
+                        robot[id].set_state(buf1[9]);
+                        robot[id].set_LatitudeH(&buf1[13]);
+                        robot[id].set_LatitudeL(&buf1[17]);
+                        robot[id].set_LongitudeH(&buf1[21]);
+                        robot[id].set_LongitudeL(&buf1[25]);
+                        
+                        agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
+                    /*   
+                        printf("%d,", buf1[5]);                    
+                        printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
+                    */
+                        break;
+                    }
+                    case RECEIVE_KALMAN:{
+                        id = buf1[5] - 'A';
+                        robot[id].set_state(buf1[9]);
+                        robot[id].set_LatitudeKH(&buf1[13]);
+                        robot[id].set_LatitudeKL(&buf1[17]);
+                        robot[id].set_LongitudeKH(&buf1[21]);
+                        robot[id].set_LongitudeKL(&buf1[25]);
+                        
+                        agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL());
+ 
+                        break;    
+                    }
+                    default:{
+                        break;
+                    }
+                }
+                    
+            
+                }
+            }
+
+            
+        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();
+            if (myGPS.fix) {
+                agz.nowStatus = GPS_AVAIL;
+                
+                if(flag){
+                    if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
+                        flag = false;
+                        x[0][0]=(double)myGPS.longitudeL;                        
+                        x[0][1]=(double)myGPS.latitudeL;   
+                    } 
+                }
+                if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
+                
+                //Kalman Filter
+                Kalman(myGPS.longitudeL,myGPS.latitudeL);
+                //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.longitudeKH=myGPS.longitudeH;//longitude after filtering
+                myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering
+                myGPS.longitudeKL=(long)x[1][0];//longitude after filtering
+                myGPS.latitudeKL=(long)x[1][1];//latitude after filtering
+                
+                agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+                agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
+                //printf("state = %d\n",agz.nowMode);
+                //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+            }
+            else agz.nowStatus = GPS_UNAVAIL;
+            
+        } 
+        
+        
+        //get base GPS
+        if( collect_Timer.read_ms() >= collect_Time){
+            collect_Timer.reset();
+            
+            //printf("Send Request:%d,%d\n",collect_flag,collect_state);
+                      
+            agz.createRequestCommand('A', collect_state);
+            //Select Destination
+            ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
+            //Send -> Base
+            xbee.send(tx64request);
+            
+            collect_flag++;
+            collect_state++;
+
+            if(collect_flag == 4){
+                collect_state = 'a';
+                collect_flag = 0;
+            }    
+         } 
+    } 
+}
\ No newline at end of file
diff -r 000000000000 -r 68d27eee9a6c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Apr 25 01:06:06 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8a40adfe8776
\ No newline at end of file