File content as of revision 18:3a42a931c95a:

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

//pid&cmps
void PidUpdate(void)
{
    
    //pid.setSetPoint(REFERENCE + cmps_set.FrontDeg); 
    cmps_set.cmps = hmc.sample()/10.0;
    cmps_set.InputPID = fmod((cmps_set.cmps+cmps_set.CmpsDiff+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 FrontHere(void){
    //ReadCmps();
    //cmps_set.CmpsInitialValue = cmps_set.cmps;
    
    cmps_set.CmpsDiff = (REFERENCE-cmps_set.FrontDeg) - cmps_set.cmps;
}
void HmcReset(void){
    hmc_reset=HMC_RST;
    wait_us(100);
    hmc_reset=HMC_RUN;
}
void TurnAttack(void){
    cmps_set.FrontDeg += cmps_set.AtkDeg;
    FrontHere();
    //ResetDegree();
}
void ValidPidUpdate(void){
    if(sys.PidFlag==0){
        sys.PidFlag=1;
    }
}
//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 AvailableSolenoid(void){
    if(sys.KickFlag==0){
        sys.KickFlag = 1;
    }
}
void DriveSolenoid(void){
    sys.KickFlag = 0;
    kicker=1;
    Solenoid_timeout.attach(&SolenoidSignal, .5);
}
void SolenoidSignal(void){
    kicker=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;
}