Diff: main.cpp

Revision:

39:ff6819c7a066

Parent:

38:e2aa1165d28d

Child:

40:aa600a5ba02c

diff -r e2aa1165d28d -r ff6819c7a066 main.cpp
--- a/main.cpp	Thu May 19 07:52:14 2016 +0000
+++ b/main.cpp	Wed May 25 10:49:29 2016 +0000
@@ -1,11 +1,11 @@
 /**********************************************/
-//  
-//    
+//
+//
 //
 //  Program name: Aigamozu ROBOT
 //  Author: Mineta Kizuku
-//  
-// 
+//
+//
 //
 /**********************************************/
 
@@ -59,7 +59,8 @@
 //Base    ID: 'a' ~ 'a'
 //manager ID: '0'(数字のゼロ)
 //
-const char MyID = 'A';
+const char MyID = 'C';
+const int other_robot = 7;
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -77,7 +78,7 @@
 /////////////////////////////////////////
 
 //Serial Connect Setting: PC <--> mbed
-Serial pc(USBTX, USBRX);    
+Serial pc(USBTX, USBRX);
 
 //Serial Connect Setting: GPS <--> mbed
 Serial * gps_Serial;
@@ -118,63 +119,66 @@
 //
 //MNUAL_MODEの時にスピードを変える
 /////////////////////////////////////////
-void Plus_Speed(uint8_t *packetdata){
-       
-    if(agz.nowMode == MANUAL_MODE){ 
+void Plus_Speed(uint8_t *packetdata)
+{
+
+    if(agz.nowMode == MANUAL_MODE) {
         agz.changeSpeed(packetdata);
     }
-    
+
 }
 
+
 /////////////////////////////////////////
 //
 //Send Status
 //
 //リクエストがきたとき、自分の位置情報などを返信する
 /////////////////////////////////////////
-void Send_Status(char SenderIDc){
+void Send_Status(char SenderIDc)
+{
     XBeeAddress64 send_Address;
-    if(SenderIDc == '0'){
+    if(SenderIDc == '0') {
         send_Address = manager_Address;
         agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
-                                        agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                                        agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                                        agz.get_agzCov_lati(),agz.get_agzCov_longi());             
+                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
     }
-    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
         send_Address = robot_Address[SenderIDc - 'A'];
         //Create GPS Infomation Packet
         agz.createReceiveStatusCommand(MyID,SenderIDc, agz.nowMode,
-                                        agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                                        agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                                        agz.get_agzCov_lati(),agz.get_agzCov_longi());
+                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
     }
-    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+    if(SenderIDc >= 'a' && SenderIDc <= 'z') {
         send_Address = base_Address[SenderIDc - 'a'];
 
         agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(),
-                                        agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                                        agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                                        agz.get_agzCov_lati(),agz.get_agzCov_longi());    
+                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
     }
     //send normal data
 
- /*   
-    //debug***************************************************
-    printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
-            agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-            agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-            agz.get_agzCov_lati(),agz.get_agzCov_longi()
-            );
-    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
-        printf("\n");
-    //debug end***************************************************
- */   
+    /*
+       //debug***************************************************
+       printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
+               agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+               agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+               agz.get_agzCov_lati(),agz.get_agzCov_longi()
+               );
+       for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
+           printf("\n");
+       //debug end***************************************************
+    */
     //Select Destination
     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
     //Send -> Base
     xbee.send(tx64request);
-    
+
 }
 
 /////////////////////////////////////////
@@ -183,55 +187,64 @@
 //
 /////////////////////////////////////////
 
-void Get_GPS(Adafruit_GPS *myGPS){
+void Get_GPS(Adafruit_GPS *myGPS)
+{
+    printf("test");
     static int flag = 0;
 //    static int save_counter = 0;
-    
+
     if (myGPS->fix) {
-        
+
         agz.nowStatus = GPS_AVAIL;
         agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
-        
-        if(flag < COUNTER_MAX){
-            flag++; 
+
+        if(flag < COUNTER_MAX) {
+            flag++;
         }
-        if(flag == 15){
+        if(flag == 15) {
             x_prev = agz.get_agzPoint_lati();
             y_prev = agz.get_agzPoint_longi();
         }
-            
-        if(flag >= 16){
-            if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001){
+
+        if(flag >= 16) {
+            if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001) {
                 Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
                 change = 1;
-            }
-            else{
+            } else {
                 change = 0;
             }
-/*            if(save_counter < 10){
-                fp = fopen(filename, "a");
-                fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
-                    change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                    agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                    agz.get_agzCov_lati(),agz.get_agzCov_longi());
-                fclose(fp);   
-                
-                if((flag - 16) % 500 == 0){
-                    filename[10]++;
-                    save_counter++;
-                }
-            }
-*/
-        } 
-            
-                printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
-                agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                agz.get_agzCov_lati(),agz.get_agzCov_longi());
-    }
-    else agz.nowStatus = GPS_UNAVAIL;
-    
-} 
+            /*            if(save_counter < 10){
+                            fp = fopen(filename, "a");
+                            fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
+                                change, agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                agz.get_agzCov_lati(),agz.get_agzCov_longi());
+                            fclose(fp);
+
+                            if((flag - 16) % 500 == 0){
+                                filename[10]++;
+                                save_counter++;
+                            }
+                        }
+            */
+        }
+
+        printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \n",
+               agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+               agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+               agz.get_agzCov_lati(),agz.get_agzCov_longi());
+
+        agz.createReceiveStatusCommand(MyID, other_robot, agz.nowMode,
+                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
+        //Select Destination
+        ZBTxRequest tx64request(robot_Address[other_robot],agz.packetData,agz.getPacketLength());
+        //Send -> Base
+        xbee.send(tx64request);
+    } else agz.nowStatus = GPS_UNAVAIL;
+
+}
 
 /////////////////////////////////////////
 //
