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 
00021 /////////////////////////////////////////
00022 //
00023 //Connection Setting
00024 //
00025 /////////////////////////////////////////
00026 
00027 //Serial Connect Setting: PC <--> mbed
00028 Serial pc(USBTX, USBRX);    
00029 
00030 //Serial Connect Setting: GPS <--> mbed
00031 Serial * gps_Serial;
00032 
00033 //Serial Connect Setting: XBEE <--> mbed
00034 XBee xbee(p13,p14);
00035 ZBRxResponse zbRx = ZBRxResponse();
00036 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
00037 
00038 AGZ_ROBOT robot[4];
00039 
00040 /////////////////////////////////////////
00041 //
00042 //Pin Setting
00043 //
00044 /////////////////////////////////////////
00045 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
00046 
00047 /////////////////////////////////////////
00048 //
00049 //Main Processing
00050 //
00051 /////////////////////////////////////////
00052 int main() {
00053     //start up time
00054     wait(3);
00055     //set pc frequency to 57600bps 
00056     pc.baud(PC_BAUD_RATE); 
00057     //set xbee frequency to 57600bps
00058     xbee.begin(XBEE_BAUD_RATE);    
00059     
00060     //GPS setting
00061     gps_Serial = new Serial(p28,p27);
00062     Adafruit_GPS myGPS(gps_Serial); 
00063     Timer refresh_Timer;
00064     const int refresh_Time = 2000; //refresh time in ms
00065     myGPS.begin(GPS_BAUD_RATE); 
00066     
00067     Timer collect_Timer;
00068     const int collect_Time = 10000; //when we collect 4 GPS point, we use 
00069     int collect_flag = 0;
00070     char collect_state = 'a';
00071     XBeeAddress64 collect_Address[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), 
00072                                         XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
00073     XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
00074     int id;
00075   
00076     
00077     //GPS Send Command
00078     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
00079     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00080     myGPS.sendCommand(PGCMD_ANTENNA);
00081     
00082     wait(2);
00083        
00084     //interrupt start
00085     AigamozuControlPackets agz(agz_motorShield);
00086     refresh_Timer.start();
00087     
00088     printf("test\n");
00089     
00090     
00091     while (true) {
00092         
00093         //Check Xbee Buffer Available
00094         xbee.readPacket();
00095             if (xbee.getResponse().isAvailable()) {
00096                 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
00097                 xbee.getResponse().getZBRxResponse(zbRx);
00098                 uint8_t *buf = zbRx.getFrameData();
00099                 uint8_t *buf1 = &buf[11]; 
00100                  
00101                  
00102                  //Check Command Type 
00103                  switch(agz.checkCommnadType(buf)){
00104                     
00105                     //CommandType -> ChanegeMode
00106                     case CHANGE_MODE :{
00107                         agz.changeMode(buf);                                        
00108                         break;
00109                         }
00110 
00111                     //CommandType -> Manual
00112                     case MANUAL:{  
00113                         //Check now Mode
00114                         if(agz.nowMode == MANUAL_MODE){ 
00115                             agz.changeSpeed(buf);
00116                             }
00117                         break;
00118                         }
00119                     
00120                     //CommandType -> Send Status
00121                     case STATUS_REQUEST:{
00122                             //Create GPS Infomation Packet
00123                             agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00124                             //Select Destination
00125                                 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
00126                             //Send -> Base
00127                             xbee.send(tx64request);
00128                             break;         
00129           
00130                         }
00131                         
00132                     case RECEIVE_STATUS:{
00133                             
00134                         printf("Receive Status\n");      
00135                            
00136                         id = buf1[6] - 'A';
00137                         robot[id].set_state(buf1[9]);
00138                         robot[id].set_LatitudeH(&buf1[13]);
00139                         robot[id].set_LatitudeL(&buf1[17]);
00140                         robot[id].set_LongitudeH(&buf1[21]);
00141                         robot[id].set_LongitudeL(&buf1[25]); 
00142                             
00143                         printf("%d\n", id);                    
00144                         printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
00145                         break;
00146                     }
00147                     
00148                     default:{
00149                         break;
00150                     }
00151                 }
00152                     
00153             
00154                 }
00155             }
00156             
00157         myGPS.read();
00158         //recive gps module
00159         //check if we recieved a new message from GPS, if so, attempt to parse it,
00160         if ( myGPS.newNMEAreceived() ) {
00161             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00162                 continue;   
00163             }       
00164         }
00165         
00166         if (refresh_Timer.read_ms() >= refresh_Time) {
00167             refresh_Timer.reset();
00168             if (myGPS.fix) {
00169                 agz.nowStatus = GPS_AVAIL;
00170                 agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00171             }
00172             else agz.nowStatus = GPS_UNAVAIL;
00173         } 
00174         
00175         
00176         if( collect_Timer.read_ms() >= collect_Time){
00177             collect_Timer.reset();
00178             
00179             printf("Send Request\n");
00180                       
00181             agz.createRequestCommand('A', collect_state);
00182             //Select Destination
00183             ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
00184             //Send -> Base
00185             xbee.send(tx64request);
00186             
00187             collect_flag++;
00188             collect_state++;
00189 
00190             if(collect_flag == 4){
00191                 collect_state = 'a';
00192                 collect_flag = 0;
00193             }    
00194          } 
00195     } 
00196 }