File content as of revision 3:5b1a26c34fdd:

#include "mbed.h"
#include "TetraOmni.h"

#define PI 3.141592653589793

//引数は(直進の速度比率,回転の速度比率,モータの最大RPS,擬似目標値の一度の増減rps値)
TetraOmni::TetraOmni(double MoveRatio,double RotationRatio,double MaxRPS,double rps_FakeDiff)
{
    _MaxRPS = MaxRPS;
    _MoveRatio = MoveRatio/(MoveRatio + RotationRatio);
    _RotationRatio = RotationRatio/(MoveRatio + RotationRatio);
    _rps_FakeDiff = rps_FakeDiff;
    FlameSize = 1;
    TGTRPS_SwitchingStage=75;
    SpeedStageSetting(33,67,100);
    init();
}



//ホイールの擬似目標値を返す．IDは1〜4
double TetraOmni::Wheel_FakeTGTRPS(int Wheel_ID)
{
    return FakeTargetRPS[Wheel_ID];
}


//ホイールの正しい目標値を返す．IDは1〜4
double TetraOmni::Wheel_TrueTGTRPS(int Wheel_ID)
{
    return _WheelTGTRPS[Wheel_ID];
}

//コントローラの値からホイール速度を計算
//引数は(レバー入力の有無,レバー角度(deg),ジャイロ角度,回転入力の有無と方向,速度段階)
//回転方向については2進数で00=not,01=right,10=left回転
//speedについては00=middle,01=low,10=highの三段階
void TetraOmni::wheelTGTRPScalc(bool LeverState,double LeverAngle,double currentYawAngle,int RotState,int _SpeedState)
{
    //ホイール目標値リセット
    for(int i =0; i<4; i++)_WheelTGTRPS[i]=0;

    //回転処理
    if(RotState==0b10) {
        for(int i =0; i<4; i++)_WheelTGTRPS[i]+=1*_RotationRatio*FlameSize;
    } else if(RotState==0b01) {
        for(int i =0; i<4; i++)_WheelTGTRPS[i]+=-1*_RotationRatio*FlameSize;
    }

    //レバー入力あり
    if(LeverState) {
        //データを整える
        LeverAngle = LeverAngle * PI / 180;//引数のdegデータをradに変換
        currentYawAngle=currentYawAngle * PI / 180;//同上
        double Move_Vx = cos(LeverAngle),Move_Vy = sin(LeverAngle);//レバー入力方向を2軸に分解
        //複数回行う計算をここでまとめる
        double WheelID_1and3_calcresult = _MoveRatio*(cos(PI/4 - currentYawAngle)*Move_Vx - sin(PI/4 - currentYawAngle)*Move_Vy);
        double WheelID_2and4_calcresult = _MoveRatio*(sin(PI/4 - currentYawAngle)*Move_Vx + cos(PI/4 - currentYawAngle)*Move_Vy);

        //各ホイールの目標rpsを計算
        _WheelTGTRPS[0] += WheelID_1and3_calcresult;
        _WheelTGTRPS[1] += WheelID_2and4_calcresult;
        _WheelTGTRPS[2] -= WheelID_1and3_calcresult;
        _WheelTGTRPS[3] -= WheelID_2and4_calcresult;
    }

    //速度段階調整
    for(int i =0; i<4; i++)_WheelTGTRPS[i] *= _SpeedStage[_SpeedState];
}

//ホイールの擬似目標RPS値の計算
void TetraOmni::FakeRPScalc()
{
    for(int i=0; i<4; i++) {
        double checkdiff = _WheelTGTRPS[i]-FakeTargetRPS[i];
        if(fabs(_preWheelTGTRPS[i] - _WheelTGTRPS[i])>1)diff[i]=checkdiff;
        if(fabs(checkdiff)>_rps_FakeDiff && fabs(FakeTargetRPS[i]) < fabs(_WheelTGTRPS[i]) )
            FakeTargetRPS[i] += fabs(diff[i]/TGTRPS_SwitchingStage)*((diff[i]>0)-(diff[i]<0));//(diff>0)-(diff<0)は正負判定
        else FakeTargetRPS[i] = _WheelTGTRPS[i];

        _preWheelTGTRPS[i] = _WheelTGTRPS[i];
    }
}

void TetraOmni::init()
{
    for(int i =0; i<4; i++) _WheelTGTRPS[i]=0;
    for(int i =0; i<4; i++) _preWheelTGTRPS[i]=0;
    for(int i =0; i<4; i++)FakeTargetRPS[i]=0;
    _StandardYaw = 0;
    for(int i =0; i<4; i++)diff[i] = 0;
}

//速度段階の設定.0~100の値．初期設定は33,67,100の状態
void TetraOmni::SpeedStageSetting(double low,double middle,double high)
{
    _SpeedStage[0] = middle * _MaxRPS/100;
    _SpeedStage[1] = low * _MaxRPS/100;
    _SpeedStage[2] = high * _MaxRPS/100;
}