aigamozu / Mbed 2 deprecated aigamozu_auto_ver2

ver2

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

Fork of aigamozu_auto_ver1 by aigamozu

main.cpp@5:45e5e9e3dff3, 2015-04-20 (annotated)

Committer:
kityann
Date:
Mon Apr 20 10:38:55 2015 +0000
Revision:
5:45e5e9e3dff3
Parent:
4:dc2babaa1f61 
a

Who changed what in which revision?

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