File content as of revision 2:635947de1583:

#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[data.irNotice][data.irPosition][IR_X];
    vy = ir_move_val[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[data.irNotice][data.irPosition][IR_X];
    vy = ir_move_val[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[data.irNotice][data.irPosition][IR_X];
    vy = ir_move_val[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;
    }
    move(0,0,10);
    LED[0]=1;
    LED[3]=1;
    if(sys.MotorFlag==1){
        tx_motor();
        sys.MotorFlag=0;
    }
    
    return;
}