File content as of revision 9:c966191926c5:

#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+720.0), 360.0);//0<cmps_set.cmps<359.0

    pid.setProcessValue(cmps_set.InputPID);
    cmps_set.OutputPID = -pid.compute();
}
void FrontHere(void){
    ReadCmps();
    cmps_set.CmpsInitialValue = cmps_set.cmps;
    
    cmps_set.CmpsDiff = (REFERENCE-cmps_set.FrontDeg) - cmps_set.cmps;
}
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] = 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];
    }
}
void move_rectan(int vx, int vy, int vs){//三輪オムニの移動（直交座標指定）
    move(vx, vy, vs);
}
void move_polar(int degree, int power, int vs){//三輪オムニの移動（極座標指定）
    int vx, vy, deg;
    deg = (degree+5)/10;
    vx = power*sin(DEG2RAD(deg));
    vy = power*cos(DEG2RAD(deg));
    move(vx, vy, vs);
}
//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;
}
//math
uint8_t GetBit(uint8_t n, uint8_t bit){
    return (n>>(bit-1))%2;
}