File content as of revision 29:b413b0bb07a1:

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

//Atk
void modeAttack0(void){
    double ir_x, ir_y;
    int vx,vy,vs;
    uint8_t LineDir[4];
    uint8_t LineStop[2];
    //uint8_t IrRange[4];
    if(data.IrFlag==1){
        ReadIr();
        data.IrFlag=0;
    }
    if(data.PidFlag==1){
        Line_ticker.detach();
        PidUpdate();
        Line_ticker.attach(&ReadLine, 0.005);
        data.PidFlag=0;
    }
    ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
    ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
    if(data.irPosition<8){
        ir_x *= data.l_pow;
        ir_y *= data.l_pow;
    }
    else{
        ir_x *= data.s_pow;
        ir_y *= data.s_pow;
    }
    //Lineを考慮していないIrのみの値
    vx = ir_x;
    vy = ir_y;
    //Line検出方向を調べる
    LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
    LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
    LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
    LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
    
    LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
    LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
    
    
    /*
    IrRange[A_SPOT] = (data.irPosition==19)||(data.irPosition==8)||(data.irPosition==9);
    IrRange[B_SPOT] = (data.irPosition==13)||(data.irPosition==14)||(data.irPosition==15);
    IrRange[C_SPOT] = (data.irPosition==16)||(data.irPosition==17)||(data.irPosition==18);
    IrRange[AB_SPOT] = (data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12);
    */
    /*
    IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
                    ||((6<=data.irPosition)&&(data.irPosition<=7))||((                   1)&&(data.irPosition<=1));
    IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
    IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
    IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||((                 1)&&(data.irPosition<=3));
    
    data.LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(data.LineBind[A_SPOT]==1)));
    data.LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(data.LineBind[B_SPOT]==1)));
    data.LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(data.LineBind[C_SPOT]==1)));
    data.LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(data.LineBind[AB_SPOT]==1)));
    */
    
    /*
    if(data.LineBind[A_SPOT]||data.LineBind[B_SPOT]||data.LineBind[C_SPOT]||data.LineBind[AB_SPOT]){
        //Line_timeout.attach();
        vx=(LINE_RF*2)*((-1)*data.LineBind[A_SPOT] +      data.LineBind[B_SPOT]);
        vy=(LINE_RF*2)*(     data.LineBind[C_SPOT] + (-1)*data.LineBind[AB_SPOT]);
        if((LineDir[A_SPOT]==0)||(LineDir[B_SPOT]==0)||(LineDir[C_SPOT]==0)||(LineDir[AB_SPOT]==0)){
            vx=(LINE_RF*2)*(-data.LineBind[A_SPOT] + data.LineBind[B_SPOT]);
            vy=(LINE_RF*2)*(data.LineBind[C_SPOT] - data.LineBind[AB_SPOT]);
        }
        else{
            vx=0;
            vy=0;
        }
    }*/
    /*if(data.LineBind[A_SPOT]||data.LineBind[B_SPOT]||data.LineBind[C_SPOT]||data.LineBind[AB_SPOT]){
        vx=(LINE_RF*2)*((-1.0)*data.LineBind[A_SPOT]*(LineDir[A_SPOT]==0) +        data.LineBind[B_SPOT]*(LineDir[B_SPOT]==0));
        vy=(LINE_RF*2)*(       data.LineBind[C_SPOT]*(LineDir[C_SPOT]==0) + (-1.0)*data.LineBind[AB_SPOT]*(LineDir[AB_SPOT]==0));
        
    }*/
    vs = data.OutputPID;
    /*
    //ForLineAll
    data.motorlog[X_AXIS] *= LineStop[X_AXIS];
    data.motorlog[Y_AXIS] *= LineStop[Y_AXIS];
    data.motorlog[X_AXIS] += vx;
    data.motorlog[Y_AXIS] += vy;
    */
    move(
        vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
        vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
        vs
    );
    
    if(data.MotorFlag==1){
        tx_motor();
        data.MotorFlag=0;
    }
    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){//ChaseOnly NonLine
int vx,vy,vs;
    if(data.IrFlag==1){
        ReadIr();
        data.IrFlag=0;
    }
    if(data.PidFlag==1){
        PidUpdate();
        data.PidFlag=0;
    }
    
    vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
    vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
    vs = data.OutputPID;
    if(data.irPosition<8){
        vx *= data.l_pow;
        vy *= data.l_pow;
    }
    else{
        vx *= data.s_pow;
        vy *= data.s_pow;
    }
    
    
    move(vx, vy, vs);
    
    if(data.MotorFlag==1){
        tx_motor();
        data.MotorFlag=0;
    }
    return;
}
void modeDebug2(void){//LineRestoringForce
    int vx,vy,vs;
    uint8_t LineStop[2];
    int LineForce[2];
    if(data.IrFlag==1){
        ReadIr();
        data.IrFlag=0;
    }
    if(data.PidFlag==1){
        PidUpdate();
        data.PidFlag=0;
    }
    vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
    vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
    vs = data.OutputPID;
    if(data.irPosition<8){
        vx *= data.l_pow;
        vy *= data.l_pow;
    }
    else{
        vx *= data.s_pow;
        vy *= data.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(data.MotorFlag==1){
        tx_motor();
        data.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(data.IrFlag==1){
        ReadIr();
        data.IrFlag=0;
    }
    if(data.PidFlag==1){
        PidUpdate();
        data.PidFlag=0;
    }
    vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
    vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
    vs = data.OutputPID;
    if(data.irPosition<8){
        vx *= data.l_pow;
        vy *= data.l_pow;
    }
    else{
        vx *= data.s_pow;
        vy *= data.s_pow;
    }
    
    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)&&(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(data.MotorFlag==1){
        tx_motor();
        data.MotorFlag=0;
    }
    return;
}
void modeDebug4(void){//solenoid
    if(data.KickFlag==1){
        DriveSolenoid();
    }
    return;
}
void modeDebug5(void){//cmpsTest
    if(data.IrFlag==1){
        ReadIr();
        data.IrFlag=0;
    }
    if(data.PidFlag==1){
        PidUpdate();
        data.PidFlag=0;
    }
    move(0,0,data.OutputPID);
    if(data.MotorFlag==1){
        tx_motor();
        data.MotorFlag=0;
    }
    
    return;
}