@@ -239,68 +252,68 @@
 //
 /////////////////////////////////////////
 
-void New_Mode(uint8_t *packetdata){
-  
-  //bool result;
-   agz.changeMode(packetdata); 
-  
+void New_Mode(uint8_t *packetdata)
+{
+
+    //bool result;
+    agz.changeMode(packetdata);
+
 }
-  
+
 /////////////////////////////////////////
 //
 //Get Status
 //
 /////////////////////////////////////////
-void Get_Status(char SenderIDc,uint8_t *packetdata){
-    
+void Get_Status(char SenderIDc,uint8_t *packetdata)
+{
+    printf("get status\n");
+
     //マネージャからデータが来たとき
-    if(SenderIDc == '0'){
+    if(SenderIDc == '0') {
         printf("get manager Status\n");
     }
     //他のロボットからデータが来たとき
-    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
-        printf("get other robots Status\n");
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
+        int id = SenderIDc - 'A';
+        printf("get other robots Status %d\n", id+1);
+
     }
     //基地局からデータが来たとき
-    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
-        printf("Get Base data\n");
+    if(SenderIDc >= 'a' && SenderIDc <= 'z') {
         int id = SenderIDc - 'a';
+        printf("Get Base data %d\n", id+1);
         agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
-        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
-        
+        //agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
+
         //debug
-        for(int i = 0;i < 4;i++){
-        printf("BASE:%d\n",i);
-        printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
-            agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
-            agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
-            );
-        }
+        printf("latitude:%f,longitude:%f\n",
+               agz.get_basePoint_lati(id),agz.get_basePoint_longi(id)
+              );
     }
 }
 
-void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
-    
+void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata)
+{
+
     //マネージャからデータが来たとき
-    if(SenderIDc == '0'){
+    if(SenderIDc == '0') {
         printf("get manager Status Kalman\n");
     }
     //他のロボットからデータが来たとき
-    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z') {
         printf("get other robots Status Kalman\n");
+        int id = SenderIDc - 'A';
     }
     //基地局からデータが来たとき
-    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+    if(SenderIDc >= 'a' && SenderIDc <= 'z') {
         printf("Get Base data Kalman\n");
         int id = SenderIDc - 'a';
         agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
-        
+
         //debug
-        for(int i = 0;i < 4;i++){
-        printf("BASE:%d\n",i);
-        printf("latitudeK:%f,longitudeK:%f\n",
-            agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i));
-        }
+        printf("BASE %d:  latitudeK:%f,longitudeK:%f\n",
+               id, agz.get_basePointKalman_lati(id),agz.get_basePointKalman_longi(id));
     }
 }
 
@@ -309,8 +322,9 @@
 //Send_Request_to_base
 //
 /////////////////////////////////////////
