aigamozu
GPS精度実験
Dependencies: ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library2 XBee2 agzIDLIST_2016 mbed
Diff: main.cpp.orig
- Revision:
- 3:693ea9476763
--- /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