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                     printf("%c\n", buf[14]);
00106                     
00107                     //CommandType -> ChanegeMode
00108                     case CHANGE_MODE :{
00109                         agz.changeMode(buf);                                        
00110                         break;
00111                         }
00112 
00113                     //CommandType -> Manual
00114                     case MANUAL:{  
00115                         //Check now Mode
00116                         if(agz.nowMode == MANUAL_MODE){ 
00117                             agz.changeSpeed(buf);
00118                             }
00119                         break;
00120                         }
00121                     
00122                     //CommandType -> Send Status
00123                     case STATUS_REQUEST:{
00124                             //Create GPS Infomation Packet
00125                             agz.createReceiveStatusCommand('B','a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00126                             //Select Destination
00127                             ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
00128                             //Send -> Base
00129                             xbee.send(tx64request);
00130                             break;         
00131           
00132                         }
00133                         
00134                         case RECEIVE_STATUS:{
00135                             
00136                             printf("ok\n");      
00137                            
00138                             id = buf1[6] - 'A';
00139                             robot[id].set_state(buf1[9]);
00140                             robot[id].set_LatitudeH(&buf1[13]);
00141                             robot[id].set_LatitudeL(&buf1[17]);
00142                             robot[id].set_LongitudeH(&buf1[21]);
00143                             robot[id].set_LongitudeL(&buf1[25]); 
00144                             
00145                             printf("%d\n", id);                    
00146                             printf("%ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
00147                             break;
00148                         }
00149                     
00150                         default:
00151                         {
00152                         break;
00153                             }
00154                     }
00155                     
00156             
00157                 }
00158             }
00159             
00160         myGPS.read();
00161         //recive gps module
00162         //check if we recieved a new message from GPS, if so, attempt to parse it,
00163         if ( myGPS.newNMEAreceived() ) {
00164             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00165                 continue;   
00166             }       
00167         }
00168         
00169         if (refresh_Timer.read_ms() >= refresh_Time) {
00170             refresh_Timer.reset();
00171             if (myGPS.fix) {
00172                 agz.nowStatus = GPS_AVAIL;
00173                 agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
00174             }
00175             else agz.nowStatus = GPS_UNAVAIL;
00176         } 
00177         
00178         
00179         if( collect_Timer.read_ms() >= collect_Time){
00180             collect_Timer.reset();
00181             
00182             printf("Send Request\n");
00183                       
00184             agz.createRequestCommand('A', collect_state);
00185             //Select Destination
00186             ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
00187             //Send -> Base
00188             xbee.send(tx64request);
00189             
00190             collect_flag++;
00191             collect_state++;
00192 
00193             if(collect_flag == 4){
00194                 printf("%d %c\n", collect_flag, collect_state);
00195                 collect_state = 'a';
00196                 collect_flag = 0;
00197             }    
00198          } 
00199     } 
00200 }