File content as of revision 21:378470320524:

#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)+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 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(speed[1],-speed[0],-speed[2],-speed[3]);//右後左無
    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);

    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){
    kicker=1;
    Solenoid_timeout.attach(&SolenoidSignal, .5);
}
void SolenoidSignal(void){
    kicker=0;
}
//ball
void AdmitBallHold(void){
    /*if(data.ball==1){
        sys.BallHoldFlag=1;
    }
    else if(data.ball==0){
        sys.BallHoldJudgeFlag=0;
        sys.BallHoldFlag=0;
    }*/
}
void GiveUpBallHold(void){
    /*if(data.ball==1){
        sys.BallHoldFlag=1;
    }
    else if(data.ball==0){
        sys.BallHoldJudgeFlag=0;
        sys.BallHoldFlag=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;
}