File content as of revision 38:67bc78f3c0ab:

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

//pid&cmps
void PidUpdate(void)
{
    cmps_set.cmps = hmc.sample()/10.0;
    cmps_set.InputPID = 
    fmod(
        (
            cmps_set.cmps+
            (cmps_set.CmpsDiff
                -cmps_set.FrontDeg
                -cmps_set.AtkDeg*(sys.TurnAtkBlind==0)
                -cmps_set.HoldDeg*(sys.TurnHoldBlind==0)
                -cmps_set.GoalDeg*(sys.TurnDriBlind==0)
            )+
            7200.0
        ),
        360.0
    );//0<cmps_set.cmps<359.0
    pid.setProcessValue(cmps_set.InputPID);
    cmps_set.OutputPID = -pid.compute();
    /*if((abs(cmps_set.OutputPID)<PID_OUT_MIN)&&(cmps_set.OutputPID!=0)){
        if(cmps_set.OutputPID>0) cmps_set.OutputPID=PID_OUT_MIN;
        else cmps_set.OutputPID=-PID_OUT_MIN;
    }*/
}
void HmcReset(void){
    hmc_reset=HMC_RST;
    wait_us(100);
    hmc_reset=HMC_RUN;
}
void TurnAttack(void){
    cmps_set.AtkDeg = 30;
}
void DriveTurn(void){
    if(sys.TurnAtkBlind==1) return;
    if(sys.TurnStopFlag==1) return;
    
    if(
        //(data.ping[L_PING]<data.ping[R_PING])||
        ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]!=A_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
    ){
        cmps_set.AtkDeg = 150;
        if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg>0)){
            cmps_set.AtkDeg = -cmps_set.GoalDeg*2;
        }
    }
    else if(
        //(data.ping[L_PING]>data.ping[R_PING])||
         ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]!=B_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
    ){
        cmps_set.AtkDeg = -150;
        if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg<0)){
            cmps_set.AtkDeg = cmps_set.GoalDeg*2;
        }
    }
    else{
        cmps_set.AtkDeg = 0;
    }
    if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg!=0)){
        Turn_timeout.attach(&TurnSignal, .25);
        Turn_stop.attach(&ValidTurn, .5);
    }
    else{
        Turn_timeout.attach(&TurnSignal, .25);
        Turn_stop.attach(&ValidTurn, .5);
    }
    sys.TurnStopFlag=1;
    
}
void TurnSignal(void){
    cmps_set.AtkDeg = 0;
}
void ValidTurn(void){
    sys.TurnStopFlag=0;
}
/*void ValidPidUpdate(void){
    if(sys.PidFlag==0){
        sys.PidFlag=1;
    }
}*/
void FaceToFront(void){
    cmps_set.AtkDeg=0;
}
//motor
void ValidTx_motor(void){
    if(sys.MotorFlag==0){
        sys.MotorFlag=1;
    }
}
void tx_motor(){//モーターへの送信
    array2(MD_1CH*speed[1],MD_2CH*speed[0],MD_3CH*speed[2],MD_4CH*speed[3]);//右後左ド
    //array2((1)*speed[1],(1)*speed[0],(-1)*speed[2],(1)*speed[3]);//右後左ドgreen
    //array2((1)*speed[1],(1)*speed[0],(1)*speed[2],(-1)*speed[3]);//右後左ドred
    motor.printf("%s",StringFIN.c_str());
}
void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
    uint8_t i;
    double pwm[4] = {0};
    
    pwm[0] = (double)((vx) + vs);
    pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy) + vs);
    pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
    pwm[3] = (sys.dri_pow)*(sys.DribbleFlag)*(sys.DriMotorBlind==0);

    for(i = 0; i < 4; i++){
        if(pwm[i] > 100){
            pwm[i] = 100;
        } else if(pwm[i] < -100){
            pwm[i] = -100;
        }
        speed[i] = pwm[i];
    }
}
//solenoid
void DriveSolenoid(void){
    if(sys.KickBlind==1) return;
    if(sys.KickStopFlag==1) return;
    
    kicker=1;
    sys.KickStopFlag=1;
    Solenoid_timeout.attach(&SolenoidSignal, .5);
    Kick_stop.attach(&ValidSolenoid, 2.0);
}
void DriveSolenoidJudge(void){
    if(sys.DriBlind==0) return;
    if((data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
        DriveTurn();
        DriveSolenoid();
    }
}
void SolenoidSignal(void){
    kicker=0;
}
void ValidSolenoid(void){
    sys.KickStopFlag=0;
}
//ball
void JudgeBallHold(void){
    if(data.ball==1){
        sys.BallHoldFlag=1;
    }
    else if(data.ball==0){
        sys.BallHoldFlag=0;
    }
    sys.BallHoldJudgeFlag=0;
}
//line
void LineLiberate(void){
    LineKeeper = LINE_FREE;
    wait_us(10);
    LineKeeper = LINE_FIX;
}
//math
uint8_t GetBit(uint8_t n, uint8_t bit){
    return (n>>(bit-1))%2;
}