aigamozu / Mbed 2 deprecated aigamozu_auto_for_robot_ver1_yokokawa

2015/05/11

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed

Fork of aigamozu_auto_for_robot_ver1 by aigamozu

main.cpp@2:2afe6fd6e565, 2015-04-27 (annotated)

Committer:
s1200058
Date:
Mon Apr 27 13:25:52 2015 +0000
Revision:
2:2afe6fd6e565
Parent:
1:004530c56f4b
Child:
3:a66f4fa1f8a8
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:567aa3346701 1 /**********************************************/
kityann 0:567aa3346701 2 //
kityann 0:567aa3346701 3 //
kityann 0:567aa3346701 4 //
kityann 0:567aa3346701 5 // Program name: Aigamozu Robot Control
kityann 0:567aa3346701 6 // Author: Atsunori Maruyama
kityann 0:567aa3346701 7 // Ver -> 1.3
kityann 0:567aa3346701 8 // Day -> 2014/06/09
kityann 0:567aa3346701 9 //
kityann 0:567aa3346701 10 //
kityann 0:567aa3346701 11 /**********************************************/
kityann 0:567aa3346701 12
kityann 0:567aa3346701 13 #include "mbed.h"
kityann 0:567aa3346701 14 #include "XBee.h"
kityann 0:567aa3346701 15 #include "MBed_Adafruit_GPS.h"
kityann 0:567aa3346701 16 #include "AigamozuControlPackets.h"
kityann 0:567aa3346701 17 #include "agzIDLIST.h"
kityann 0:567aa3346701 18 #include "aigamozuSetting.h"
kityann 0:567aa3346701 19 #include "agz_common.h"
kityann 0:567aa3346701 20 #include "Kalman.h"
kityann 0:567aa3346701 21
kityann 0:567aa3346701 22 /////////////////////////////////////////
kityann 0:567aa3346701 23 //
kityann 0:567aa3346701 24 //Connection Setting
kityann 0:567aa3346701 25 //
kityann 0:567aa3346701 26 /////////////////////////////////////////
kityann 0:567aa3346701 27
kityann 0:567aa3346701 28 //Serial Connect Setting: PC <--> mbed
kityann 0:567aa3346701 29 Serial pc(USBTX, USBRX);
kityann 0:567aa3346701 30
kityann 0:567aa3346701 31 //Serial Connect Setting: GPS <--> mbed
kityann 0:567aa3346701 32 Serial * gps_Serial;
kityann 0:567aa3346701 33
kityann 0:567aa3346701 34 //Serial Connect Setting: XBEE <--> mbed
kityann 0:567aa3346701 35 XBee xbee(p13,p14);
kityann 0:567aa3346701 36 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:567aa3346701 37 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
kityann 0:567aa3346701 38
kityann 0:567aa3346701 39 AGZ_ROBOT robot[5];
kityann 0:567aa3346701 40
kityann 0:567aa3346701 41 char MyID = 'Z';
kityann 0:567aa3346701 42
kityann 0:567aa3346701 43 /////////////////////////////////////////
kityann 0:567aa3346701 44 //
kityann 0:567aa3346701 45 //Pin Setting
kityann 0:567aa3346701 46 //
kityann 0:567aa3346701 47 /////////////////////////////////////////
kityann 0:567aa3346701 48 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:567aa3346701 49
kityann 0:567aa3346701 50
kityann 0:567aa3346701 51
kityann 0:567aa3346701 52 /////////////////////////////////////////
kityann 0:567aa3346701 53 //
kityann 0:567aa3346701 54 //Kalman Processing
kityann 0:567aa3346701 55 //
kityann 0:567aa3346701 56 /////////////////////////////////////////
kityann 0:567aa3346701 57
kityann 0:567aa3346701 58
kityann 0:567aa3346701 59 double sigmaGPS[2][2] = {{250,0},{0,250}};
kityann 0:567aa3346701 60 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
kityann 0:567aa3346701 61 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
kityann 0:567aa3346701 62 double y[2],x[2][2]={0};
kityann 0:567aa3346701 63
kityann 0:567aa3346701 64
kityann 0:567aa3346701 65 void get_K(){
kityann 0:567aa3346701 66 double temp[2][2]={
kityann 0:567aa3346701 67 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
kityann 0:567aa3346701 68 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
kityann 0:567aa3346701 69 };
kityann 0:567aa3346701 70 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
kityann 0:567aa3346701 71 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
kityann 0:567aa3346701 72 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
kityann 0:567aa3346701 73 }
kityann 0:567aa3346701 74
kityann 0:567aa3346701 75
kityann 0:567aa3346701 76 void get_x(){
kityann 0:567aa3346701 77 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
kityann 0:567aa3346701 78 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
kityann 0:567aa3346701 79 }
kityann 0:567aa3346701 80
kityann 0:567aa3346701 81
kityann 0:567aa3346701 82 void get_sigma(){
kityann 0:567aa3346701 83 double temp[2][2];
kityann 0:567aa3346701 84 for(int i=0;i<2;i++) {
kityann 0:567aa3346701 85 for(int j=0;j<2;j++) {
kityann 0:567aa3346701 86 for(int k=0;k<2;k++) {
kityann 0:567aa3346701 87 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 0:567aa3346701 88 }
kityann 0:567aa3346701 89 }
kityann 0:567aa3346701 90 }
kityann 0:567aa3346701 91 for(int i = 0;i < 2;i++){
kityann 0:567aa3346701 92 for(int j = 0;j < 2;j++){
kityann 0:567aa3346701 93 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 0:567aa3346701 94 }
kityann 0:567aa3346701 95 }
kityann 0:567aa3346701 96 }
kityann 0:567aa3346701 97
kityann 0:567aa3346701 98 void Kalman(double Latitude,double Longitude){
kityann 0:567aa3346701 99 y[0] = Latitude;
kityann 0:567aa3346701 100 y[1] = Longitude;
kityann 0:567aa3346701 101 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 0:567aa3346701 102 get_K();
kityann 0:567aa3346701 103 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 0:567aa3346701 104 get_x();
kityann 0:567aa3346701 105 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 0:567aa3346701 106 get_sigma();
kityann 0:567aa3346701 107 }
kityann 0:567aa3346701 108
kityann 0:567aa3346701 109
kityann 0:567aa3346701 110 /////////////////////////////////////////
kityann 0:567aa3346701 111 //
kityann 0:567aa3346701 112 //Main Processing
kityann 0:567aa3346701 113 //
kityann 0:567aa3346701 114 /////////////////////////////////////////
kityann 0:567aa3346701 115 int main() {
kityann 0:567aa3346701 116 //start up time
kityann 0:567aa3346701 117 wait(3);
kityann 0:567aa3346701 118 //set pc frequency to 57600bps
kityann 0:567aa3346701 119 pc.baud(PC_BAUD_RATE);
kityann 0:567aa3346701 120 //set xbee frequency to 57600bps
kityann 0:567aa3346701 121 xbee.begin(XBEE_BAUD_RATE);
kityann 0:567aa3346701 122
kityann 0:567aa3346701 123 //GPS setting
kityann 0:567aa3346701 124 gps_Serial = new Serial(p28,p27);
kityann 0:567aa3346701 125 Adafruit_GPS myGPS(gps_Serial);
kityann 0:567aa3346701 126 Timer refresh_Timer;
kityann 0:567aa3346701 127 const int refresh_Time = 2000; //refresh time in ms
kityann 0:567aa3346701 128 myGPS.begin(GPS_BAUD_RATE);
kityann 0:567aa3346701 129
kityann 0:567aa3346701 130 Timer collect_Timer;
kityann 0:567aa3346701 131 const int collect_Time = 200; //when we collect 4 GPS point, we use
kityann 0:567aa3346701 132 int collect_flag = 0;
kityann 0:567aa3346701 133 char collect_state = 'a';
s1200058 2:2afe6fd6e565 134 XBeeAddress64 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
s1200058 1:004530c56f4b 135 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L),
s1200058 1:004530c56f4b 136 XBeeAddress64(BASE5_32H,BASE5_32L)};
kityann 0:567aa3346701 137 XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
kityann 0:567aa3346701 138 int id,SenderIDc;
kityann 0:567aa3346701 139 bool flag = true;
kityann 0:567aa3346701 140
kityann 0:567aa3346701 141 //GPS Send Command
kityann 0:567aa3346701 142 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:567aa3346701 143 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:567aa3346701 144 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:567aa3346701 145
kityann 0:567aa3346701 146 wait(2);
kityann 0:567aa3346701 147
kityann 0:567aa3346701 148 //interrupt start
kityann 0:567aa3346701 149 AigamozuControlPackets agz(agz_motorShield);
kityann 0:567aa3346701 150 refresh_Timer.start();
kityann 0:567aa3346701 151
kityann 0:567aa3346701 152 printf("start\n");
kityann 0:567aa3346701 153
kityann 0:567aa3346701 154
kityann 0:567aa3346701 155 while (true) {
kityann 0:567aa3346701 156
kityann 0:567aa3346701 157 //Check Xbee Buffer Available
kityann 0:567aa3346701 158 xbee.readPacket();
kityann 0:567aa3346701 159 if (xbee.getResponse().isAvailable()) {
kityann 0:567aa3346701 160 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:567aa3346701 161 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:567aa3346701 162 uint8_t *buf = zbRx.getFrameData();
kityann 0:567aa3346701 163 uint8_t *buf1 = &buf[11];
kityann 0:567aa3346701 164 id = buf1[5] - 'A';
kityann 0:567aa3346701 165 SenderIDc = buf1[5];
kityann 0:567aa3346701 166
kityann 0:567aa3346701 167
kityann 0:567aa3346701 168 //Check Command Type
kityann 0:567aa3346701 169 switch(agz.checkCommnadType(buf)){
kityann 0:567aa3346701 170
kityann 0:567aa3346701 171 //CommandType -> ChanegeMode
kityann 0:567aa3346701 172 case CHANGE_MODE :{
kityann 0:567aa3346701 173 agz.changeMode(buf);
kityann 0:567aa3346701 174 break;
kityann 0:567aa3346701 175 }
kityann 0:567aa3346701 176
kityann 0:567aa3346701 177 //CommandType -> Manual
kityann 0:567aa3346701 178 case MANUAL:{
kityann 0:567aa3346701 179 //Check now Mode
kityann 0:567aa3346701 180 if(agz.nowMode == MANUAL_MODE){
kityann 0:567aa3346701 181 agz.changeSpeed(buf);
kityann 0:567aa3346701 182 }
kityann 0:567aa3346701 183 break;
kityann 0:567aa3346701 184 }
kityann 0:567aa3346701 185
kityann 0:567aa3346701 186 //CommandType -> Send Status
kityann 0:567aa3346701 187 case STATUS_REQUEST:{
kityann 0:567aa3346701 188 if(SenderIDc == 'Z'){
kityann 0:567aa3346701 189 //send normal data
kityann 0:567aa3346701 190 //Create GPS Infomation Packet
kityann 0:567aa3346701 191 agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:567aa3346701 192 //Select Destination
kityann 0:567aa3346701 193 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
kityann 0:567aa3346701 194 //Send -> Base
kityann 0:567aa3346701 195 xbee.send(tx64request);
kityann 0:567aa3346701 196
kityann 0:567aa3346701 197 //send Kalman data
kityann 0:567aa3346701 198 agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
kityann 0:567aa3346701 199 //Select Destination
kityann 0:567aa3346701 200 ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
kityann 0:567aa3346701 201 //Send -> Base
kityann 0:567aa3346701 202 xbee.send(tx64request1);
kityann 0:567aa3346701 203
kityann 0:567aa3346701 204 }else{
kityann 0:567aa3346701 205
s1200058 1:004530c56f4b 206 printf("%d: catch request\n", id);
s1200058 1:004530c56f4b 207
kityann 0:567aa3346701 208 //send normal data
kityann 0:567aa3346701 209 //Create GPS Infomation Packet
kityann 0:567aa3346701 210 agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:567aa3346701 211 //Select Destination
kityann 0:567aa3346701 212 ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength());
kityann 0:567aa3346701 213 //Send -> Base
kityann 0:567aa3346701 214 xbee.send(tx64request2);
kityann 0:567aa3346701 215
kityann 0:567aa3346701 216 //send Kalman data
kityann 0:567aa3346701 217 agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
kityann 0:567aa3346701 218 //Select Destination
kityann 0:567aa3346701 219 ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
kityann 0:567aa3346701 220 //Send -> Base
kityann 0:567aa3346701 221 xbee.send(tx64request3);
kityann 0:567aa3346701 222 }
kityann 0:567aa3346701 223 break;
kityann 0:567aa3346701 224
kityann 0:567aa3346701 225 }
kityann 0:567aa3346701 226
kityann 0:567aa3346701 227 case RECEIVE_STATUS:{
kityann 0:567aa3346701 228
kityann 0:567aa3346701 229 //printf("Receive Status\n");
s1200058 1:004530c56f4b 230
kityann 0:567aa3346701 231 id = buf1[5] - 'A';
kityann 0:567aa3346701 232 robot[id].set_state(buf1[9]);
kityann 0:567aa3346701 233 robot[id].set_LatitudeH(&buf1[13]);
kityann 0:567aa3346701 234 robot[id].set_LatitudeL(&buf1[17]);
kityann 0:567aa3346701 235 robot[id].set_LongitudeH(&buf1[21]);
kityann 0:567aa3346701 236 robot[id].set_LongitudeL(&buf1[25]);
kityann 0:567aa3346701 237
kityann 0:567aa3346701 238 agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
kityann 0:567aa3346701 239 /*
kityann 0:567aa3346701 240 printf("%d,", buf1[5]);
kityann 0:567aa3346701 241 printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
kityann 0:567aa3346701 242 */
kityann 0:567aa3346701 243 break;
kityann 0:567aa3346701 244 }
kityann 0:567aa3346701 245 case RECEIVE_KALMAN:{
kityann 0:567aa3346701 246 id = buf1[5] - 'A';
kityann 0:567aa3346701 247 robot[id].set_state(buf1[9]);
kityann 0:567aa3346701 248 robot[id].set_LatitudeKH(&buf1[13]);
kityann 0:567aa3346701 249 robot[id].set_LatitudeKL(&buf1[17]);
kityann 0:567aa3346701 250 robot[id].set_LongitudeKH(&buf1[21]);
kityann 0:567aa3346701 251 robot[id].set_LongitudeKL(&buf1[25]);
kityann 0:567aa3346701 252
kityann 0:567aa3346701 253 agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL());
kityann 0:567aa3346701 254
kityann 0:567aa3346701 255 break;
kityann 0:567aa3346701 256 }
kityann 0:567aa3346701 257 default:{
kityann 0:567aa3346701 258 break;
kityann 0:567aa3346701 259 }
kityann 0:567aa3346701 260 }
kityann 0:567aa3346701 261
kityann 0:567aa3346701 262
kityann 0:567aa3346701 263 }
kityann 0:567aa3346701 264 }
kityann 0:567aa3346701 265
kityann 0:567aa3346701 266
kityann 0:567aa3346701 267 myGPS.read();
kityann 0:567aa3346701 268 //recive gps module
kityann 0:567aa3346701 269 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:567aa3346701 270 if ( myGPS.newNMEAreceived() ) {
kityann 0:567aa3346701 271 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:567aa3346701 272 continue;
kityann 0:567aa3346701 273 }
kityann 0:567aa3346701 274 }
kityann 0:567aa3346701 275 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:567aa3346701 276 refresh_Timer.reset();
kityann 0:567aa3346701 277 if (myGPS.fix) {
kityann 0:567aa3346701 278 agz.nowStatus = GPS_AVAIL;
kityann 0:567aa3346701 279
kityann 0:567aa3346701 280 if(flag){
kityann 0:567aa3346701 281 if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
kityann 0:567aa3346701 282 flag = false;
kityann 0:567aa3346701 283 x[0][0]=(double)myGPS.longitudeL;
kityann 0:567aa3346701 284 x[0][1]=(double)myGPS.latitudeL;
kityann 0:567aa3346701 285 }
kityann 0:567aa3346701 286 }
kityann 0:567aa3346701 287 if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
kityann 0:567aa3346701 288
kityann 0:567aa3346701 289 //Kalman Filter
kityann 0:567aa3346701 290 Kalman(myGPS.longitudeL,myGPS.latitudeL);
kityann 0:567aa3346701 291 //kousinn
kityann 0:567aa3346701 292 for(int i = 0;i < 2;i++){
kityann 0:567aa3346701 293 for(int j = 0;j < 2;j++){
kityann 0:567aa3346701 294 K[0][i][j]=K[1][i][j];
kityann 0:567aa3346701 295 x[0][i]=x[1][i];
kityann 0:567aa3346701 296 sigma[0][i][j]=sigma[1][i][j];
kityann 0:567aa3346701 297 }
kityann 0:567aa3346701 298 }
kityann 0:567aa3346701 299
kityann 0:567aa3346701 300 myGPS.longitudeKH=myGPS.longitudeH;//longitude after filtering
kityann 0:567aa3346701 301 myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering
kityann 0:567aa3346701 302 myGPS.longitudeKL=(long)x[1][0];//longitude after filtering
kityann 0:567aa3346701 303 myGPS.latitudeKL=(long)x[1][1];//latitude after filtering
kityann 0:567aa3346701 304
kityann 0:567aa3346701 305 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:567aa3346701 306 agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
kityann 0:567aa3346701 307 //printf("state = %d\n",agz.nowMode);
kityann 0:567aa3346701 308 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:567aa3346701 309 }
kityann 0:567aa3346701 310 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:567aa3346701 311
kityann 0:567aa3346701 312 }
kityann 0:567aa3346701 313
kityann 0:567aa3346701 314
kityann 0:567aa3346701 315 //get base GPS
kityann 0:567aa3346701 316 //if this program is for base,this program don't execute
kityann 0:567aa3346701 317
kityann 0:567aa3346701 318 if( collect_Timer.read_ms() >= collect_Time){
kityann 0:567aa3346701 319 collect_Timer.reset();
kityann 0:567aa3346701 320
kityann 0:567aa3346701 321 //printf("Send Request:%d,%d\n",collect_flag,collect_state);
kityann 0:567aa3346701 322
kityann 0:567aa3346701 323 agz.createRequestCommand(MyID, collect_state);
kityann 0:567aa3346701 324 //Select Destination
kityann 0:567aa3346701 325 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
kityann 0:567aa3346701 326 //Send -> Base
kityann 0:567aa3346701 327 xbee.send(tx64request);
kityann 0:567aa3346701 328
kityann 0:567aa3346701 329 collect_flag++;
kityann 0:567aa3346701 330 collect_state++;
kityann 0:567aa3346701 331
kityann 0:567aa3346701 332 if(collect_flag == 4){
kityann 0:567aa3346701 333 collect_state = 'a';
kityann 0:567aa3346701 334 collect_flag = 0;
kityann 0:567aa3346701 335 }
kityann 0:567aa3346701 336 }
kityann 0:567aa3346701 337
kityann 0:567aa3346701 338 }
kityann 0:567aa3346701 339 }