aigamozu / Mbed 2 deprecated get_GPS_data_ver1

get GPS

Dependencies:   AigamozuControlPackets XBee agz_common mbed

main.cpp@0:25715989dbc2, 2015-04-08 (annotated)

Committer:
kityann
Date:
Wed Apr 08 13:16:59 2015 +0000
Revision:
0:25715989dbc2
get GPS;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:25715989dbc2 1
kityann 0:25715989dbc2 2 #include "mbed.h"
kityann 0:25715989dbc2 3 #include "XBee.h"
kityann 0:25715989dbc2 4
kityann 0:25715989dbc2 5 #include "agz_common.h"
kityann 0:25715989dbc2 6 #include "AigamozuControlPackets.h"
kityann 0:25715989dbc2 7
kityann 0:25715989dbc2 8
kityann 0:25715989dbc2 9 #define MY_BASE_NUMBER 0
kityann 0:25715989dbc2 10
kityann 0:25715989dbc2 11 XBee xbee(p13,p14);
kityann 0:25715989dbc2 12 Serial pc(USBTX, USBRX); // tx, rx
kityann 0:25715989dbc2 13
kityann 0:25715989dbc2 14 Ticker timer;
kityann 0:25715989dbc2 15
kityann 0:25715989dbc2 16 //robot data
kityann 0:25715989dbc2 17 AGZ_ROBOT robot[AGZ_NUM_ROBOTS];
kityann 0:25715989dbc2 18
kityann 0:25715989dbc2 19
kityann 0:25715989dbc2 20 double sigmaGPS[2][2] = {{250,0},{0,250}};
kityann 0:25715989dbc2 21 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
kityann 0:25715989dbc2 22 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
kityann 0:25715989dbc2 23 double y[2],x[2][2];
kityann 0:25715989dbc2 24
kityann 0:25715989dbc2 25
kityann 0:25715989dbc2 26 //function
kityann 0:25715989dbc2 27 //transform packetdata to robot data
kityann 0:25715989dbc2 28 void agz_transform_papcket(uint8_t* packet_data,int id){
kityann 0:25715989dbc2 29 //set latitude, longitude, state
kityann 0:25715989dbc2 30 robot[id].set_state(packet_data[9]);
kityann 0:25715989dbc2 31 robot[id].set_LatitudeH(&packet_data[13]);
kityann 0:25715989dbc2 32 robot[id].set_LatitudeL(&packet_data[17]);
kityann 0:25715989dbc2 33 robot[id].set_LongitudeH(&packet_data[21]);
kityann 0:25715989dbc2 34 robot[id].set_LongitudeL(&packet_data[25]);
kityann 0:25715989dbc2 35 }
kityann 0:25715989dbc2 36
kityann 0:25715989dbc2 37 /*---------registere robot--------------*/
kityann 0:25715989dbc2 38 /*
kityann 0:25715989dbc2 39 void agz_register_robot(uint8_*t packet_data){
kityann 0:25715989dbc2 40 uint8_t robot_id = packet_data[5];
kityann 0:25715989dbc2 41 robot[robot_id].set_state(0);
kityann 0:25715989dbc2 42 send_register_ack();
kityann 0:25715989dbc2 43 }
kityann 0:25715989dbc2 44 */
kityann 0:25715989dbc2 45
kityann 0:25715989dbc2 46 /*---------timer set----*/
kityann 0:25715989dbc2 47
kityann 0:25715989dbc2 48 void agz_send_request(){
kityann 0:25715989dbc2 49 static uint8_t next_send_address = 0,i;
kityann 0:25715989dbc2 50 AigamozuControlPackets agz_packet;
kityann 0:25715989dbc2 51
kityann 0:25715989dbc2 52 /*------search next moving robot--------*/
kityann 0:25715989dbc2 53
kityann 0:25715989dbc2 54 for(i = next_send_address;i < AGZ_NUM_ROBOTS;i++){
kityann 0:25715989dbc2 55 if(robot[i].get_state() != 0xFF){
kityann 0:25715989dbc2 56 next_send_address = i;
kityann 0:25715989dbc2 57 break;
kityann 0:25715989dbc2 58 }
kityann 0:25715989dbc2 59 }
kityann 0:25715989dbc2 60 if(i == AGZ_NUM_ROBOTS){
kityann 0:25715989dbc2 61 for(i = 0;i < AGZ_NUM_ROBOTS;i++){
kityann 0:25715989dbc2 62 if(robot[i].get_state() != 0xFF){
kityann 0:25715989dbc2 63 next_send_address = i;
kityann 0:25715989dbc2 64 break;
kityann 0:25715989dbc2 65 }
kityann 0:25715989dbc2 66 }
kityann 0:25715989dbc2 67 }
kityann 0:25715989dbc2 68
kityann 0:25715989dbc2 69 agz_packet.createRequestCommand(MY_BASE_NUMBER,next_send_address);
kityann 0:25715989dbc2 70
kityann 0:25715989dbc2 71 ZBTxRequest agz_request_packet = ZBTxRequest(Agz_XBee_Remote_Address[next_send_address], agz_packet.packetData, REQUEST_COMMNAD_LENGTH );
kityann 0:25715989dbc2 72
kityann 0:25715989dbc2 73 xbee.send(agz_request_packet);
kityann 0:25715989dbc2 74
kityann 0:25715989dbc2 75 next_send_address++;
kityann 0:25715989dbc2 76 }
kityann 0:25715989dbc2 77
kityann 0:25715989dbc2 78 void get_K(){
kityann 0:25715989dbc2 79 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]);
kityann 0:25715989dbc2 80 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(sigma[1][1][1]+sigmaGPS[1][1]);
kityann 0:25715989dbc2 81 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(sigma[1][0][0]+sigmaGPS[0][0]);
kityann 0:25715989dbc2 82 }
kityann 0:25715989dbc2 83
kityann 0:25715989dbc2 84 void get_x(){
kityann 0:25715989dbc2 85 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[1][0]);
kityann 0:25715989dbc2 86 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[1][1]);
kityann 0:25715989dbc2 87 }
kityann 0:25715989dbc2 88
kityann 0:25715989dbc2 89 void get_sigma(){
kityann 0:25715989dbc2 90 double temp[2][2];
kityann 0:25715989dbc2 91 for(int i=0;i<2;i++) {
kityann 0:25715989dbc2 92 for(int j=0;j<2;j++) {
kityann 0:25715989dbc2 93 for(int k=0;k<2;k++) {
kityann 0:25715989dbc2 94 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 0:25715989dbc2 95 }
kityann 0:25715989dbc2 96 }
kityann 0:25715989dbc2 97 }
kityann 0:25715989dbc2 98 for(int i = 0;i < 2;i++){
kityann 0:25715989dbc2 99 for(int j = 0;j < 2;j++){
kityann 0:25715989dbc2 100 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 0:25715989dbc2 101 }
kityann 0:25715989dbc2 102 }
kityann 0:25715989dbc2 103 }
kityann 0:25715989dbc2 104
kityann 0:25715989dbc2 105 void Kalman(double Latitude,double Longitude){
kityann 0:25715989dbc2 106 y[0] = Latitude;
kityann 0:25715989dbc2 107 y[1] = Longitude;
kityann 0:25715989dbc2 108 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 0:25715989dbc2 109 get_K();
kityann 0:25715989dbc2 110 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 0:25715989dbc2 111 get_x();
kityann 0:25715989dbc2 112 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 0:25715989dbc2 113 get_sigma();
kityann 0:25715989dbc2 114 }
kityann 0:25715989dbc2 115
kityann 0:25715989dbc2 116
kityann 0:25715989dbc2 117 /*---------init---------*/
kityann 0:25715989dbc2 118
kityann 0:25715989dbc2 119 void agz_base_init(){
kityann 0:25715989dbc2 120 xbee.begin(57600);
kityann 0:25715989dbc2 121 pc.baud(57600);
kityann 0:25715989dbc2 122 timer.attach(&agz_send_request,1.0);
kityann 0:25715989dbc2 123
kityann 0:25715989dbc2 124 }
kityann 0:25715989dbc2 125
kityann 0:25715989dbc2 126 void Kalman_init(){
kityann 0:25715989dbc2 127 }
kityann 0:25715989dbc2 128
kityann 0:25715989dbc2 129
kityann 0:25715989dbc2 130 int main(void){
kityann 0:25715989dbc2 131 int sender,reciever;
kityann 0:25715989dbc2 132
kityann 0:25715989dbc2 133
kityann 0:25715989dbc2 134 ZBRxResponse xbeeresponse;
kityann 0:25715989dbc2 135
kityann 0:25715989dbc2 136 agz_base_init();
kityann 0:25715989dbc2 137
kityann 0:25715989dbc2 138 wait(1);
kityann 0:25715989dbc2 139
kityann 0:25715989dbc2 140 printf("start\n");
kityann 0:25715989dbc2 141
kityann 0:25715989dbc2 142
kityann 0:25715989dbc2 143 robot[0].set_state(0);
kityann 0:25715989dbc2 144 //robot[1].set_state(0);
kityann 0:25715989dbc2 145 //robot[2].set_state(0);
kityann 0:25715989dbc2 146 //robot[3].set_state(0);
kityann 0:25715989dbc2 147 //robot[4].set_state(0);
kityann 0:25715989dbc2 148 //robot[5].set_state(0);
kityann 0:25715989dbc2 149
kityann 0:25715989dbc2 150 int count=0;
kityann 0:25715989dbc2 151
kityann 0:25715989dbc2 152
kityann 0:25715989dbc2 153 while(1){
kityann 0:25715989dbc2 154 xbee.readPacket();
kityann 0:25715989dbc2 155
kityann 0:25715989dbc2 156
kityann 0:25715989dbc2 157
kityann 0:25715989dbc2 158 if(xbee.getResponse().isAvailable() ){
kityann 0:25715989dbc2 159 xbee.getResponse(xbeeresponse);
kityann 0:25715989dbc2 160
kityann 0:25715989dbc2 161 //get data packet
kityann 0:25715989dbc2 162 if(xbee.getResponse().getApiId() == ZB_RX_RESPONSE){
kityann 0:25715989dbc2 163 uint8_t *packet_data = xbeeresponse.getData();
kityann 0:25715989dbc2 164 uint8_t packet_kinds = packet_data[3];
kityann 0:25715989dbc2 165
kityann 0:25715989dbc2 166 //get sensor data
kityann 0:25715989dbc2 167 if(packet_kinds == 'R'){
kityann 0:25715989dbc2 168 sender = packet_data[5]-1;// - 'A';
kityann 0:25715989dbc2 169 reciever = packet_data[7]-1;// - 'a';
kityann 0:25715989dbc2 170 agz_transform_papcket(packet_data,sender);
kityann 0:25715989dbc2 171
kityann 0:25715989dbc2 172
kityann 0:25715989dbc2 173 if(count==0){
kityann 0:25715989dbc2 174 count++;
kityann 0:25715989dbc2 175 x[1][0]=robot[sender].get_LatitudeL();
kityann 0:25715989dbc2 176 x[1][1]=robot[sender].get_LongitudeL();
kityann 0:25715989dbc2 177 }else if(count == 100){
kityann 0:25715989dbc2 178 printf("/////////////////////////Count = 100///////////////////////\n");
kityann 0:25715989dbc2 179 printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
kityann 0:25715989dbc2 180 robot[sender].get_LatitudeH(),x[1][0],
kityann 0:25715989dbc2 181 robot[sender].get_LongitudeH(),x[1][1]);
kityann 0:25715989dbc2 182 for(int i = 0;i < 2;i++){
kityann 0:25715989dbc2 183 for(int j = 0;j < 2;j++){
kityann 0:25715989dbc2 184 printf("%lf ",sigma[0][i][j]);
kityann 0:25715989dbc2 185 }
kityann 0:25715989dbc2 186 printf("\n");
kityann 0:25715989dbc2 187 }
kityann 0:25715989dbc2 188 count = 0;
kityann 0:25715989dbc2 189 }else{
kityann 0:25715989dbc2 190 count++;
kityann 0:25715989dbc2 191 Kalman(robot[sender].get_LatitudeL(),robot[sender].get_LongitudeL());
kityann 0:25715989dbc2 192
kityann 0:25715989dbc2 193
kityann 0:25715989dbc2 194 printf("%d,%d,%d,%ld.%ld,%ld.%ld\n",robot[sender].get_state(),sender,reciever,
kityann 0:25715989dbc2 195 robot[sender].get_LatitudeH(),robot[sender].get_LatitudeL(),
kityann 0:25715989dbc2 196 robot[sender].get_LongitudeH(),robot[sender].get_LongitudeL());
kityann 0:25715989dbc2 197
kityann 0:25715989dbc2 198
kityann 0:25715989dbc2 199 printf("state=%d,sender=%d,reciever=%d,latH=%ld.latL=%lf,longH=%ld.longL=%lf\n",robot[sender].get_state(),sender,reciever,
kityann 0:25715989dbc2 200 robot[sender].get_LatitudeH(),x[1][0],
kityann 0:25715989dbc2 201 robot[sender].get_LongitudeH(),x[1][1]);
kityann 0:25715989dbc2 202 //kousinn
kityann 0:25715989dbc2 203 for(int i = 0;i < 2;i++){
kityann 0:25715989dbc2 204 for(int j = 0;j < 2;j++){
kityann 0:25715989dbc2 205 K[0][i][j]=K[1][i][j];
kityann 0:25715989dbc2 206 x[0][i]=x[1][i];
kityann 0:25715989dbc2 207 sigma[0][i][j]=sigma[1][i][j];
kityann 0:25715989dbc2 208 }
kityann 0:25715989dbc2 209 }
kityann 0:25715989dbc2 210 }
kityann 0:25715989dbc2 211
kityann 0:25715989dbc2 212 }
kityann 0:25715989dbc2 213 //get register packet
kityann 0:25715989dbc2 214 if(packet_kinds == 'E'){
kityann 0:25715989dbc2 215 //agz_register_robot(packet_data);
kityann 0:25715989dbc2 216 }
kityann 0:25715989dbc2 217 }else{
kityann 0:25715989dbc2 218 //get not rx response
kityann 0:25715989dbc2 219 }
kityann 0:25715989dbc2 220 }else{
kityann 0:25715989dbc2 221
kityann 0:25715989dbc2 222 }
kityann 0:25715989dbc2 223 }
kityann 0:25715989dbc2 224 }