Revision 3:693ea9476763, committed 2016-10-14

Comitter:

s1200058

Date:

Fri Oct 14 07:58:59 2016 +0000

Parent:

2:575479bf1302

Commit message:

test;

Changed in this revision

--- a/MBed_Adafruit-GPS-Library2.lib	Wed Sep 14 11:03:42 2016 +0000
+++ b/MBed_Adafruit-GPS-Library2.lib	Fri Oct 14 07:58:59 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library2/#7b7e9dc49edd
+https://developer.mbed.org/teams/aigamozu/code/MBed_Adafruit-GPS-Library2/#f1c5757af8dd

--- a/main.cpp	Wed Sep 14 11:03:42 2016 +0000
+++ b/main.cpp	Fri Oct 14 07:58:59 2016 +0000
@@ -361,13 +361,13 @@
     const int collect_Time = 1000; 
 
     char SenderIDc;
+     wait_ms(2000);
     //GPS Send Command
-    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_ALLDATA);
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
     myGPS.sendCommand(PGCMD_ANTENNA);
-
-    wait_ms(2000);
-
+        wait_ms(2000);
+        
     //interrupt start
     refresh_Timer.start();
     auto_Timer.start();
@@ -377,6 +377,11 @@
     printf("start\n");
 
     agz.auto_count = 0;
+    
+ //   myGPS.count_[0] = 3;
+ //   myGPS.count_[1] = 3;
+ //   myGPS.count_[2] = 3;
+ myGPS.print_ok = 0;
 
     while (true) {
 
@@ -403,7 +408,7 @@
                     }
                     case STATUS_REQUEST: {
                         Send_Status(SenderIDc);
-                        printf("%c\n", SenderIDc);
+                      //  printf("%c\n", SenderIDc);
                         break;
                     }
                     case CHANGE_MODE: {
@@ -422,7 +427,9 @@
             }//endifZB_RX_RESPONSE
         }//endifisAvailable
 
-        myGPS.read();
+
+             myGPS.read();
+
         //recive gps module
         //check if we recieved a new message from GPS, if so, attempt to parse it,
         if ( myGPS.newNMEAreceived() ) {
@@ -451,11 +458,11 @@
         
 
         //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
-        if (refresh_Timer.read_ms() >= 1000){
-            refresh_Timer.reset();
+//   if (refresh_Timer.read_ms() >= 1000){
+        //    refresh_Timer.reset();
             //print_gps(count);
 
-        }
+     //   }
 
         
     }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Fri Oct 14 07:58:59 2016 +0000
