Diff: main_processing/strategy_parts/output.cpp

Revision:

25:a7460e23e02e

Parent:

24:34ef6379b0df

Child:

26:6ca88eeaa2b4

--- a/main_processing/strategy_parts/output.cpp	Thu Mar 17 02:49:45 2016 +0000
+++ b/main_processing/strategy_parts/output.cpp	Thu Mar 17 13:00:24 2016 +0000
@@ -5,7 +5,7 @@
 void PidUpdate(void)
 {
     cmps_set.cmps = hmc.sample()/10.0;
-    cmps_set.InputPID = fmod((cmps_set.cmps+(cmps_set.CmpsDiff-cmps_set.FrontDeg-cmps_set.AtkDeg)+7200.0), 360.0);//0<cmps_set.cmps<359.0
+    cmps_set.InputPID = fmod((cmps_set.cmps+(cmps_set.CmpsDiff-cmps_set.FrontDeg-cmps_set.AtkDeg-cmps_set.HoldDeg)+7200.0), 360.0);//0<cmps_set.cmps<359.0
 
     pid.setProcessValue(cmps_set.InputPID);
     cmps_set.OutputPID = -pid.compute();
@@ -25,7 +25,15 @@
 void DriveTurn(void){
     if(sys.TurnStopFlag==1) return;
     
-    cmps_set.AtkDeg = 150;
+    if(data.ping[L_PING]<data.ping[R_PING]){
+        cmps_set.AtkDeg = 150;
+    }
+    else if(data.ping[L_PING]>data.ping[R_PING]){
+        cmps_set.AtkDeg = -150;
+    }
+    else{
+        cmps_set.AtkDeg = 0;
+    }
     sys.TurnStopFlag=1;
     Turn_timeout.attach(&TurnSignal, .5);
     Turn_stop.attach(&ValidTurn, 2.0);
@@ -88,23 +96,14 @@
     sys.KickStopFlag=0;
 }
 //ball
-void AdmitBallHold(void){
-    /*if(data.ball==1){
+void JudgeBallHold(void){
+    if(data.ball==1){
         sys.BallHoldFlag=1;
     }
     else if(data.ball==0){
-        sys.BallHoldJudgeFlag=0;
         sys.BallHoldFlag=0;
-    }*/
-}
-void GiveUpBallHold(void){
-    /*if(data.ball==1){
-        sys.BallHoldFlag=1;
     }
-    else if(data.ball==0){
-        sys.BallHoldJudgeFlag=0;
-        sys.BallHoldFlag=0;
-    }*/
+    sys.BallHoldJudgeFlag=0;
 }
 //line
 void LineLiberate(void){