File content as of revision 27:769cb5a7ea37:

#include "mbed.h"
#include "extern.h"

//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(
        ir_move_val[data.irNotice][data.irPosition][IR_X],
        ir_move_val[data.irNotice][data.irPosition][IR_Y],
        data.OutputPID
    );*/
    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;
}
void modeAttack1(void){
    return;
}
void modeAttack2(void){
    return;
}
void modeAttack3(void){
    return;
}
void modeAttack4(void){
    return;
}
void modeAttack5(void){
    return;
}
//Debug
void modeDebug0(void){
    return;
}
void modeDebug1(void){
    return;
}
void modeDebug2(void){
    return;
}
void modeDebug3(void){
    return;
}
void modeDebug4(void){
    return;
}
void modeDebug5(void){
    return;
}