Diff: main.cpp

Revision:

0:567aa3346701

Child:

1:004530c56f4b

--- /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