Diff: main_processing/strategy/strategy.cpp

Revision:

27:769cb5a7ea37

Parent:

26:fbb03281fc7d

Child:

28:8ac6c3c1e643

--- a/main_processing/strategy/strategy.cpp	Sun Jan 31 06:53:54 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Sun Jan 31 15:46:19 2016 +0000
@@ -3,11 +3,21 @@
 
 //Atk
 void modeAttack0(void){
-    
+    int vx,vy,vs;
+    uint8_t LineStop[2];
+    static uint8_t moveLnFlag[4]={1, 1, 1, 1};
+    static int moveLnlog[4];
+    //int LineForce[2];
+    //int Return;
+    //int line
     if(data.IrFlag==1){
         ReadIr();
         data.IrFlag=0;
     }
+    if(data.PidFlag==1){
+        PidUpdate();
+        data.PidFlag=0;
+    }
     //move(30,30,data.OutputPID);
     //move(30,0,data.OutputPID);
     /*move(
@@ -15,9 +25,78 @@
         ir_move_val[data.irNotice][data.irPosition][IR_Y],
         data.OutputPID
     );*/
-    move(
+    vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
+    vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+    vs = data.OutputPID;
+    
+    data.motorlog[X_AXIS] += vx;
+    data.motorlog[Y_AXIS] += vy;
+    
+    if((data.lnFlag[A_SPOT]==1)&&(1)){
+        moveLnFlag[A_SPOT]=0;
+        moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
+    }
+    if((data.lnFlag[B_SPOT]==1)&&(1)){
+        moveLnFlag[B_SPOT]=0;
+        moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
+    }
+    if((data.lnFlag[C_SPOT]==1)&&(1)){
+        moveLnFlag[C_SPOT]=0;
+        moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
+    }
+    if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
+        moveLnFlag[AB_SPOT]=0;
+        moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
+    }
+    
+    if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
+    if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
+    if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
+    if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
+    
+    //LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
+    //LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
+    
+    LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
+    LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
     
+    move(
+        vx*LineStop[X_AXIS],
+        vy*LineStop[Y_AXIS],
+        vs
     );
+    /*
+    LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
+    if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
+        LineForce[X_AXIS] = -LINE_RF;
+    }
+    if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
+        LineForce[X_AXIS] = LINE_RF;
+    }
+    if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
+        LineForce[Y_AXIS] = -LINE_RF;
+    }
+    if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
+        LineForce[Y_AXIS] = LINE_RF;
+    }
+    
+    move(
+        vx*LineStop[X_AXIS] + LineForce[X_AXIS],
+        vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
+        vs
+    );*/
+    
+    /*
+    move(
+        0,//vx*LineStop[X_AXIS] + LineForce[X_AXIS],
+        0,//vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
+        vs
+    );
+    */
+    if(data.MotorFlag==1){
+        tx_motor();
+        data.MotorFlag=0;
+    }
     //move(0,0,data.OutputPID);
     return;
 }