aigamozu
test
Dependencies: ADXL345 aigamozu_auto_ver3_1 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of
aigamozu_auto_ver3_1
by 
aigamozu
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /**********************************************/ 00002 // 00003 // 00004 // 00005 // Program name: Aigamozu Robot Control 00006 // Author: Atsunori Maruyama 00007 // Ver -> 1.3 00008 // Day -> 2014/06/09 00009 // 00010 // 00011 /**********************************************/ 00012 00013 #include "mbed.h" 00014 #include "XBee.h" 00015 #include "MBed_Adafruit_GPS.h" 00016 #include "AigamozuControlPackets.h" 00017 #include "agzIDLIST.h" 00018 #include "aigamozuSetting.h" 00019 #include "agz_common.h" 00020 #include "Kalman.h" 00021 00022 ///////////////////////////////////////// 00023 // 00024 //Connection Setting 00025 // 00026 ///////////////////////////////////////// 00027 00028 //Serial Connect Setting: PC <--> mbed 00029 Serial pc(USBTX, USBRX); 00030 00031 //Serial Connect Setting: GPS <--> mbed 00032 Serial * gps_Serial; 00033 00034 //Serial Connect Setting: XBEE <--> mbed 00035 XBee xbee(p13,p14); 00036 ZBRxResponse zbRx = ZBRxResponse(); 00037 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L); 00038 00039 AGZ_ROBOT robot[5]; 00040 00041 char Base_ID = 'A'; 00042 00043 ///////////////////////////////////////// 00044 // 00045 //Pin Setting 00046 // 00047 ///////////////////////////////////////// 00048 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); 00049 00050 00051 00052 ///////////////////////////////////////// 00053 // 00054 //Kalman Processing 00055 // 00056 ///////////////////////////////////////// 00057 00058 00059 double sigmaGPS[2][2] = {{250,0},{0,250}}; 00060 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}}; 00061 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}}; 00062 double y[2],x[2][2]={0}; 00063 00064 00065 void get_K(){ 00066 double temp[2][2]={ 00067 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]}, 00068 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]} 00069 }; 00070 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1]; 00071 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]); 00072 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]); 00073 } 00074 00075 00076 void get_x(){ 00077 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]); 00078 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]); 00079 } 00080 00081 00082 void get_sigma(){ 00083 double temp[2][2]; 00084 for(int i=0;i<2;i++) { 00085 for(int j=0;j<2;j++) { 00086 for(int k=0;k<2;k++) { 00087 temp[i][j]+=K[1][i][k]*sigma[0][k][j]; 00088 } 00089 } 00090 } 00091 for(int i = 0;i < 2;i++){ 00092 for(int j = 0;j < 2;j++){ 00093 sigma[1][i][j] = sigma[0][i][j]-temp[i][j]; 00094 } 00095 } 00096 } 00097 00098 void Kalman(double Latitude,double Longitude){ 00099 y[0] = Latitude; 00100 y[1] = Longitude; 00101 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS]; 00102 get_K(); 00103 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]); 00104 get_x(); 00105 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t]; 00106 get_sigma(); 00107 } 00108 00109 00110 ///////////////////////////////////////// 00111 // 00112 //Main Processing 00113 // 00114 ///////////////////////////////////////// 00115 int main() { 00116 //start up time 00117 wait(3); 00118 //set pc frequency to 57600bps 00119 pc.baud(PC_BAUD_RATE); 00120 //set xbee frequency to 57600bps 00121 xbee.begin(XBEE_BAUD_RATE); 00122 00123 //GPS setting 00124 gps_Serial = new Serial(p28,p27); 00125 Adafruit_GPS myGPS(gps_Serial); 00126 Timer refresh_Timer; 00127 const int refresh_Time = 2000; //refresh time in ms 00128 myGPS.begin(GPS_BAUD_RATE); 00129 00130 Timer collect_Timer; 00131 const int collect_Time = 200; //when we collect 4 GPS point, we use 00132 int collect_flag = 0; 00133 char collect_state = 'a'; 00134 XBeeAddress64 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 00135 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), XBeeAddress64(BASE5_32H,BASE5_32L)}; 00136 XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L); 00137 int id; 00138 bool flag = true; 00139 00140 //GPS Send Command 00141 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 00142 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); 00143 myGPS.sendCommand(PGCMD_ANTENNA); 00144 00145 wait(2); 00146 00147 //interrupt start 00148 AigamozuControlPackets agz(agz_motorShield); 00149 refresh_Timer.start(); 00150 00151 printf("test\n"); 00152 00153 00154 while (true) { 00155 00156 //Check Xbee Buffer Available 00157 xbee.readPacket(); 00158 if (xbee.getResponse().isAvailable()) { 00159 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { 00160 xbee.getResponse().getZBRxResponse(zbRx); 00161 uint8_t *buf = zbRx.getFrameData(); 00162 uint8_t *buf1 = &buf[11]; 00163 id = buf1[5] - 'A'; 00164 00165 00166 //Check Command Type 00167 switch(agz.checkCommnadType(buf)){ 00168 00169 //CommandType -> ChanegeMode 00170 case CHANGE_MODE :{ 00171 agz.changeMode(buf); 00172 break; 00173 } 00174 00175 //CommandType -> Manual 00176 case MANUAL:{ 00177 //Check now Mode 00178 if(agz.nowMode == MANUAL_MODE){ 00179 agz.changeSpeed(buf); 00180 } 00181 break; 00182 } 00183 00184 //CommandType -> Send Status 00185 case STATUS_REQUEST:{ 00186 //send normal data 00187 //Create GPS Infomation Packet 00188 agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00189 //Select Destination 00190 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength()); 00191 //Send -> Base 00192 xbee.send(tx64request); 00193 00194 //send Kalman data 00195 agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); 00196 //Select Destination 00197 ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength()); 00198 //Send -> Base 00199 xbee.send(tx64request); 00200 00201 //send normal data 00202 //Create GPS Infomation Packet 00203 agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00204 //Select Destination 00205 ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength()); 00206 //Send -> Base 00207 xbee.send(tx64request); 00208 00209 //send Kalman data 00210 agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); 00211 //Select Destination 00212 ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength()); 00213 //Send -> Base 00214 xbee.send(tx64request); 00215 break; 00216 00217 } 00218 00219 case RECEIVE_STATUS:{ 00220 00221 //printf("Receive Status\n"); 00222 00223 id = buf1[5] - 'A'; 00224 robot[id].set_state(buf1[9]); 00225 robot[id].set_LatitudeH(&buf1[13]); 00226 robot[id].set_LatitudeL(&buf1[17]); 00227 robot[id].set_LongitudeH(&buf1[21]); 00228 robot[id].set_LongitudeL(&buf1[25]); 00229 00230 agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL()); 00231 /* 00232 printf("%d,", buf1[5]); 00233 printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL()); 00234 */ 00235 break; 00236 } 00237 case RECEIVE_KALMAN:{ 00238 id = buf1[5] - 'A'; 00239 robot[id].set_state(buf1[9]); 00240 robot[id].set_LatitudeKH(&buf1[13]); 00241 robot[id].set_LatitudeKL(&buf1[17]); 00242 robot[id].set_LongitudeKH(&buf1[21]); 00243 robot[id].set_LongitudeKL(&buf1[25]); 00244 00245 agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL()); 00246 00247 break; 00248 } 00249 default:{ 00250 break; 00251 } 00252 } 00253 00254 00255 } 00256 } 00257 00258 00259 myGPS.read(); 00260 //recive gps module 00261 //check if we recieved a new message from GPS, if so, attempt to parse it, 00262 if ( myGPS.newNMEAreceived() ) { 00263 if ( !myGPS.parse(myGPS.lastNMEA()) ) { 00264 continue; 00265 } 00266 } 00267 if (refresh_Timer.read_ms() >= refresh_Time) { 00268 refresh_Timer.reset(); 00269 if (myGPS.fix) { 00270 agz.nowStatus = GPS_AVAIL; 00271 00272 if(flag){ 00273 if(myGPS.longitudeH==139 && myGPS.latitudeH==37){ 00274 flag = false; 00275 x[0][0]=(double)myGPS.longitudeL; 00276 x[0][1]=(double)myGPS.latitudeL; 00277 } 00278 } 00279 if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue; 00280 00281 //Kalman Filter 00282 Kalman(myGPS.longitudeL,myGPS.latitudeL); 00283 //kousinn 00284 for(int i = 0;i < 2;i++){ 00285 for(int j = 0;j < 2;j++){ 00286 K[0][i][j]=K[1][i][j]; 00287 x[0][i]=x[1][i]; 00288 sigma[0][i][j]=sigma[1][i][j]; 00289 } 00290 } 00291 00292 myGPS.longitudeKH=myGPS.longitudeH;//longitude after filtering 00293 myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering 00294 myGPS.longitudeKL=(long)x[1][0];//longitude after filtering 00295 myGPS.latitudeKL=(long)x[1][1];//latitude after filtering 00296 00297 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00298 agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); 00299 //printf("state = %d\n",agz.nowMode); 00300 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); 00301 } 00302 else agz.nowStatus = GPS_UNAVAIL; 00303 00304 } 00305 00306 00307 //get base GPS 00308 if( collect_Timer.read_ms() >= collect_Time){ 00309 collect_Timer.reset(); 00310 00311 //printf("Send Request:%d,%d\n",collect_flag,collect_state); 00312 00313 agz.createRequestCommand('A', collect_state); 00314 //Select Destination 00315 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength()); 00316 //Send -> Base 00317 xbee.send(tx64request); 00318 00319 collect_flag++; 00320 collect_state++; 00321 00322 if(collect_flag == 5){ 00323 collect_state = 'a'; 00324 collect_flag = 0; 00325 } 00326 } 00327 } 00328 }
Generated on Sun Jul 17 2022 19:49:10 by
1.7.2