Diff: main.cpp

Revision:

3:1229ca3df855

Parent:

2:95955f38f47a

Child:

4:39d2aadf3068

--- a/main.cpp	Wed May 28 11:54:22 2014 +0000
+++ b/main.cpp	Fri May 30 15:36:33 2014 +0000
@@ -1,33 +1,28 @@
 /**********************************************/
-//
+//  
+//    
 //
 //  Program name: Aigamozu Robot Control
 //  author: Atsunori Maruyama
-//  
+//  ver ->  1.0
+//  day -> 2014/05/21
 //
-//  ver ->  1.0
 //  
 //
 /**********************************************/
-
 #include "mbed.h"
 #include "XBee.h"
 #include "MBed_Adafruit_GPS.h"
 #include "VNH5019.h"
 #include "agzIDLIST.h"
-
-
-#define GPS_BAUD_RATE 9600
-#define XBEE_BAUD_RATE 57600
-#define PC_BAUD_RATE 57600
-#define IMU_BAUD_RATE 400000
+#include "aigamozuSetting.h"
+#include  "AigamozuControlPackets.h"
 
 /////////////////////////////////////////
 //
 //Connection Setting
 //
 /////////////////////////////////////////
-
 //Serial Connect Setting: PC <--> mbed
 Serial pc(USBTX, USBRX);    
 
@@ -44,9 +39,10 @@
 //Pin Setting
 //
 /////////////////////////////////////////
-
 //Motor Contorol Pin Setting
 VNH5019 motorShield(p21,p22,p23,p24,p25,p26);
+AigamozuControlPackets agz;
+
 
 //Interrupt Setting
 Ticker axis;
@@ -75,22 +71,6 @@
 int manual_count = 0;
 int manual_flag = 0;
 
-//test value
-long sub_latH = 12345;
-long sub_latL = 67890;
-long sub_lonH = 98765;
-long sub_lonL = 43211;
-
-
-union UNI_TEST_T
-{
-    long a;
-    uint8_t b[4];
-};
-
-UNI_TEST_T latH,latL,lonH,lonL;
-
-
 /////////////////////////////////////////
 //
 //Interruption processing 1: time -> 0.1s
@@ -129,8 +109,6 @@
     count++;
     }   
 
-
-
 /////////////////////////////////////////
 //
 //Main Processing
@@ -162,8 +140,7 @@
     //interrupt start
     axis.attach(&axisRenovation, 0.1);
     refresh_Timer.start();
-    
-    /// @todo change to true
+
     while (true) {
         
         //recive xbee module 
@@ -173,17 +150,12 @@
                 xbee.getResponse().getZBRxResponse(zbRx);
                 unsigned char *buf = zbRx.getFrameData(); 
                  
-                 /// @todo use preporcessor (#define) or const.
-                 /// @note It's magic number.
-                 // COMMAND_TYPE commandType = buf[14];
-                 /// switch(commandType)
-                 /// {case MANUAL:}
                  
-                 
-                 //switch(aigamoCTR.checkCommandType(buf)){
-                 switch((unsigned char)buf[14]){
-                    //ChanegeMode
-                    case 'C':{
+                 //Analyze the command type
+                 switch(agz.checkCommnadType(buf)){
+                    
+                    //CommnadType -> ChangeModeCommand;;;
+                    case CHANGE_MODE:{
                         my_mode = buf[19];
                         motorShield.changeSpeed(0,0,0,0);
                         if(my_mode == 2)  auth_axis.attach(&randomRenovation, 1.0);
@@ -191,14 +163,8 @@
                         break;
                         }
                     
-                    /*
-                    case 'C':
-                    agz.change_Mode();
-                    break;
-                    */
-                    
-                    //MunualCommand
-                    case 'M':{  
+                    //CommnadType -> ManualCommand;;;
+                    case MANUAL:{  
                             /// @todo Magic.
                             if(my_mode == 1){ 
                             manual_count = 0;
@@ -207,33 +173,17 @@
                         break;
                         }
                     
-                    /*
-                    case 'M':
-                    uint8_t sppeds_buf[4] = agz.parseMunualCommand(buf);
-                    motorShield.changeSpeed(sppedbuf[0],sppedbuf[1],sppedbuf[2],sppedbuf[3]);
-                    break;
-                    */
-                    
                     //SendStatus
-                    case 'S':{
-                            latH.a = myGPS.latitudeH;;
-                            latL.a = myGPS.latitudeL;
-                            lonH.a = myGPS.longitudeH;
-                            lonL.a = myGPS.longitudeL;
-                            //dummy data
-                            //latH.a = sub_latH;latL.a = sub_latL;lonH.a = sub_lonH;lonL.a = sub_lonL;
-                            
-                            // if(validateCommand(commandType));
-                            // createReceiveStatusData();
-                            uint8_t data[] = {65,71,83,82,70,'2',84,'A',83,my_status,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69};
+                    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
                             xbee.send(tx64request);
-                            break;         
-                    /*
-                    case 'S':
-                    */        
+                            break;          
                         }
-                    
+                        
+                    //Default
                         default:
                         {
                         break;
@@ -241,6 +191,7 @@
                     }
                 }
             }
+        
             
         myGPS.read();
         //recive gps module