-void Send_Request_Base(int basenumber){
-    printf("send\n");
+void Send_Request_Base(int basenumber)
+{
+    //printf("send\n");
     agz.createRequestCommand(MyID, basenumber);
     //Select Destination
     ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
@@ -326,33 +340,34 @@
 //Kalmanを通した値を出力(Baseと自分)
 /////////////////////////////////////////
 
-void auto_Move(){
- 
- bool result;
- int i;
- 
- result = agz.gpsAuto();
- //agz.set_agzAutoGPS();
- //agz.set_agzKalmanGPS();
- 
- if(result == true){
+void auto_Move()
+{
+
+    bool result;
+    int i;
+
+    result = agz.gpsAuto();
+//agz.set_agzAutoGPS();
+//agz.set_agzKalmanGPS();
+
+    if(result == true) {
         printf("Out Area\n");
- }
-else if(result == false){
+    } else if(result == false) {
         printf("In Area\n");
-}  
-for(i = 0; i < 4; i++){
-    printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
- }
- printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
- 
+    }
+    for(i = 0; i < 4; i++) {
+        printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
+    }
+    printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
+
 }
 
-void print_gps(int count){
-    
+void print_gps(int count)
+{
+
     printf("%d times:\n", count);
     printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
-    
+
 }
 
 
@@ -361,37 +376,39 @@
 //Kalman Processing
 //
 /////////////////////////////////////////
-void calc_Kalman(){
-  //calc Kalman gain
-  Kx = s2x_prev/(s2x_prev+s2_R);
-  Ky = s2y_prev/(s2y_prev+s2_R);
-  //estimate
-  x_cur = x_prev + Kx*(zx-x_prev);
-  y_cur = y_prev + Ky*(zy-y_prev);
-  //calc sigma
-  s2x_cur = s2x_prev-Kx*s2x_prev;
-  s2y_cur = s2y_prev-Ky*s2y_prev;
+void calc_Kalman()
+{
+    //calc Kalman gain
+    Kx = s2x_prev/(s2x_prev+s2_R);
+    Ky = s2y_prev/(s2y_prev+s2_R);
+    //estimate
+    x_cur = x_prev + Kx*(zx-x_prev);
+    y_cur = y_prev + Ky*(zy-y_prev);
+    //calc sigma
+    s2x_cur = s2x_prev-Kx*s2x_prev;
+    s2y_cur = s2y_prev-Ky*s2y_prev;
 
 }
 
-void Kalman(double Latitude,double Longitude){
+void Kalman(double Latitude,double Longitude)
+{
 
     zx = Latitude;
     zy = Longitude;
 
     calc_Kalman();
-    
+
     //更新
     x_prev = x_cur;
     y_prev = y_cur;
     s2x_prev = s2x_cur;
     s2y_prev = s2y_cur;
-    
+
     //agzPontKalmanとagzCovに格納する
     agz.set_agzPointKalman_lati(x_cur);
     agz.set_agzPointKalman_longi(y_cur);
     agz.set_agzCov(s2x_cur,s2y_cur);
-    
+
 }
 
 /////////////////////////////////////////
@@ -399,144 +416,148 @@
 //Main Processing
 //
 /////////////////////////////////////////
-int main() {
+int main()
+{
     //start up time
     wait(3);
-    //set pc frequency to 57600bps 
-    pc.baud(PC_BAUD_RATE); 
+    //set pc frequency to 57600bps
+    pc.baud(PC_BAUD_RATE);
     //set xbee frequency to 57600bps
-    xbee.begin(XBEE_BAUD_RATE);    
-        
+    xbee.begin(XBEE_BAUD_RATE);
+
 
     //GPS setting
     gps_Serial = new Serial(p28,p27);
-    Adafruit_GPS myGPS(gps_Serial); 
+    Adafruit_GPS myGPS(gps_Serial);
     Timer refresh_Timer;
     const int refresh_Time = 1000; //refresh time in ms
     Timer auto_Timer;
     const int auto_Time = 2000; //refresh time in ms
     int count = 0;
-    
+    Timer startGPS_Timer;
+    const int startGPS_Time = 180000;
+
     const int straight = 8000, turning = 12000, wait = 10000;
-    
-    myGPS.begin(GPS_BAUD_RATE); 
-    
+
+    myGPS.begin(GPS_BAUD_RATE);
+
     Timer collect_Timer;
-    const int collect_Time = 2000; //when we collect 4 GPS point, we use 
+    const int collect_Time = 2000; //when we collect 4 GPS point, we use
     int collect_flag = 0;
-    
+
     char SenderIDc;
     //GPS Send Command
-    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
     myGPS.sendCommand(PGCMD_ANTENNA);
-    
+
     wait_ms(2000);
-       
+
     //interrupt start
     refresh_Timer.start();
     auto_Timer.start();
     agz.Move_Timer.start();
     collect_Timer.start();
+    startGPS_Timer.start();
     printf("start\n");
-    
-    
+
+
     while (true) {
-        
+
         //Check Xbee Buffer Available
         xbee.readPacket();
-            
+
         if (xbee.getResponse().isAvailable()) {
             xbee.getResponse().getZBRxResponse(zbRx);
             uint8_t *buf = zbRx.getFrameData();
-                
+
             if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
                 xbee.getResponse().getZBRxResponse(zbRx);
                 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
                 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
                 SenderIDc = buf1[5];//送信元のIDを取得する
                 char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する
-                 
-                //Check Command Type 
-                switch(Command_type){
-                    //Get Request command
-                    case MANUAL:{
+
+                //Check Command Type
+                switch(Command_type) {
+                        //Get Request command
+                    case MANUAL: {
                         Plus_Speed(buf);
                         break;
                     }
-                    case STATUS_REQUEST:{
-                        Send_Status(SenderIDc); 
-                        printf("%c\n", SenderIDc);                  
-                        break;          
+                    case STATUS_REQUEST: {
+                        Send_Status(SenderIDc);
+                        printf("%c\n", SenderIDc);
+                        break;
                     }
-                    case CHANGE_MODE:{
+                    case CHANGE_MODE: {
                         New_Mode(buf);
                         break;
                     }
-                    case RECEIVE_STATUS:{
+                    case RECEIVE_STATUS: {
                         Get_Status(SenderIDc,buf1);
                         break;
                     }
-                    default:{
+                    default: {
                         break;
                     }
                 }//endswitch
             }//endifZB_RX_RESPONSE
         }//endifisAvailable
-                       
+
         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;   
-            } 
-            else{
+                continue;
+            } else {
                 count++;
-            }    
+            }
         }
         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
         if (refresh_Timer.read_ms() >= refresh_Time) {
             refresh_Timer.reset();
             //print_gps(count);
-            Get_GPS(&myGPS);
-
+            if(startGPS_Timer.read_ms() >= startGPS_Time) {
+                Get_GPS(&myGPS);
+            }
         }
-    
-                 //get base GPS
-        if( collect_Timer.read_ms() >= collect_Time){
-            collect_Timer.reset();   
-                     
+
+        //get base GPS
+        if( collect_Timer.read_ms() >= collect_Time) {
+            collect_Timer.reset();
+
             Send_Request_Base(collect_flag);
-            
+
             collect_flag++;
 
-            if(collect_flag == 4){
+            if(collect_flag == 4) {
                 collect_flag = 0;
-            }    
-         } 
-            
-        if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
-           auto_Timer.reset();
-           auto_Move(); 
+            }
+        }
+
+        if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time) {
+            auto_Timer.reset();
+            auto_Move();
         }
-        
-     if(agz.nowMode == AUTO_GPS_MODE){
-        if(agz.Move_Timer.read_ms() < straight){
-           agz.test_Auto(0); //straight
-        }
-        if(agz.Move_Timer.read_ms() > straight && agz.Move_Timer.read_ms() < wait){
-            agz.test_Auto(1);
-        }
-        if(agz.Move_Timer.read_ms() > wait && agz.Move_Timer.read_ms() < turning){
-            agz.test_Auto(2); //Turn Right
-        }   
-        if(agz.Move_Timer.read_ms() > turning){
-            agz.test_Auto(3);
-            wait_ms(200);
-            agz.Move_Timer.reset();
+
+        if(agz.nowMode == AUTO_GPS_MODE) {
+            if(agz.Move_Timer.read_ms() < straight) {
+                agz.test_Auto(0); //straight
+            }
+            if(agz.Move_Timer.read_ms() > straight && agz.Move_Timer.read_ms() < wait) {
+                agz.test_Auto(1);
+            }
+            if(agz.Move_Timer.read_ms() > wait && agz.Move_Timer.read_ms() < turning) {
+                agz.test_Auto(2); //Turn Right
+            }
+            if(agz.Move_Timer.read_ms() > turning) {
+                agz.test_Auto(3);
+                wait_ms(200);
+                agz.Move_Timer.reset();
+            }
         }
     }
-    }
-    
+
 }
\ No newline at end of file