Diff: main_processing/strategy/strategy2.cpp

Revision:

18:3a42a931c95a

Parent:

10:6df631c39f9b

--- a/main_processing/strategy/strategy2.cpp	Mon Mar 14 08:56:04 2016 +0000
+++ b/main_processing/strategy/strategy2.cpp	Tue Mar 15 12:03:51 2016 +0000
@@ -1,161 +1,4 @@
 #include "mbed.h"
 #include "extern.h"
 
-//Debug
-void modeDebug0(void){
-    return;
-}
-void modeDebug1(void){//ChaseOnly NonLine
-int vx,vy,vs;
-    if(sys.IrFlag==1){
-        ReadIr();
-        sys.IrFlag=0;
-    }
-    if(sys.PidFlag==1){
-        PidUpdate();
-        sys.PidFlag=0;
-    }
-    
-    vx = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
-    vy = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
-    vs = cmps_set.OutputPID;
-    if(data.irPosition<8){
-        vx *= sys.l_pow;
-        vy *= sys.l_pow;
-    }
-    else{
-        vx *= sys.s_pow;
-        vy *= sys.s_pow;
-    }
-    
-    
-    move(vx, vy, vs);
-    
-    if(sys.MotorFlag==1){
-        tx_motor();
-        sys.MotorFlag=0;
-    }
-    return;
-}
-void modeDebug2(void){//LineRestoringForce
-    int vx,vy,vs;
-    uint8_t LineStop[2];
-    int LineForce[2];
-    if(sys.IrFlag==1){
-        ReadIr();
-        sys.IrFlag=0;
-    }
-    if(sys.PidFlag==1){
-        PidUpdate();
-        sys.PidFlag=0;
-    }
-    vx = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
-    vy = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
-    vs = cmps_set.OutputPID;
-    if(data.irPosition<8){
-        vx *= sys.l_pow;
-        vy *= sys.l_pow;
-    }
-    else{
-        vx *= sys.s_pow;
-        vy *= sys.s_pow;
-    }
-    
-    
-    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))));
-    
-    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
-    );
-    if(sys.MotorFlag==1){
-        tx_motor();
-        sys.MotorFlag=0;
-    }
-    return;
-}
-void modeDebug3(void){//movesum
-    int vx,vy,vs;
-    uint8_t LineStop[2];
-    static uint8_t moveLnFlag[4]={1, 1, 1, 1};
-    //static int moveLnlog[4];
-    if(sys.IrFlag==1){
-        ReadIr();
-        sys.IrFlag=0;
-    }
-    if(sys.PidFlag==1){
-        PidUpdate();
-        sys.PidFlag=0;
-    }
-    vx = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
-    vy = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
-    vs = cmps_set.OutputPID;
-    if(data.irPosition<8){
-        vx *= sys.l_pow;
-        vy *= sys.l_pow;
-    }
-    else{
-        vx *= sys.s_pow;
-        vy *= sys.s_pow;
-    }
-    
-    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
-    );
-    if(sys.MotorFlag==1){
-        tx_motor();
-        sys.MotorFlag=0;
-    }
-    return;
-}
-void modeDebug4(void){//solenoid
-    if(sys.KickFlag==1){
-        DriveSolenoid();
-    }
-    return;
-}
-void modeDebug5(void){//cmpsTest
-    if(sys.IrFlag==1){
-        ReadIr();
-        sys.IrFlag=0;
-    }
-    if(sys.PidFlag==1){
-        PidUpdate();
-        sys.PidFlag=0;
-    }
-    wait(2);
-    kicker = 1;
-    wait(.5);
-    kicker = 0;
-    wait(.5);
-    //move(0,0,10);
-    //LED[0]=1;
-    //LED[3]=1;
-    if(sys.MotorFlag==1){
-        tx_motor();
-        sys.MotorFlag=0;
-    }
-    
-    return;
-}
+