Diff: main.cpp

Revision:

0:75c267089975

diff -r 000000000000 -r 75c267089975 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 06 10:49:22 2016 +0000
@@ -0,0 +1,546 @@
+/**********************************************/
+//  
+//    
+//
+//  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を追加した。
+//
+//2015/05/29
+//auto_Move関数の実装とAigamozuControlPackets内にcontrol_Mortor関数の実装
+//
+//2015/05/30
+//新カルマンフィルタの実装
+//
+//2015/06/04
+//内外判定の結果によって動きを変えるように変更
+//ロボットA,B,C,D,Eは基地局a,b,c,dの内側を
+//ロボットF,G,H,I,Jは基地局e,f,g,hの内側を走る
+/**********************************************/
+
+#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
+
+//************ID Number*****************
+//Robot   ID: 'A' ~ 'Z'
+//Base    ID: 'a' ~ 'a'
+//manager ID: '0'(数字のゼロ)
+//
+const char MyID = 'G';
+
+const int collect_base_address[4]={0,1,2,3};//1,2,3,4,5,6,7,8
+//************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_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;
+
+
+LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
+
+/////////////////////////////////////////
+//
+//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(collect_base_address),
+                                        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;
+    
+    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;
+            }
+        /*    fp = fopen(filename, "w");
+            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());
+                */
+            
+        } 
+            
+                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;
+    
+} 
+
+/////////////////////////////////////////
+//
+//New Mode
+//
+/////////////////////////////////////////
+
+void New_Mode(uint8_t *packetdata){
+  
+  //bool result;
+   agz.changeMode(packetdata); 
+  
+}
+  
+/////////////////////////////////////////
+//
+//Get Status
+//
+/////////////////////////////////////////
+void Get_Status(char SenderIDc,uint8_t *packetdata){
+    
+    //マネージャからデータが来たとき
+    if(SenderIDc == '0'){
+        printf("get manager Status\n");
+    }
+    //他のロボットからデータが来たとき
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+        printf("get other robots Status\n");
+    }
+    //基地局からデータが来たとき
+    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+        printf("Get Base data\n");
+        int id = SenderIDc - 'a';
+        agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
+        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)
+            );
+        }
+    }
+}
+
+void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){
+    
+    //マネージャからデータが来たとき
+    if(SenderIDc == '0'){
+        printf("get manager Status Kalman\n");
+    }
+    //他のロボットからデータが来たとき
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+        printf("get other robots Status Kalman\n");
+    }
+    //基地局からデータが来たとき
+    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));
+        }
+    }
+}
+
+/////////////////////////////////////////
+//
+//Send_Request_to_base
+//
+/////////////////////////////////////////
+void Send_Request_Base(int basenumber){
+    printf("send\n");
+    agz.createRequestCommand(MyID, basenumber);
+    //Select Destination
+    ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
+    //Send -> Base
+    xbee.send(tx64request);
+}
+
+/////////////////////////////////////////
+//
+//auto_Move
+//
+//InAreaかOutAreaの判定
+//Kalmanを通した値を出力(Baseと自分)
+//2015/05/29
+//外側判定と処理の実装
+//内側判定：シーケンス動作
+//外側判定：10秒間バック、3秒間旋回を行い、その後シーケンス動作へ
+/////////////////////////////////////////
+
+void auto_Move(){
+ 
+ bool result=false; // 毎回の内外判定の結果を格納
+ static bool out_flag = false; // 外側処理の実行フラグ
+ const int sequenceTime[4] = {30000, 31000, 34000, 34200};
+ const int outSequenceTime[4] = {10000, 11000, 14000, 14200};
+ 
+ 
+ //内外判定を行うtrueの時は外 falseの時は内側にいる
+ result = agz.gpsAuto(collect_base_address);
+ 
+ //agz.set_agzAutoGPS();
+ //agz.set_agzKalmanGPS();
+ 
+ if(out_flag == false && result == true){
+    out_flag = true;
+    agz.Out_Timer.reset();
+    }
+ 
+ if(out_flag == false){
+    if(agz.Move_Timer.read_ms() < sequenceTime[0]){
+          agz.control_Motor(0); //straight
+        }
+    if(agz.Move_Timer.read_ms() > sequenceTime[0] && agz.Move_Timer.read_ms() < sequenceTime[1]){
+          agz.control_Motor(1);
+        }
+    if(agz.Move_Timer.read_ms() > sequenceTime[1] && agz.Move_Timer.read_ms() < sequenceTime[2]){
+          agz.control_Motor(2); //Turn Right
+        }   
+    if(agz.Move_Timer.read_ms() > sequenceTime[2] && agz.Move_Timer.read_ms() < sequenceTime[3]){
+          agz.control_Motor(1);
+        }
+    if(agz.Move_Timer.read_ms() > sequenceTime[3]){
+          agz.Move_Timer.reset();
+        }
+    } 
+ if(out_flag == true){
+    if(agz.Out_Timer.read_ms() < outSequenceTime[0]){
+           agz.control_Motor(3); //back
+        }
+    if(agz.Out_Timer.read_ms() > outSequenceTime[0] && agz.Out_Timer.read_ms() < outSequenceTime[1]){
+           agz.control_Motor(1);
+        }
+    if(agz.Out_Timer.read_ms() > outSequenceTime[1] && agz.Out_Timer.read_ms() < outSequenceTime[2]){
+           agz.control_Motor(2); //Turn Right
+        }   
+    if(agz.Out_Timer.read_ms() > outSequenceTime[2] && agz.Out_Timer.read_ms() < outSequenceTime[3]){
+           agz.control_Motor(1);
+        }
+    if(agz.Out_Timer.read_ms() > outSequenceTime[3]){
+           agz.Out_Timer.reset();
+           agz.Move_Timer.reset();
+           out_flag = false;
+        }
+  }
+}
+
+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+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);
+    
+}
+
+/////////////////////////////////////////
+//
+//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;
+
+    
+    
+    
+    myGPS.begin(GPS_BAUD_RATE); 
+    
+    Timer collect_Timer;
+    //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
+    printf("start\n");
+    
+    char input_data=0;
+    int speed=64;
+    while (true) {
+        input_data=0;
+        scanf("%c",&input_data);
+        
+        switch(input_data){ 
+            case 'i'://stanby
+            printf("stanby\n");
+            agz.createChangeModeCommand('A','G',0);
+            ZBTxRequest tx64request1(robot_Address[6],agz.packetData,agz.getPacketLength());
+            xbee.send(tx64request1);
+            break;
+            case 'o'://manual
+            printf("manual\n");
+            agz.createChangeModeCommand('A','G',1);
+            ZBTxRequest tx64request2(robot_Address[6],agz.packetData,agz.getPacketLength());
+            xbee.send(tx64request2);
+            break; 
+            case 'w'://zennsinn
+            printf("sensin\n");
+            agz.createManualCommad('A','G',1,speed,1,speed);
+            ZBTxRequest tx64request3(robot_Address[6],agz.packetData,agz.getPacketLength());
+            xbee.send(tx64request3);
+            break;
+            case 'a'://hidari
+            printf("hidari\n");
+            agz.createManualCommad('A','G',2,speed,1,speed);
+            ZBTxRequest tx64request4(robot_Address[6],agz.packetData,agz.getPacketLength());
+            xbee.send(tx64request4);
+            break;
+            case 'd'://migi
+            printf("migi\n");
+            agz.createManualCommad('A','G',1,speed,2,speed);
+            ZBTxRequest tx64request5(robot_Address[6],agz.packetData,agz.getPacketLength());
+            xbee.send(tx64request5);
+            break;
+            case 's'://stop
+            printf("stop\n");
+            agz.createManualCommad('A','G',0,speed,0,speed);
+            ZBTxRequest tx64request6(robot_Address[6],agz.packetData,agz.getPacketLength());
+            xbee.send(tx64request6);
+            break;
+            case 'z'://back
+            printf("back\n");
+            agz.createManualCommad('A','G',2,speed,2,speed);
+            ZBTxRequest tx64request7(robot_Address[6],agz.packetData,agz.getPacketLength());
+            xbee.send(tx64request7);
+            break;
+            case '1':
+            printf("change\n");
+            speed=32;
+            break;
+            case '2':
+            printf("change\n");
+            speed = 64;
+            break;
+            case '3':
+            printf("change\n");
+            speed=128;
+            break;
+            default:
+            break;
+            }
+        }//endifisAvailable
+                       
+
+    }
+    
+