Diff: main_processing/strategy_parts/output.cpp

Revision:

32:367b16d69a32

Parent:

30:5998ba42237e

Child:

33:aa115c30892e

--- a/main_processing/strategy_parts/output.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/strategy_parts/output.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -5,7 +5,20 @@
 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-cmps_set.HoldDeg-cmps_set.GoalDeg)+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*(sys.TurnAtkBlind==0)
+                -cmps_set.HoldDeg*(sys.TurnHoldBlind==0)
+                -cmps_set.GoalDeg*(sys.TurnDriBlind==0)
+            )+
+            7200.0
+        ),
+        360.0
+    );//0<cmps_set.cmps<359.0
     pid.setProcessValue(cmps_set.InputPID);
     cmps_set.OutputPID = -pid.compute();
     /*if((abs(cmps_set.OutputPID)<PID_OUT_MIN)&&(cmps_set.OutputPID!=0)){
@@ -22,28 +35,40 @@
     cmps_set.AtkDeg = 30;
 }
 void DriveTurn(void){
+    if(sys.TurnAtkBlind==1) return;
     if(sys.TurnStopFlag==1) return;
     
     if(
-        (data.ping[L_PING]<data.ping[R_PING])||
-        (data.lnRawOrderLog1[0]==B_SPOT)||
-        (data.lnRawOrder[0]==B_SPOT)
+        //(data.ping[L_PING]<data.ping[R_PING])||
+        ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]!=A_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
     ){
         cmps_set.AtkDeg = 150;
+        if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg>0)){
+            cmps_set.AtkDeg = -cmps_set.GoalDeg*2;
+        }
     }
     else if(
-        (data.ping[L_PING]>data.ping[R_PING])||
-        (data.lnRawOrderLog1[0]==A_SPOT)||
-        (data.lnRawOrder[0]==A_SPOT)
+        //(data.ping[L_PING]>data.ping[R_PING])||
+         ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]!=B_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
     ){
         cmps_set.AtkDeg = -150;
+        if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg<0)){
+            cmps_set.AtkDeg = cmps_set.GoalDeg*2;
+        }
     }
     else{
         cmps_set.AtkDeg = 0;
     }
+    if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg!=0)){
+        Turn_timeout.attach(&TurnSignal, .25);
+        Turn_stop.attach(&ValidTurn, .5);
+    }
+    else{
+        Turn_timeout.attach(&TurnSignal, .25);
+        Turn_stop.attach(&ValidTurn, .5);
+    }
     sys.TurnStopFlag=1;
-    Turn_timeout.attach(&TurnSignal, .5);
-    Turn_stop.attach(&ValidTurn, 2.0);
+    
 }
 void TurnSignal(void){
     cmps_set.AtkDeg = 0;
@@ -91,6 +116,7 @@
 }
 //solenoid
 void DriveSolenoid(void){
+    if(sys.KickBlind==1) return;
     if(sys.KickStopFlag==1) return;
     
     kicker=1;
@@ -98,6 +124,12 @@
     Solenoid_timeout.attach(&SolenoidSignal, .5);
     Kick_stop.attach(&ValidSolenoid, 2.0);
 }
+void DriveSolenoidJudge(void){
+    if((data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
+        DriveTurn();
+        DriveSolenoid();
+    }
+}
 void SolenoidSignal(void){
     kicker=0;
 }