20150511, Nakazawa

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

Fork of aigamozu_auto_ver3_3_2 by aigamozu

Revision:
7:209c07cc2b80
Parent:
4:480f4eb9bcbc
diff -r 11a8ae7ab71e -r 209c07cc2b80 main.cpp
--- a/main.cpp	Sun Apr 26 16:52:54 2015 +0000
+++ b/main.cpp	Mon May 11 13:23:52 2015 +0000
@@ -9,6 +9,16 @@
 //  
 //
 /**********************************************/
+//
+//  中澤遥菜
+//  変更点:メイン関数の中の処理を関数化しました。
+//  setting(), interrupt_start(VNH5019 motorShield),
+//  GPS_Setting(int refresh_Time, int collect_Time),
+//  GPS_SendCommand(), check_CommandType(utf8_t command_type),
+//  LatLong_afterFiltering(double longitude, double latitude, double x0, double x1),
+//  send_GPSdata(), get_GPSdata(), getKalman()
+//
+/**********************************************/
 
 #include "mbed.h"
 #include "XBee.h"
@@ -106,65 +116,72 @@
     get_sigma();
 }
 
-
 /////////////////////////////////////////
 //
-//Main Processing
+//setting
 //
 /////////////////////////////////////////
-int main() {
-    //start up time
+void setting(){
+     //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);    
+    }
+
+/////////////////////////////////////////
+//
+//interrupt start
+//
+/////////////////////////////////////////
+void interrupt_start(VNH5019 motorShield, Timer refresh){
+    AigamozuControlPackets agz(motorShield);
+    refresh.start();
     
-    //GPS setting
+    printf("start\n");
+}
+
+/////////////////////////////////////////
+//
+//GPS Setting
+//
+/////////////////////////////////////////
+XBeeAddress64 collect_Address[5];
+XBeeAddress64 robot_Address
+
+void GPS_Setting(Timer refresh_Timer, Timer collect_Timer){
     gps_Serial = new Serial(p28,p27);
     Adafruit_GPS myGPS(gps_Serial); 
     Timer refresh_Timer;
-    const int refresh_Time = 2000; //refresh time in ms
+    const int refresh_Time = refresh_Timer; //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[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
+    const int collect_Time = collect_Timer; //when we collect 4 GPS point, we use 
+    collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
                                         XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), XBeeAddress64(BASE5_32H,BASE5_32L)};
-    XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
-    int id;
-    bool flag = true;
-    
-    //GPS Send Command
+    robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
+    }
+
+/////////////////////////////////////////
+//
+//GPS Send Command
+//
+/////////////////////////////////////////
+void GPS_SendCommand(){
     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]; 
-                id = buf1[5] - 'A';
-                 
-                 
-                 //Check Command Type 
-                 switch(agz.checkCommnadType(buf)){
+/////////////////////////////////////////
+//
+//Check Command Type
+//
+/////////////////////////////////////////
+void check_CommandType(utf8_t command_type, AigamozuControlPackets ai(motorShield), ){
+    switch(command_type){
                     
                     //CommandType -> ChanegeMode
                     case CHANGE_MODE :{
@@ -183,26 +200,12 @@
                     
                     //CommandType -> Send Status
                     case STATUS_REQUEST:{
+                           
                             //send normal data
                             //Create GPS Infomation Packet
                             agz.createReceiveStatusCommand(Base_ID,'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(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
-                            //Select Destination
-                            ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
-                            //Send -> Base
-                            xbee.send(tx64request);
-                            
-                            //send normal data
-                            //Create GPS Infomation Packet
-                            agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
-                            //Select Destination
-                            ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength());
+                            ZBTxRequest tx64request2(collect_Address[id],ai.packetData,ai.getPacketLength());
                             //Send -> Base
                             xbee.send(tx64request);
                             
@@ -215,47 +218,109 @@
                             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;
                     }
                 }
-                    
-            
-                }
             }
 
+/////////////////////////////////////////
+//
+//latitude and longitude after filtering
+//
+/////////////////////////////////////////
+void LatLong_afterFiltering(double longitude, double latitude, double x0, double x1){
+    myGPS.longitudeKH=longitude;//longitude after filtering
+    myGPS.latitudeKH=latitude;//latitude after filtering
+    myGPS.longitudeKL=(long)x0;//longitude after filtering
+    myGPS.latitudeKL=(long)x1;//latitude after filtering
+}
+
+/////////////////////////////////////////
+//
+//send_GPSdata()
+//
+/////////////////////////////////////////
+void send_GPSdata(){
+    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';+
+                           
+        //Check Command Type 
+        check_CommandType(agz.checkCommnadType(buf));
             
+    }
+}
+
+/////////////////////////////////////////
+//
+//get_GPSdata
+//
+/////////////////////////////////////////
+void get_GPSdata(){
+    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;
+}
+
+/////////////////////////////////////////
+//
+//get_Kalman
+//
+/////////////////////////////////////////
+void get_Kalman(){
+    //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];
+        }
+    }
+                
+    LatLong_afterFiltering(myGPS.longitudeH, myGPS.latitudeH, x[1][0], x[1][1]);
+    }
+/////////////////////////////////////////
+//
+//Main Processing
+//
+/////////////////////////////////////////
+int main() {
+    setting();
+    
+    //GPS setting
+    GPS_Setting(2000, 200);
+    int collect_flag = 0;
+    char collect_state = 'a';
+    int id;
+    bool flag = true;
+    
+    //GPS Send Command
+    GPS_SendCommand();
+    
+    wait(2);
+       
+    //interrupt start
+    interrupt_start(agz_motorShield, refreshTime);
+    
+    while (true) {
+        
+        //Check Xbee Buffer Available
+        xbee.readPacket();
+            if (xbee.getResponse().isAvailable()) {
+                send_GPSdata();
+            }
+           
         myGPS.read();
         //recive gps module
         //check if we recieved a new message from GPS, if so, attempt to parse it,
@@ -269,30 +334,9 @@
             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;
+                get_GPSdata();
                 
-                //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
+                get_Kalman();
                 
                 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                 agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
@@ -303,26 +347,6 @@
             
         } 
         
-        
-        //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 == 5){
-                collect_state = 'a';
-                collect_flag = 0;
-            }    
-         } 
+    
     } 
 }
\ No newline at end of file