Diff: main.cpp

Revision:

0:d8f3aa3b6876

Child:

1:a5f98c7e1feb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 11 11:57:26 2015 +0000
@@ -0,0 +1,151 @@
+/**********************************************/
+//  
+//    
+//
+//  Program name: Aigamozu Robot Control
+//  Author: Atsunori Maruyama
+//  Ver ->  1.3
+//  Day ->  2014/06/09
+//  
+//
+/**********************************************/
+
+#include "mbed.h"
+#include "XBee.h"
+#include "MBed_Adafruit_GPS.h"
+#include "AigamozuControlPackets.h"
+#include "agzIDLIST.h"
+#include "aigamozuSetting.h"
+
+/////////////////////////////////////////
+//
+//Connection Setting
+//
+/////////////////////////////////////////
+
+//Serial Connect Setting: PC <--> mbed
+Serial pc(USBTX, USBRX);    
+
+//Serial Connect Setting: GPS <--> mbed
+Serial * gps_Serial;
+
+//Serial Connect Setting: XBEE <--> mbed
+XBee xbee(p13,p14);
+ZBRxResponse zbRx = ZBRxResponse();
+XBeeAddress64 remoteAddress = XBeeAddress64(PAN1B1_32H,PAN1B1_32L);
+
+//Timer
+Timer t;
+int flag=0;
+
+/////////////////////////////////////////
+//
+//Pin Setting
+//
+/////////////////////////////////////////
+VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
+
+/////////////////////////////////////////
+//
+//Main Processing
+//
+/////////////////////////////////////////
+int main() {
+    int count = 0;
+    //start up time
+    wait(3);
+    //set pc frequency to 57600bps 
+    pc.baud(PC_BAUD_RATE); 
+    //set xbee frequency to 57600bps
+    xbee.begin(XBEE_BAUD_RATE);    
+    
+    //GPS setting
+    gps_Serial = new Serial(p28,p27);
+    Adafruit_GPS myGPS(gps_Serial); 
+    Timer refresh_Timer;
+    const int refresh_Time = 2000; //refresh time in ms
+    myGPS.begin(GPS_BAUD_RATE); 
+    
+    //GPS Send Command
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
+    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
+    myGPS.sendCommand(PGCMD_ANTENNA);
+    
+    wait(2);
+       
+    //interrupt start
+    AigamozuControlPackets agz(agz_motorShield);
+    refresh_Timer.start();
+    
+    
+    while (true) {
+       
+        //Check Xbee Buffer Available
+        xbee.readPacket();
+            if (xbee.getResponse().isAvailable()) {
+                if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
+                xbee.getResponse().getZBRxResponse(zbRx);
+                uint8_t *buf = zbRx.getFrameData(); 
+  
+                 
+                 //Check Command Type 
+                 switch(agz.checkCommnadType(buf)){
+                    
+                    //CommandType -> ChanegeMode
+                    case CHANGE_MODE :{
+                        agz.changeMode(buf);                                        
+                        break;
+                        }
+
+                    //CommandType -> Manual
+                    case MANUAL:{  
+                        //Check now Mode
+                        if(agz.nowMode == MANUAL_MODE){ 
+                            agz.changeSpeed(buf);
+                            }
+                        break;
+                        }
+                    
+                    //CommandType -> Send Status
+                    case STATUS_REQUEST:{
+                            //Create GPS Infomation Packet
+                            agz.createReceiveStatusCommand(1,6,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+                            //Select Destination
+                            ZBTxRequest tx64request(remoteAddress,agz.packetData,agz.getPacketLength());
+                            //Send -> Base
+                            xbee.send(tx64request);
+                            break;         
+          
+                        }
+                    
+                        default:
+                        {
+                        break;
+                            }
+                    }
+                }
+            }
+            
+        myGPS.read();
+        //recive gps module
+        //check if we recieved a new message from GPS, if so, attempt to parse it,
+        if ( myGPS.newNMEAreceived() ) {
+            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+                continue;   
+            }       
+        }
+        
+        if (refresh_Timer.read_ms() >= refresh_Time) {
+            refresh_Timer.reset();
+            if (myGPS.fix) {
+                agz.nowStatus = GPS_AVAIL;
+                agz.reNewPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
+            }
+            else agz.nowStatus = GPS_UNAVAIL;
+        } 
+        
+
+        
+            
+    }
+}
\ No newline at end of file