File content as of revision 31:0b0f64831771:

#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];
    //uint8_t LineBind[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];
    
    //Ir
    /*
    //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){
    double ir_x, ir_y;
    int vx,vy,vs;
    uint8_t LineDir[4];
    uint8_t LineStop[2];
    //uint8_t IrRange[4];
    //uint8_t LineBind[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];
    
    if(data.IrFlag==1){
        ReadIr();
        data.IrFlag=0;
    }
    if(data.PidFlag==1){
        PidUpdate();
        data.PidFlag=0;
    }
    vs = data.OutputPID;
    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 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;
}