Diff: AigamozuControlPackets.cpp

Revision:

49:3f097e4ad60f

Parent:

48:4695e72853c6

--- a/AigamozuControlPackets.cpp	Fri Mar 17 09:02:41 2017 +0000
+++ b/AigamozuControlPackets.cpp	Fri Mar 17 10:12:14 2017 +0000
@@ -1,8 +1,8 @@
 #include "AigamozuControlPackets.h"
 #include "VNH5019.h"
 
-#define RIGHT_SPEED 96
-#define LEFT_SPEED 96
+#define HIGH_SPEED 96
+#define LOW_SPEED 32
 #define FORWARD_SPEED 96
 
 //////////////////////////////
@@ -36,7 +36,7 @@
 
 
 //////////////////////////////
-// Robot -> Xbee     
+// Robot -> PC   
 // ジャイロ・加速度データを送信する     //
 //////////////////////////////
 
@@ -69,7 +69,7 @@
      
 
 //////////////////////////////
-// Robot -> Xbee     
+// PC -> Robot    
 // 受信したジャイロ・加速度データを変換する    //
 //////////////////////////////
 //Updata Base Point   
@@ -95,6 +95,50 @@
 }  
 
 //////////////////////////////
+// PC -> Robot   
+// ジャイロ・加速度データを送信する     //
+//////////////////////////////
+ void AigamozuControlPackets::CreateManualCommand(uint8_t fromID,uint8_t toID,char c)
+ {  
+    uint8_t directionL, directionR, pwmL, pwmR;
+    if(c == 'a'){
+        directionL = LOW_SPEED;
+        pwmL = 1;
+        directionR = HIGH_SPEED;
+        pwmR = 1;
+    }
+    else if(c == 'w'){
+        directionL = FORWARD_SPEED;
+        pwmL = 1;
+        directionR = FORWARD_SPEED;
+        pwmR = 1;
+    }
+    if(c == 'd'){
+        directionL = HIGH_SPEED;
+        pwmL = 1;
+        directionR = LOW_SPEED;
+        pwmR = 1;
+    }
+    if(c == 'z'){
+        directionL = FORWARD_SPEED;
+        pwmL = 2;
+        directionR = FORWARD_SPEED;
+        pwmR = 2;
+    }
+    if(c == 's'){
+        directionL = FORWARD_SPEED;
+        pwmL = 0;
+        directionR = FORWARD_SPEED;
+        pwmR = 0;
+    }
+        
+    
+    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 < MANUAL_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
+    packetLength = MANUAL_COMMAND_LENGTH;
+     }
+
+//////////////////////////////
 //   each mode interrupt    //
 //////////////////////////////
 
@@ -114,7 +158,7 @@
     
    if(nowMode == AUTO_GPS_MODE){
        //if(Move_Timer.read_ms() < 5000){
-            _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn
+            _agzSheild.changeSpeed(1,HIGH_SPEED,1,LOW_SPEED);//turn
       //  }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){
       //      _agzSheild.changeSpeed(0,0,0,0);//stop
         //    Move_Timer.reset();