test

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

Fork of aigamozu_auto_ver3_4 by aigamozu

Files at this revision

API Documentation at this revision

Comitter:
kityann
Date:
Mon Apr 27 12:08:38 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 567aa3346701 ADXL345.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345.lib	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ADXL345/#45faba962a46
diff -r 000000000000 -r 567aa3346701 AigamozuControlPackets.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AigamozuControlPackets.lib	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#ac839aff80bc
diff -r 000000000000 -r 567aa3346701 HMC5843.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5843.lib	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/elrafapadron/code/HMC5843/#fdab96fc6fff
diff -r 000000000000 -r 567aa3346701 ITG3200.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Digixx/code/ITG3200/#8967cbe04d96
diff -r 000000000000 -r 567aa3346701 Kalman/Kalman.cpp
diff -r 000000000000 -r 567aa3346701 Kalman/Kalman.h
diff -r 000000000000 -r 567aa3346701 MBed_Adafruit-GPS-Library.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MBed_Adafruit-GPS-Library.lib	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library/#cc9ab73d0624
diff -r 000000000000 -r 567aa3346701 XBee.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XBee.lib	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/okini3939/code/XBee/#b36422ef864f
diff -r 000000000000 -r 567aa3346701 agzIDLIST.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agzIDLIST.lib	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agzIDLIST/#3f90a1c00a72
diff -r 000000000000 -r 567aa3346701 agz_common.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/agz_common.lib	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/agz_common/#14e469b0c33e
diff -r 000000000000 -r 567aa3346701 aigamozuSetting.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aigamozuSetting.h	Mon Apr 27 12:08:38 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 567aa3346701 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,336 @@
+/**********************************************/
+//  
+//    
+//
+//  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[5];
+
+char MyID = 'Z';
+
+/////////////////////////////////////////
+//
+//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,SenderIDc;
+    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("start\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]; 
+                id = buf1[5] - 'A';
+                SenderIDc = buf1[5];
+                 
+                 
+                 //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:{
+                            if(SenderIDc == 'Z'){
+                                //send normal data
+                                //Create GPS Infomation Packet
+                                agz.createReceiveStatusCommand(MyID,SenderIDc,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(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
+                                //Select Destination
+                                ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
+                                //Send -> Base
+                                xbee.send(tx64request1);
+                                
+                            }else{
+                                
+                                //send normal data
+                                //Create GPS Infomation Packet
+                                agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+                                //Select Destination
+                                ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength());
+                                //Send -> Base
+                                xbee.send(tx64request2);
+                            
+                                //send Kalman data
+                                agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
+                                //Select Destination
+                                ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
+                                //Send -> Base
+                                xbee.send(tx64request3);
+                            }
+                            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 this program is for base,this program don't execute
+        
+        if( collect_Timer.read_ms() >= collect_Time){
+            collect_Timer.reset();
+            
+            //printf("Send Request:%d,%d\n",collect_flag,collect_state);
+                      
+            agz.createRequestCommand(MyID, 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 567aa3346701 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Apr 27 12:08:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8a40adfe8776
\ No newline at end of file