@@ -0,0 +1,467 @@
+/**********************************************/
+//
+//
+//
+//  Program name: Aigamozu ROBOT
+//  Author: Mineta Kizuku
+//
+//
+//
+/**********************************************/
+
+/**********************************************/
+//更新情報
+//2015/05/11
+//ロボットプログラムの作成
+//
+//2015/05/13
+//カルマンフィルタの共分散の値を0.0001以下にならないようにした
+//共分散の値を10進数に変換するようにした
+//
+//2015/05/13 Yokokawa
+//何回目のGPSでとられたGPSか確認するようにしました。
+//
+//2015/05/15
+//プログラムcreateReceiveStatusCommand()にて
+//Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。
+//
+//2015/05//17
+//Send_Status関数内を変更：基地局への送信データのみ現在のモードを入れるパケットの部分に内外判定の結果を入れるようにした
+//基地局以外には現在のモードを送信するようにしてある
+//要確認！！！！
+//
+//2015/05/17
+//Get_GPS()の中身longitudeの範囲138〜140に変更
+//
+//2015/05/19
+//autoモードのとき、三十秒前進・三秒右に転回に変更した。
+//
+//2015/05/24
+//Kalmanフィルターを十進数で計算するようにした。
+//Kalmanフィルターの計算式を変更した。
+//set_kalmanを追加した。
+//
+/**********************************************/
+
+#include "mbed.h"
+#include "XBee.h"
+#include "MBed_Adafruit_GPS.h"
+#include "AigamozuControlPackets.h"
+#include "agzIDLIST.h"
+#include "aigamozuSetting.h"
+#include "Kalman.h"
+#include "math.h"
+
+#define SIGMA_MIN 0.0001
+#define KALMAN_START_TIME 5
+
+//************ID Number*****************
+//Robot   ID: 'A' ~ 'Z'
+//Base    ID: 'a' ~ 'z'
+//manager ID: '0'(数字のゼロ)
+//
+const char MyID = 'B';
+const char Collect_Number = 'a';
+//************ID Number*****************
+
+/////////////////////////////////////////
+//
+//Pin Setting
+//
+/////////////////////////////////////////
+VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
+
+
+/////////////////////////////////////////
+//
+//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();
+
+//set up GPS module
+
+//set up AigamozuControlPackets library
+AigamozuControlPackets agz(agz_motorShield);
+
+
+/////////////////////////////////////////
+//
+//For Kalman data
+//
+/////////////////////////////////////////
+#define FIRST_S1 0.0001
+#define FIRST_S2 0.000001
+#define COUNTER_MAX 10000
+double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
+double s2x_cur=FIRST_S1,s2x_prev=FIRST_S1,s2y_cur=FIRST_S1,s2y_prev=FIRST_S1;//緯度経度のの時刻tと時刻t-1での共分散
+double s2_R=FIRST_S2;//GPSセンサの分散
+double Kx=0,Ky=0;//カルマンゲイン
+double zx,zy;//観測値
+void Kalman(double Latitude,double Longitude);
+int change = 0;
+
+/*
+LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
+FILE *fp;
+char filename[16] = "/local/out0.txt";
+*/
+/////////////////////////////////////////
+//
+//Plus Speed
+//
+//MNUAL_MODEの時にスピードを変える
+/////////////////////////////////////////
+void Plus_Speed(uint8_t *packetdata)
+{
+
+    if(agz.nowMode == MANUAL_MODE) {
+        agz.changeSpeed(packetdata);
+    }
+
+}
+
+
+/////////////////////////////////////////
+//
+//Send Status
+//
+//リクエストがきたとき、自分の位置情報などを返信する
+/////////////////////////////////////////
+void Send_Status(char SenderIDc)
+{
+    XBeeAddress64 send_Address;
+    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());
+    }
+    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());
+    }
+    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());
+    }
+    //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***************************************************
+    */
+    //Select Destination
+    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
+    //Send -> Base
+    xbee.send(tx64request);
+
+}
+
+/////////////////////////////////////////
+//
+//Get GPS function
+//
+/////////////////////////////////////////
+
+void Get_GPS(Adafruit_GPS *myGPS)
+{
+    static int flag = 0;
+//    static int save_counter = 0;
+
+    if (myGPS->fix) {
+
+        agz.nowStatus = GPS_AVAIL;
+
+            /*            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++;
+                            }
+                        }
+            */
+
+    } else agz.nowStatus = GPS_UNAVAIL;
+
+}
+
+/////////////////////////////////////////
+//
+//New Mode
+//
+/////////////////////////////////////////
+
+void New_Mode(uint8_t *packetdata)
+{
+
+    //bool result;
+    agz.changeMode(packetdata);
+
+}
+
+/////////////////////////////////////////
+//
+//Get Status
+//
+/////////////////////////////////////////
+void Get_Status(char SenderIDc,uint8_t *packetdata)
+{
+    int id = 0;
+
+        //printf("Get data\n");
+        agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
+        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
+        agz.set_agzCov_from_packet(&packetdata[45],&packetdata[53]);
+
+
+        printf("%d, %f, %f\n",
+               SenderIDc, agz.get_basePoint_lati(id),agz.get_basePoint_longi(id)
+              );
+    
+
+}
+
+
+/////////////////////////////////////////
+//
+//Send_Request
+//
+/////////////////////////////////////////
+void Send_Request(int number)
+{
+
+    agz.createRequestCommand(MyID, number);
+    //Select Destination!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+    //基地局の場合
+    if('a' <= number && number <= 'z'){
+    number = number - 'a';
+    //printf("Send b:%d\n",number);
+    ZBTxRequest tx64request(base_Address[number],agz.packetData,agz.getPacketLength());
+    //Send
+    xbee.send(tx64request);
+    }
+    //ロボットの場合
+    if('A' <= number && number <= 'Z'){
+    number = number - 'A';
+    //printf("Send r:%d\n",MyID);
+    ZBTxRequest tx64request(robot_Address[number],agz.packetData,agz.getPacketLength());
+    //Send
+    xbee.send(tx64request);
+    }
+
+}
+
+
+void print_gps(int count)
+{
+
+    printf("%d times:\n", count);
+    printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
+
+}
+
+
+/////////////////////////////////////////
+//
+//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 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);
+
+}
+
+/////////////////////////////////////////
+//
+//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;
+    Timer auto_Timer;
+//    const int straight = 8000, turning = 12000, wait = 10000;
+
+    myGPS.begin(GPS_BAUD_RATE);
+
+    Timer collect_Timer;
+    const int collect_Time = 1000; 
+
+    char SenderIDc;
+    //GPS Send Command
+    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();
+    agz.Automove_Timer.start();
+    printf("start\n");
+
+    agz.auto_count = 0;
+
+    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: {
+                        Plus_Speed(buf);
+                        break;
+                    }
+                    case STATUS_REQUEST: {
+                        Send_Status(SenderIDc);
+                        //printf("%c\n", SenderIDc);
+                        break;
+                    }
+                    case CHANGE_MODE: {
+                        New_Mode(buf);
+                        break;
+                    }
+                    case RECEIVE_STATUS: {
+                        //printf("GET");
+                        Get_Status(SenderIDc,buf1);
+                        break;
+                    }
+                    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{
+ 
+            }    
+            if(strstr(myGPS.lastNMEA(), "$GPGSA")){
+                //myGPS.GPGSAdata=myGPS.lastNMEA();
+                //printf("%s",myGPS.GPGSAdata);
+            }
+            if(strstr(myGPS.lastNMEA(), "$GPGGA")){
+                //myGPS.GPGGAdata=myGPS.lastNMEA();
+                //printf("%s",myGPS.GPGGAdata);
+            }
+            if(strstr(myGPS.lastNMEA(), "$GPRMC")){
+                //myGPS.GPRMCdata=myGPS.lastNMEA();
+                //printf("%s",myGPS.GPRMCdata);
+            }
+            if(strstr(myGPS.lastNMEA(), "$GPGSV")){
+                //myGPS.GPRMCdata=myGPS.lastNMEA();
+                //printf("%s",myGPS.GPRMCdata);
+            } 
+        }
+        
+
+        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
+      if(refresh_Timer.read_ms() >= 3000 && myGPS.print_ok == 1){
+            printf("%s\n",myGPS.GPGGAdata);               
+            printf("%s\n",myGPS.GPGSAdata);
+            printf("%s\n",myGPS.GPRMCdata);
+            printf("%s\n",myGPS.GPGSVdataA);
+            printf("%s\n",myGPS.GPGSVdataB);
+            printf("%s\n",myGPS.GPGSVdataC);
+            printf("%s\n",myGPS.GPGSVdataD);
+            refresh_Timer.reset();
+    }
+
+        
+    }
+
+}
\ No newline at end of file