main.cpp

00001 
00002 #include "mbed.h"
00003 #include "XBee.h"
00004 
00005 #include "agz_common.h"
00006 #include "AigamozuControlPackets.h"
00007 
00008 
00009 #define MY_BASE_NUMBER 0
00010 
00011 XBee xbee(p13,p14);
00012 Serial pc(USBTX, USBRX); // tx, rx
00013 
00014 Ticker timer;
00015 
00016 //robot data
00017 AGZ_ROBOT robot[AGZ_NUM_ROBOTS];
00018 
00019 
00020     double sigmaGPS[2][2] = {{250,0},{0,250}};
00021     double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
00022     double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
00023     double y[2],x[2][2];
00024 
00025 
00026 //function
00027 //transform packetdata to robot data
00028 void agz_transform_papcket(uint8_t* packet_data,int id){
00029     //set latitude, longitude, state
00030     robot[id].set_state(packet_data[9]);
00031     robot[id].set_LatitudeH(&packet_data[13]);
00032     robot[id].set_LatitudeL(&packet_data[17]);
00033     robot[id].set_LongitudeH(&packet_data[21]);
00034     robot[id].set_LongitudeL(&packet_data[25]);    
00035 }
00036 
00037 /*---------registere robot--------------*/
00038 /*
00039 void agz_register_robot(uint8_*t packet_data){
00040     uint8_t robot_id = packet_data[5];
00041     robot[robot_id].set_state(0);
00042     send_register_ack();
00043 }
00044 */
00045 
00046 /*---------timer set----*/
00047 
00048 void agz_send_request(){
00049     static uint8_t next_send_address = 0,i;
00050     AigamozuControlPackets agz_packet;
00051     
00052     /*------search next moving robot--------*/
00053     
00054     for(i = next_send_address;i < AGZ_NUM_ROBOTS;i++){
00055         if(robot[i].get_state() != 0xFF){
00056             next_send_address = i;
00057             break;
00058         }
00059     }
00060     if(i == AGZ_NUM_ROBOTS){
00061         for(i = 0;i < AGZ_NUM_ROBOTS;i++){
00062             if(robot[i].get_state() != 0xFF){
00063                 next_send_address = i;
00064                 break;
00065             }
00066         }
00067     }
00068     
00069     agz_packet.createRequestCommand(MY_BASE_NUMBER,next_send_address);
00070     
00071     ZBTxRequest agz_request_packet = ZBTxRequest(Agz_XBee_Remote_Address[next_send_address], agz_packet.packetData, REQUEST_COMMNAD_LENGTH );   
00072     
00073     xbee.send(agz_request_packet);
00074     
00075     next_send_address++;
00076 }
00077 
00078 void get_K(){
00079     double ad_bc = (sigma[0][0][0]+sigmaGPS[0][0])*(sigma[0][1][1]+sigmaGPS[1][1])-(sigma[0][1][0]+sigmaGPS[1][0])*(sigma[0][0][1]+sigmaGPS[0][1]);
00080     K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(sigma[1][1][1]+sigmaGPS[1][1]);
00081     K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(sigma[1][0][0]+sigmaGPS[0][0]);
00082 }
00083 
00084 void get_x(){
00085     x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[1][0]);
00086     x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[1][1]);
00087 }
00088 
00089 void get_sigma(){
00090     double temp[2][2];
00091     for(int i=0;i<2;i++) {
00092         for(int j=0;j<2;j++) {
00093             for(int k=0;k<2;k++) {
00094                 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
00095             }
00096         }
00097    }
00098     for(int i = 0;i < 2;i++){
00099         for(int j = 0;j < 2;j++){
00100             sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
00101         }
00102     }   
00103 }
00104 
00105 void Kalman(double Latitude,double Longitude){
00106     y[0] = Latitude;
00107     y[1] = Longitude;
00108     //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
00109     get_K();
00110     //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
00111     get_x();
00112     //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
00113     get_sigma();
00114 }
00115 
00116 
00117 /*---------init---------*/
00118 
00119 void agz_base_init(){
00120     xbee.begin(57600);
00121     pc.baud(57600);
00122     timer.attach(&agz_send_request,1.0);
00123     
00124 }
00125 
00126 void Kalman_init(){
00127 }
00128 
00129 
00130 int main(void){
00131     int sender,reciever;
00132 
00133     
00134     ZBRxResponse xbeeresponse;
00135     
00136     agz_base_init();
00137     
00138     wait(1);
00139     
00140     printf("start\n");
00141     
00142     
00143     robot[0].set_state(0);
00144     //robot[1].set_state(0);
00145     //robot[2].set_state(0);
00146     //robot[3].set_state(0);
00147     //robot[4].set_state(0);
00148     //robot[5].set_state(0);
00149     
00150     int count=0;
00151         
00152         
00153     while(1){
00154         xbee.readPacket();
00155 
00156 
00157 
00158         if(xbee.getResponse().isAvailable() ){ 
00159             xbee.getResponse(xbeeresponse);
00160             
00161             //get data packet
00162             if(xbee.getResponse().getApiId() == ZB_RX_RESPONSE){
00163                 uint8_t *packet_data = xbeeresponse.getData();
00164                 uint8_t packet_kinds = packet_data[3];
00165                 
00166                 //get sensor data
00167                 if(packet_kinds == 'R'){
00168                     sender = packet_data[5]-1;// - 'A';
00169                     reciever = packet_data[7]-1;// - 'a';
00170                     agz_transform_papcket(packet_data,sender);
00171                     
00172                     
00173                     if(count==0){
00174                         count++;
00175                         x[1][0]=robot[sender].get_LatitudeL();                        
00176                         x[1][1]=robot[sender].get_LongitudeL();
00177                     }else if(count == 100){
00178                         printf("/////////////////////////Count = 100///////////////////////\n");
00179                         printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
00180                         robot[sender].get_LatitudeH(),x[1][0],
00181                         robot[sender].get_LongitudeH(),x[1][1]);
00182                         for(int i = 0;i < 2;i++){
00183                             for(int j = 0;j < 2;j++){
00184                                 printf("%lf ",sigma[0][i][j]);
00185                             }
00186                             printf("\n");
00187                         }
00188                         count = 0;          
00189                     }else{
00190                     count++;
00191                     Kalman(robot[sender].get_LatitudeL(),robot[sender].get_LongitudeL());
00192                     
00193                     
00194                     printf("%d,%d,%d,%ld.%ld,%ld.%ld\n",robot[sender].get_state(),sender,reciever,
00195                     robot[sender].get_LatitudeH(),robot[sender].get_LatitudeL(),
00196                     robot[sender].get_LongitudeH(),robot[sender].get_LongitudeL());
00197                     
00198                     
00199                     printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
00200                     robot[sender].get_LatitudeH(),x[1][0],
00201                     robot[sender].get_LongitudeH(),x[1][1]);
00202                     //kousinn
00203                     for(int i = 0;i < 2;i++){
00204                         for(int j = 0;j < 2;j++){
00205                             K[0][i][j]=K[1][i][j];
00206                             x[0][i]=x[1][i];
00207                             sigma[0][i][j]=sigma[1][i][j];
00208                         }
00209                     }
00210                     }
00211                     
00212                 }
00213                 //get register packet
00214                 if(packet_kinds == 'E'){
00215                     //agz_register_robot(packet_data);
00216                 }
00217             }else{
00218                 //get not rx response
00219             }
00220         }else{
00221           
00222         }
00223     }
00224 }