aigamozu / Mbed 2 deprecated aigamozu_auto_ver2

ver2

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

Fork of aigamozu_auto_ver1 by aigamozu

Files at this revision

API Documentation at this revision

Revision 5:45e5e9e3dff3, committed 2015-04-20

Comitter:
kityann
Date:
Mon Apr 20 10:38:55 2015 +0000
Parent:
4:dc2babaa1f61
Commit message:
a

Changed in this revision

AigamozuControlPackets.lib 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
agz_common.lib 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 
--- a/AigamozuControlPackets.lib	Tue Apr 14 15:11:29 2015 +0000
+++ b/AigamozuControlPackets.lib	Mon Apr 20 10:38:55 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#eaab0ccb9255
+http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#a5bc425540a7

--- a/MBed_Adafruit-GPS-Library.lib	Tue Apr 14 15:11:29 2015 +0000
+++ b/MBed_Adafruit-GPS-Library.lib	Mon Apr 20 10:38:55 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library/#8203e954d8e1
+http://mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library/#cc9ab73d0624

--- a/agz_common.lib	Tue Apr 14 15:11:29 2015 +0000
+++ b/agz_common.lib	Mon Apr 20 10:38:55 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/aigamozu/code/agz_common/#54e62ef6d287
+http://mbed.org/teams/aigamozu/code/agz_common/#14e469b0c33e

--- a/main.cpp	Tue Apr 14 15:11:29 2015 +0000
+++ b/main.cpp	Mon Apr 20 10:38:55 2015 +0000
@@ -60,16 +60,22 @@
 double y[2],x[2][2];
     
 void get_K(){
-    double ad_bc = (sigma[0][0][0]+sigmaGPS[0][0])*(sigma[0][1][1]+sigmaGPS[1][1])-(sigma[0][1][0]+sigmaGPS[1][0])*(sigma[0][0][1]+sigmaGPS[0][1]);
-    K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(sigma[1][1][1]+sigmaGPS[1][1]);
-    K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(sigma[1][0][0]+sigmaGPS[0][0]);
+  double temp[2][2]={
+    {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][1][0]+sigmaGPS[1][0]},
+    {sigma[0][0][1]+sigmaGPS[0][1],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[0][0])+sigma[0][1][0]*(1/ad_bc)*(temp[0][1]);
+  K[1][1][1] = sigma[0][0][1]*(1/ad_bc)*(temp[1][0])+sigma[0][1][1]*(1/ad_bc)*(temp[1][1]);
 }
 
+
 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]);
+  x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0])+K[1][1][0]*(y[0]-x[0][0]);
+  x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1])+K[1][0][1]*(y[1]-x[1][1]);
 }
 
+
 void get_sigma(){
     double temp[2][2];
     for(int i=0;i<2;i++) {
@@ -173,10 +179,18 @@
                     
                     //CommandType -> Send Status
                     case STATUS_REQUEST:{
+                            //send normal data
                             //Create GPS Infomation Packet
-                            agz.createReceiveStatusCommand('D','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+                            agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                             //Select Destination
-                                ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
+                            ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
+                            //Send -> Base
+                            xbee.send(tx64request);
+                            
+                            //send Kalman data
+                            agz.createReceiveStatusCommandwithKalman('B','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;         
@@ -201,7 +215,18 @@
                     */
                         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;
                     }
@@ -242,10 +267,13 @@
                         sigma[0][i][j]=sigma[1][i][j];
                     }
                 }
-                myGPS.longitudeL=(long)x[1][0];//longitude after filtering
-                myGPS.latitudeL=(long)x[1][1];//latitude after filtering
+                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);
             }