File content as of revision 45:c23f25c00d0d:

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

//pid&cmps
void PidUpdate(void)
{   
    pid.setSetPoint(REFERENCE + data.FrontDeg); 
    data.cmps = hmc.sample()/10.0;
    data.InputPID = fmod((data.cmps+data.CmpsDiff+720.0), 360.0);//0<data.cmps<359.0

    pid.setProcessValue(data.InputPID);
    data.OutputPID = -pid.compute();
}
void ValidPidUpdate(void){
    if(data.PidFlag==0){
        data.PidFlag=1;
    }
}
//motor

void ValidTx_motor(void){
    if(data.MotorFlag==0){
        data.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];
    }
    
    data.motorlog[X_AXIS] += vx;
    data.motorlog[Y_AXIS] += vy;
}
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);
}
void move_mouse(int32_t point[2], int vs){//三輪オムニの移動（マウスでの直交座標指定）
    int vx,vy;
    vx = (point[0] - data.mouse[0])/10000;
    vy = (point[1] - data.mouse[1])/10000;
    move(vx, vy, vs);
}
//solenoid
void AvailableSolenoid(void){
    if(data.KickFlag==0){
        data.KickFlag = 1;
    }
}
void DriveSolenoid(void){
    data.KickFlag = 0;
    kicker=1;
    Solenoid_timeout.attach(&SolenoidSignal, .5);
}
void SolenoidSignal(void){
    kicker=0;
}