aigamozu
test
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Revision 0:567aa3346701, committed 2015-04-27
- Comitter:
- kityann
- Date:
- Mon Apr 27 12:08:38 2015 +0000
- Commit message:
- test
Changed in this revision
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