Diff: AigamozuControlPackets.cpp

Revision:

2:3f2d4f53ceed

Parent:

1:80448565c15c

Child:

3:282dad679fca

Child:

4:04dadf67ecb6

--- a/AigamozuControlPackets.cpp	Wed Jun 04 09:12:20 2014 +0000
+++ b/AigamozuControlPackets.cpp	Sun Jun 08 09:44:19 2014 +0000
@@ -2,12 +2,44 @@
 #include "VNH5019.h"
 
 
+AigamozuControlPackets::AigamozuControlPackets(){
+    packetData = new uint8_t[50];
+    packetLength = 0;
+    
+    }
+
 
 
-uint8_t* AigamozuControlPackets::createStatusPacket(long latitudeH,long latitudeL,long longitudeH,long longitudeL,uint8_t fromID,uint8_t toID)
+//////////////////////////////
+// Controller/Base -> Robot //
+//////////////////////////////
+ void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR)
+ {         
+    uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'};
+    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
+    packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
+     }
+     
+void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID,uint8_t)
 {
-    uint8_t* data = new uint8_t[RECEIVES_GPS_STATUS_LENGTH];
+    uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'};
+    for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 
+    packetLength = REQUEST_COMMNAD_LENGTH;
+    }
     
+void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){
+    uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'};
+    for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
+    packetLength = CHANGE_MODE_COMMAND_LENGTH;
+    }
+    
+
+//////////////////////////////
+// Robot -> Controller/Base //
+//////////////////////////////
+
+void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL)
+{
     //useing union's split::: long hoge.a(4byte) == uint8_t hoge.b[4] 
     TEST_T latH,latL,lonH,lonL;
     
@@ -17,41 +49,43 @@
     lonL.a = longitudeL;
     
     uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowStatus,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};
-    for(int i = 0; i < RECEIVES_GPS_STATUS_LENGTH; ++i) data[i] = tmp[i];
-    return data;
+    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
+    packetLength =  RECEIVE_STATUS_COMMNAD_LENGTH;
 }
 
-uint8_t* AigamozuControlPackets::createAckPacket(uint8_t fromID,uint8_t toID){ 
-    uint8_t* data = new uint8_t[RECEIVES_ACK_LENGTH];
-    uint8_t tmp[] = {'A','G','S','c','F',fromID,'T',toID,nowMode,nowStatus,'A','G','E'};
-    for(int i = 0; i < RECEIVES_ACK_LENGTH; ++i) data[i] = tmp[i];
-    return data;
+
+//////////////////////////////
+//    Using createPacket    //
+//////////////////////////////
+
+uint8_t* AigamozuControlPackets::getPacketData(){
+    return packetData;
+    }
+
+int AigamozuControlPackets::getPacketLength(){
+        return packetLength;
     }
 
 uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){
     return buf[14];
     }
-
-
-
-
-
-/*
-bool checkCommandAvilable(COMMAND_TYPE requestCommand){
-         
-         switch(nowMode){
-            case MANUAL:
-            if(requestCommand == ) return false;
-            break;
-             
-
-            default:
-            return true;
-            break;
-             
-             }
-             
-            return true;
-             
-    }
-*/
\ No newline at end of file
+    
+bool AigamozuControlPackets::changeMode(uint8_t *buf){
+    
+    switch(buf[19]){
+        
+        case 0:
+            nowMode = STANDBY_MODE;
+        break;
+        
+        case 1:
+            nowMode = MANUAL_MODE;
+        break;
+        
+        case 2:
+            nowMode = AUTO_MODE;
+        break;
+        }
+    return false;
+}
+