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@4:dc2babaa1f61, 2015-04-14 (annotated)

Committer:
kityann
Date:
Tue Apr 14 15:11:29 2015 +0000
Revision:
4:dc2babaa1f61
Parent:
3:7affc8af6db6
Child:
5:45e5e9e3dff3
final

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 4:dc2babaa1f61 63 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 4:dc2babaa1f61 64 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(sigma[1][1][1]+sigmaGPS[1][1]);
kityann 4:dc2babaa1f61 65 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(sigma[1][0][0]+sigmaGPS[0][0]);
kityann 4:dc2babaa1f61 66 }
kityann 4:dc2babaa1f61 67
kityann 4:dc2babaa1f61 68 void get_x(){
kityann 4:dc2babaa1f61 69 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
kityann 4:dc2babaa1f61 70 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
kityann 4:dc2babaa1f61 71 }
kityann 4:dc2babaa1f61 72
kityann 4:dc2babaa1f61 73 void get_sigma(){
kityann 4:dc2babaa1f61 74 double temp[2][2];
kityann 4:dc2babaa1f61 75 for(int i=0;i<2;i++) {
kityann 4:dc2babaa1f61 76 for(int j=0;j<2;j++) {
kityann 4:dc2babaa1f61 77 for(int k=0;k<2;k++) {
kityann 4:dc2babaa1f61 78 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 4:dc2babaa1f61 79 }
kityann 4:dc2babaa1f61 80 }
kityann 4:dc2babaa1f61 81 }
kityann 4:dc2babaa1f61 82 for(int i = 0;i < 2;i++){
kityann 4:dc2babaa1f61 83 for(int j = 0;j < 2;j++){
kityann 4:dc2babaa1f61 84 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 4:dc2babaa1f61 85 }
kityann 4:dc2babaa1f61 86 }
kityann 4:dc2babaa1f61 87 }
kityann 4:dc2babaa1f61 88
kityann 4:dc2babaa1f61 89 void Kalman(double Latitude,double Longitude){
kityann 4:dc2babaa1f61 90 y[0] = Latitude;
kityann 4:dc2babaa1f61 91 y[1] = Longitude;
kityann 4:dc2babaa1f61 92 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 4:dc2babaa1f61 93 get_K();
kityann 4:dc2babaa1f61 94 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 4:dc2babaa1f61 95 get_x();
kityann 4:dc2babaa1f61 96 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 4:dc2babaa1f61 97 get_sigma();
kityann 4:dc2babaa1f61 98 }
kityann 4:dc2babaa1f61 99
kityann 4:dc2babaa1f61 100
kityann 0:d8f3aa3b6876 101 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 102 //
kityann 0:d8f3aa3b6876 103 //Main Processing
kityann 0:d8f3aa3b6876 104 //
kityann 0:d8f3aa3b6876 105 /////////////////////////////////////////
kityann 0:d8f3aa3b6876 106 int main() {
kityann 0:d8f3aa3b6876 107 //start up time
kityann 0:d8f3aa3b6876 108 wait(3);
kityann 0:d8f3aa3b6876 109 //set pc frequency to 57600bps
kityann 0:d8f3aa3b6876 110 pc.baud(PC_BAUD_RATE);
kityann 0:d8f3aa3b6876 111 //set xbee frequency to 57600bps
kityann 0:d8f3aa3b6876 112 xbee.begin(XBEE_BAUD_RATE);
kityann 0:d8f3aa3b6876 113
kityann 0:d8f3aa3b6876 114 //GPS setting
kityann 0:d8f3aa3b6876 115 gps_Serial = new Serial(p28,p27);
kityann 0:d8f3aa3b6876 116 Adafruit_GPS myGPS(gps_Serial);
kityann 0:d8f3aa3b6876 117 Timer refresh_Timer;
kityann 0:d8f3aa3b6876 118 const int refresh_Time = 2000; //refresh time in ms
kityann 0:d8f3aa3b6876 119 myGPS.begin(GPS_BAUD_RATE);
kityann 0:d8f3aa3b6876 120
kityann 1:a5f98c7e1feb 121 Timer collect_Timer;
kityann 1:a5f98c7e1feb 122 const int collect_Time = 200; //when we collect 4 GPS point, we use
kityann 1:a5f98c7e1feb 123 int collect_flag = 0;
kityann 1:a5f98c7e1feb 124 char collect_state = 'a';
kityann 1:a5f98c7e1feb 125 XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
kityann 1:a5f98c7e1feb 126 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
kityann 1:a5f98c7e1feb 127 XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
kityann 1:a5f98c7e1feb 128 int id;
kityann 4:dc2babaa1f61 129 bool flag = true;
kityann 1:a5f98c7e1feb 130
kityann 0:d8f3aa3b6876 131 //GPS Send Command
kityann 0:d8f3aa3b6876 132 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:d8f3aa3b6876 133 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:d8f3aa3b6876 134 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:d8f3aa3b6876 135
kityann 0:d8f3aa3b6876 136 wait(2);
kityann 0:d8f3aa3b6876 137
kityann 0:d8f3aa3b6876 138 //interrupt start
kityann 0:d8f3aa3b6876 139 AigamozuControlPackets agz(agz_motorShield);
kityann 0:d8f3aa3b6876 140 refresh_Timer.start();
kityann 0:d8f3aa3b6876 141
kityann 1:a5f98c7e1feb 142 printf("test\n");
kityann 1:a5f98c7e1feb 143
kityann 0:d8f3aa3b6876 144
kityann 0:d8f3aa3b6876 145 while (true) {
kityann 1:a5f98c7e1feb 146
kityann 0:d8f3aa3b6876 147 //Check Xbee Buffer Available
kityann 0:d8f3aa3b6876 148 xbee.readPacket();
kityann 0:d8f3aa3b6876 149 if (xbee.getResponse().isAvailable()) {
kityann 0:d8f3aa3b6876 150 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:d8f3aa3b6876 151 xbee.getResponse().getZBRxResponse(zbRx);
kityann 1:a5f98c7e1feb 152 uint8_t *buf = zbRx.getFrameData();
kityann 1:a5f98c7e1feb 153 uint8_t *buf1 = &buf[11];
kityann 1:a5f98c7e1feb 154
kityann 0:d8f3aa3b6876 155
kityann 0:d8f3aa3b6876 156 //Check Command Type
kityann 0:d8f3aa3b6876 157 switch(agz.checkCommnadType(buf)){
kityann 0:d8f3aa3b6876 158
kityann 0:d8f3aa3b6876 159 //CommandType -> ChanegeMode
kityann 0:d8f3aa3b6876 160 case CHANGE_MODE :{
kityann 0:d8f3aa3b6876 161 agz.changeMode(buf);
kityann 0:d8f3aa3b6876 162 break;
kityann 0:d8f3aa3b6876 163 }
kityann 0:d8f3aa3b6876 164
kityann 0:d8f3aa3b6876 165 //CommandType -> Manual
kityann 0:d8f3aa3b6876 166 case MANUAL:{
kityann 0:d8f3aa3b6876 167 //Check now Mode
kityann 0:d8f3aa3b6876 168 if(agz.nowMode == MANUAL_MODE){
kityann 0:d8f3aa3b6876 169 agz.changeSpeed(buf);
kityann 0:d8f3aa3b6876 170 }
kityann 0:d8f3aa3b6876 171 break;
kityann 0:d8f3aa3b6876 172 }
kityann 0:d8f3aa3b6876 173
kityann 0:d8f3aa3b6876 174 //CommandType -> Send Status
kityann 0:d8f3aa3b6876 175 case STATUS_REQUEST:{
kityann 0:d8f3aa3b6876 176 //Create GPS Infomation Packet
kityann 2:e9a8664b52ff 177 agz.createReceiveStatusCommand('D','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:d8f3aa3b6876 178 //Select Destination
kityann 1:a5f98c7e1feb 179 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
kityann 0:d8f3aa3b6876 180 //Send -> Base
kityann 0:d8f3aa3b6876 181 xbee.send(tx64request);
kityann 0:d8f3aa3b6876 182 break;
kityann 0:d8f3aa3b6876 183
kityann 0:d8f3aa3b6876 184 }
kityann 1:a5f98c7e1feb 185
kityann 1:a5f98c7e1feb 186 case RECEIVE_STATUS:{
kityann 1:a5f98c7e1feb 187
s1200058 3:7affc8af6db6 188 //printf("Receive Status\n");
kityann 1:a5f98c7e1feb 189
kityann 2:e9a8664b52ff 190 id = buf1[5] - 'A';
kityann 1:a5f98c7e1feb 191 robot[id].set_state(buf1[9]);
kityann 1:a5f98c7e1feb 192 robot[id].set_LatitudeH(&buf1[13]);
kityann 1:a5f98c7e1feb 193 robot[id].set_LatitudeL(&buf1[17]);
kityann 1:a5f98c7e1feb 194 robot[id].set_LongitudeH(&buf1[21]);
kityann 4:dc2babaa1f61 195 robot[id].set_LongitudeL(&buf1[25]);
kityann 2:e9a8664b52ff 196
kityann 2:e9a8664b52ff 197 agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
s1200058 3:7affc8af6db6 198 /*
s1200058 3:7affc8af6db6 199 printf("%d,", buf1[5]);
s1200058 3:7affc8af6db6 200 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 201 */
kityann 1:a5f98c7e1feb 202 break;
kityann 1:a5f98c7e1feb 203 }
kityann 0:d8f3aa3b6876 204
kityann 1:a5f98c7e1feb 205 default:{
kityann 0:d8f3aa3b6876 206 break;
kityann 0:d8f3aa3b6876 207 }
kityann 0:d8f3aa3b6876 208 }
kityann 1:a5f98c7e1feb 209
kityann 1:a5f98c7e1feb 210
kityann 1:a5f98c7e1feb 211 }
kityann 0:d8f3aa3b6876 212 }
kityann 1:a5f98c7e1feb 213
kityann 0:d8f3aa3b6876 214
kityann 0:d8f3aa3b6876 215 myGPS.read();
kityann 0:d8f3aa3b6876 216 //recive gps module
kityann 0:d8f3aa3b6876 217 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:d8f3aa3b6876 218 if ( myGPS.newNMEAreceived() ) {
kityann 0:d8f3aa3b6876 219 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:d8f3aa3b6876 220 continue;
kityann 0:d8f3aa3b6876 221 }
kityann 0:d8f3aa3b6876 222 }
kityann 0:d8f3aa3b6876 223 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:d8f3aa3b6876 224 refresh_Timer.reset();
kityann 0:d8f3aa3b6876 225 if (myGPS.fix) {
kityann 0:d8f3aa3b6876 226 agz.nowStatus = GPS_AVAIL;
kityann 4:dc2babaa1f61 227
kityann 4:dc2babaa1f61 228 if(flag){
kityann 4:dc2babaa1f61 229 if(myGPS.longitudeH!=0 || myGPS.latitudeH!=0){
kityann 4:dc2babaa1f61 230 flag = false;
kityann 4:dc2babaa1f61 231 x[0][0]=(double)myGPS.longitudeL;
kityann 4:dc2babaa1f61 232 x[0][1]=(double)myGPS.latitudeL;
kityann 4:dc2babaa1f61 233 }
kityann 4:dc2babaa1f61 234 }
kityann 4:dc2babaa1f61 235 //Kalman Filter
kityann 4:dc2babaa1f61 236 Kalman(myGPS.longitudeL,myGPS.latitudeL);
kityann 4:dc2babaa1f61 237 //kousinn
kityann 4:dc2babaa1f61 238 for(int i = 0;i < 2;i++){
kityann 4:dc2babaa1f61 239 for(int j = 0;j < 2;j++){
kityann 4:dc2babaa1f61 240 K[0][i][j]=K[1][i][j];
kityann 4:dc2babaa1f61 241 x[0][i]=x[1][i];
kityann 4:dc2babaa1f61 242 sigma[0][i][j]=sigma[1][i][j];
kityann 4:dc2babaa1f61 243 }
kityann 4:dc2babaa1f61 244 }
kityann 4:dc2babaa1f61 245 myGPS.longitudeL=(long)x[1][0];//longitude after filtering
kityann 4:dc2babaa1f61 246 myGPS.latitudeL=(long)x[1][1];//latitude after filtering
kityann 4:dc2babaa1f61 247
kityann 2:e9a8664b52ff 248 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
s1200058 3:7affc8af6db6 249 //printf("state = %d\n",agz.nowMode);
s1200058 3:7affc8af6db6 250 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:d8f3aa3b6876 251 }
kityann 0:d8f3aa3b6876 252 else agz.nowStatus = GPS_UNAVAIL;
kityann 1:a5f98c7e1feb 253
kityann 0:d8f3aa3b6876 254 }
kityann 0:d8f3aa3b6876 255
kityann 4:dc2babaa1f61 256
kityann 1:a5f98c7e1feb 257 //get base GPS
kityann 1:a5f98c7e1feb 258 if( collect_Timer.read_ms() >= collect_Time){
kityann 1:a5f98c7e1feb 259 collect_Timer.reset();
kityann 0:d8f3aa3b6876 260
s1200058 3:7affc8af6db6 261 //printf("Send Request:%d,%d\n",collect_flag,collect_state);
kityann 1:a5f98c7e1feb 262
kityann 1:a5f98c7e1feb 263 agz.createRequestCommand('A', collect_state);
kityann 1:a5f98c7e1feb 264 //Select Destination
kityann 1:a5f98c7e1feb 265 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
kityann 1:a5f98c7e1feb 266 //Send -> Base
kityann 1:a5f98c7e1feb 267 xbee.send(tx64request);
kityann 1:a5f98c7e1feb 268
kityann 1:a5f98c7e1feb 269 collect_flag++;
kityann 1:a5f98c7e1feb 270 collect_state++;
kityann 1:a5f98c7e1feb 271
kityann 1:a5f98c7e1feb 272 if(collect_flag == 4){
kityann 1:a5f98c7e1feb 273 collect_state = 'a';
kityann 1:a5f98c7e1feb 274 collect_flag = 0;
kityann 1:a5f98c7e1feb 275 }
kityann 1:a5f98c7e1feb 276 }
kityann 1:a5f98c7e1feb 277 }
kityann 0:d8f3aa3b6876 278 }