File content as of revision 3:5b1a26c34fdd:

#ifndef TetraOmni_h
#define TetraOmni_h

#include "mbed.h"

class TetraOmni
{
public:
    //引数は(直進の速度比率,回転の速度比率,モータの最大RPS,擬似目標値の一度の増減rps値)
    TetraOmni(double MoveRatio,double RotationRatio,double MaxRPS,double rps_FakeDiff);



    //ホイールの擬似目標値を返す．IDは1〜4
    double Wheel_FakeTGTRPS(int Wheel_ID);
    


    //ホイールの正しい目標値を返す．IDは1〜4
    double Wheel_TrueTGTRPS(int Wheel_ID);

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

    //ホイールの擬似目標RPS値の計算
    void FakeRPScalc();

    void init();

    //速度段階の設定.0~100の値．初期設定は33,67,100の状態
    void SpeedStageSetting(double low,double middle,double high);

    double TGTRPS_SwitchingStage;  //デフォは10．数値は1以上にする
    double FlameSize;  //基本は1だが機体の回転速度が早すぎると思ったら下げればいい

private:
    double _MoveRatio,_RotationRatio;
    double _MaxRPS;
    double _StandardYaw;
    double _rps_FakeDiff;
    double _WheelTGTRPS[4],_preWheelTGTRPS[4];
    double _SpeedStage[3];
    double FakeTargetRPS[4];
    double diff[4];
};


#endif