Diff: main.cpp

Revision:

4:39d2aadf3068

Parent:

3:1229ca3df855

Child:

5:940424ec98a8

--- a/main.cpp	Fri May 30 15:36:33 2014 +0000
+++ b/main.cpp	Wed Jun 04 09:12:29 2014 +0000
@@ -5,24 +5,25 @@
 //  Program name: Aigamozu Robot Control
 //  author: Atsunori Maruyama
 //  ver ->  1.0
-//  day -> 2014/05/21
 //
 //  
 //
 /**********************************************/
+
 #include "mbed.h"
 #include "XBee.h"
 #include "MBed_Adafruit_GPS.h"
+#include "AigamozuControlPackets.h"
 #include "VNH5019.h"
 #include "agzIDLIST.h"
 #include "aigamozuSetting.h"
-#include  "AigamozuControlPackets.h"
 
 /////////////////////////////////////////
 //
 //Connection Setting
 //
 /////////////////////////////////////////
+
 //Serial Connect Setting: PC <--> mbed
 Serial pc(USBTX, USBRX);    
 
@@ -39,11 +40,11 @@
 //Pin Setting
 //
 /////////////////////////////////////////
+
 //Motor Contorol Pin Setting
 VNH5019 motorShield(p21,p22,p23,p24,p25,p26);
 AigamozuControlPackets agz;
 
-
 //Interrupt Setting
 Ticker axis;
 Ticker auth_axis;
@@ -109,6 +110,8 @@
     count++;
     }   
 
+
+
 /////////////////////////////////////////
 //
 //Main Processing
@@ -140,10 +143,11 @@
     //interrupt start
     axis.attach(&axisRenovation, 0.1);
     refresh_Timer.start();
-
+    
+    
     while (true) {
         
-        //recive xbee module 
+        //Check Xbee Buffer Available
         xbee.readPacket();
             if (xbee.getResponse().isAvailable()) {
                 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
@@ -151,39 +155,42 @@
                 unsigned char *buf = zbRx.getFrameData(); 
                  
                  
-                 //Analyze the command type
+                 //Check Command Type 
                  switch(agz.checkCommnadType(buf)){
-                    
-                    //CommnadType -> ChangeModeCommand;;;
-                    case CHANGE_MODE:{
+                    //CommandType -> ChanegeMode
+                    case CHANGE_MODE :{
                         my_mode = buf[19];
                         motorShield.changeSpeed(0,0,0,0);
                         if(my_mode == 2)  auth_axis.attach(&randomRenovation, 1.0);
                         else auth_axis.detach();
                         break;
                         }
-                    
-                    //CommnadType -> ManualCommand;;;
+
+                    //CommandType -> Manual
                     case MANUAL:{  
-                            /// @todo Magic.
-                            if(my_mode == 1){ 
-                            manual_count = 0;
-                            motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
+                        //Check now Mode
+                        if(agz.nowMode == MANUAL_MODE){ 
+                                //manual_count = 0;
+                                //Change Motor Behavior
+                                motorShield.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
                             }
                         break;
                         }
                     
-                    //SendStatus
+                    //CommandType -> Send Status
                     case STATUS_REQUEST:{
-                            //Create packet
-                            uint8_t* data = agz.createStatusPacket(myGPS.latitudeH, myGPS.latitudeL, myGPS.longitudeH, myGPS.longitudeL,1,2);
-                            ZBTxRequest tx64request(remoteAddress,data,sizeof(data));
-                            //Send GPS ingomation
+                            //Create GPS Infomation Packet
+                            uint8_t* data = agz.createStatusPacket(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL,'B','a');
+                            //Select Destination
+                            ZBTxRequest tx64request(remoteAddress,data,RECEIVES_GPS_STATUS_LENGTH);
+                            //Send -> Base
                             xbee.send(tx64request);
-                            break;          
+                            //Buffer Release
+                            delete data;
+                            break;         
+          
                         }
-                        
-                    //Default
+                    
                         default:
                         {
                         break;
@@ -191,7 +198,6 @@
                     }
                 }
             }
-        
             
         myGPS.read();
         //recive gps module
@@ -204,8 +210,8 @@
         
         if (refresh_Timer.read_ms() >= refresh_Time) {
             refresh_Timer.reset();
-            if (myGPS.fix) my_status = 0;
-            else my_status = 1;
+            if (myGPS.fix) agz.nowStatus = GPS_AVAIL;
+            else agz.nowStatus = GPS_UNAVAIL;
         }     
     }
 }
\ No newline at end of file