Diff: AigamozuControlPackets.cpp

Revision:

5:3f51eeb5aedc

Parent:

4:04dadf67ecb6

Child:

6:f164a716be46

--- a/AigamozuControlPackets.cpp	Sun Jun 08 11:54:53 2014 +0000
+++ b/AigamozuControlPackets.cpp	Sun Jun 08 15:09:02 2014 +0000
@@ -2,12 +2,12 @@
 #include "VNH5019.h"
 
 
-AigamozuControlPackets::AigamozuControlPackets(Ticker autoInt){
+AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
     packetData = new uint8_t[50];
     packetLength = 0;
-    
     }
 
+
 //////////////////////////////
 // Controller/Base -> Robot //
 //////////////////////////////
@@ -68,10 +68,36 @@
         return packetLength;
     }
 
+void AigamozuControlPackets::changeSpeed(uint8_t* buf){
+    if(nowMode == MANUAL_MODE){
+            manualCount =0;
+            _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
+        }
+    }
 
+void AigamozuControlPackets::randomAuto(){
     
+    randomCount++;
+    
+    if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128);
+    else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128);
+    else randomCount = 0;
+    
+    }
+    
+void AigamozuControlPackets::manualMode(){
+    manualCount++;
+    if(manualCount > 10){
+        _agzSheild.changeSpeed(0,0,0,0);
+        manualCount = 0;
+        }
+    }
+
 bool AigamozuControlPackets::changeMode(uint8_t *buf){
     
+    _agzSheild.changeSpeed(0,0,0,0);
+    hoge.detach();
+
     switch(buf[19]){
         
         case 0:
@@ -79,18 +105,25 @@
         break;
         
         case 1:
+            hoge.attach(this,&AigamozuControlPackets::manualMode,1.0);
             nowMode = MANUAL_MODE;
         break;
         
         case 2:
             nowMode = AUTO_MODE;
+            hoge.attach(this,&AigamozuControlPackets::randomAuto,1.0);
+
         break;
         
         case 3:
             nowMode = AUTO_GPS_MODE;
         break;
         
+        
+        
         }
     return false;
 }
+
+