aigamozu / Mbed 2 deprecated aigamozu_auto_ver3_3_2_Nakazawa

20150511, Nakazawa

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

Fork of aigamozu_auto_ver3_3_2 by aigamozu

main.cpp@0:68d27eee9a6c, 2015-04-25 (annotated)

Committer:
kityann
Date:
Sat Apr 25 01:06:06 2015 +0000
Revision:
0:68d27eee9a6c
Child:
1:d351cf39a6e3
test

Who changed what in which revision?

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