Diff: main.cpp

Revision:

0:1fcc61be1dcf

Child:

1:f1f7413ae6bd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 09 07:46:44 2015 +0000
@@ -0,0 +1,253 @@
+/**********************************************/
+//更新情報
+
+/**********************************************/
+
+#include "mbed.h"
+#include "XBee.h"
+#include "MBed_Adafruit_GPS.h"
+#include "AigamozuControlPackets.h"
+#include "agzIDLIST.h"
+#include "aigamozuSetting.h"
+#include "math.h"
+
+#define SIGMA_MIN 0.0001
+
+/////////////////////////////////////////
+//
+//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_S2_1 1.0e-8
+#define FIRST_S2_2 1.0e-6
+#define COUNTER_MAX 10000
+#define ERROR_RANGE 0.001
+
+double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
+double s2x_cur=FIRST_S2_1,s2x_prev=FIRST_S2_1,s2y_cur=FIRST_S2_1,s2y_prev=FIRST_S2_1;//緯度経度のの時刻tと時刻t-1での共分散
+double s2_R=FIRST_S2_2;//GPSセンサの分散
+double s2_Q=FIRST_S2_2;
+double Kx=0,Ky=0;//カルマンゲイン
+double zx,zy;//観測値
+void Kalman(double Latitude,double Longitude);
+int change = 0;
+
+
+
+/////////////////////////////////////////
+//
+//Kalman Processing
+//
+/////////////////////////////////////////
+void calc_Kalman(){
+  //calc Kalman gain
+  Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q);
+  Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q);
+  //estimate
+  x_cur = x_prev + Kx*(zx-x_prev);
+  y_cur = y_prev + Ky*(zy-y_prev);
+  //calc sigma
+  s2x_cur = (1-Kx)*(s2x_prev+s2_Q);
+  s2y_cur = (1-Ky)*(s2y_prev+s2_Q);
+
+}
+
+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);
+    
+}
+
+/////////////////////////////////////////
+//
+//Get GPS function
+//
+/////////////////////////////////////////
+
+void Get_GPS(Adafruit_GPS *myGPS){
+    static int flag = 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 == 5){
+            x_prev = agz.get_agzPoint_lati();
+            y_prev = agz.get_agzPoint_longi();
+        }
+            
+        if(flag >= 6){
+            if(abs(x_prev - agz.get_agzPoint_lati()) < ERROR_RANGE && abs(y_prev - agz.get_agzPoint_longi()) < ERROR_RANGE){
+                Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
+                change = 1;
+            }
+            else{
+                change = 0;
+            }            
+                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;
+    
+} 
+
+/******************************
+スタンバイモード
+******************************/
+void standby(){
+    ;
+}
+
+/******************************
+落下モード
+******************************/
+void falling(){
+                
+}
+
+/******************************
+走行モード
+******************************/
+void running(){
+            
+}
+
+/******************************
+停止モード
+******************************/
+void stopping(){
+            
+            
+}
+
+/////////////////////////////////////////
+//
+//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 = 1000; //refresh time in ms
+    Timer auto_Timer;
+    const int auto_Time = 100; //refresh time in ms
+    int count = 0;
+
+    myGPS.begin(GPS_BAUD_RATE); 
+    
+    //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();
+
+    printf("start\n");
+    
+    int mode = -1;
+    
+    while (true) {
+        
+        switch(mode){
+            //スタートモード：パラシュートが開くまではこのモードを実行
+            case -1:
+                standby();
+            break;
+            //落下モード：落下時はこのモード。気圧計または、時間でロボットとパラシュートを分離する
+            case 1:
+                falling();
+            break;
+            //走行モード：ターゲットにむかって走行を行う
+            case 2:
+                running();
+            break;
+            //停止モード：ターゲット
+            case 100:
+                stopping();
+            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;   
+            } 
+            else{
+                count++;
+            }    
+        }
+        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
+        if (refresh_Timer.read_ms() >= refresh_Time) {
+            refresh_Timer.reset();
+            //print_gps(count);
+            Get_GPS(&myGPS);
+
+        }
+    }
+    
+}
\ No newline at end of file