自分用4オムニ

Dependents:   FourOmniMecha

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers TetraOmni.cpp Source File

TetraOmni.cpp

00001 #include "mbed.h"
00002 #include "TetraOmni.h"
00003 
00004 #define PI 3.141592653589793
00005 
00006 //引数は(直進の速度比率,回転の速度比率,モータの最大RPS,擬似目標値の一度の増減rps値)
00007 TetraOmni::TetraOmni(double MoveRatio,double RotationRatio,double MaxRPS,double rps_FakeDiff)
00008 {
00009     _MaxRPS = MaxRPS;
00010     _MoveRatio = MoveRatio/(MoveRatio + RotationRatio);
00011     _RotationRatio = RotationRatio/(MoveRatio + RotationRatio);
00012     _rps_FakeDiff = rps_FakeDiff;
00013     FlameSize = 1;
00014     TGTRPS_SwitchingStage=75;
00015     SpeedStageSetting(33,67,100);
00016     init();
00017 }
00018 
00019 
00020 
00021 //ホイールの擬似目標値を返す.IDは1〜4
00022 double TetraOmni::Wheel_FakeTGTRPS(int Wheel_ID)
00023 {
00024     return FakeTargetRPS[Wheel_ID];
00025 }
00026 
00027 
00028 //ホイールの正しい目標値を返す.IDは1〜4
00029 double TetraOmni::Wheel_TrueTGTRPS(int Wheel_ID)
00030 {
00031     return _WheelTGTRPS[Wheel_ID];
00032 }
00033 
00034 //コントローラの値からホイール速度を計算
00035 //引数は(レバー入力の有無,レバー角度(deg),ジャイロ角度,回転入力の有無と方向,速度段階)
00036 //回転方向については2進数で00=not,01=right,10=left回転
00037 //speedについては00=middle,01=low,10=highの三段階
00038 void TetraOmni::wheelTGTRPScalc(bool LeverState,double LeverAngle,double currentYawAngle,int RotState,int _SpeedState)
00039 {
00040     //ホイール目標値リセット
00041     for(int i =0; i<4; i++)_WheelTGTRPS[i]=0;
00042 
00043     //回転処理
00044     if(RotState==0b10) {
00045         for(int i =0; i<4; i++)_WheelTGTRPS[i]+=1*_RotationRatio*FlameSize;
00046     } else if(RotState==0b01) {
00047         for(int i =0; i<4; i++)_WheelTGTRPS[i]+=-1*_RotationRatio*FlameSize;
00048     }
00049 
00050     //レバー入力あり
00051     if(LeverState) {
00052         //データを整える
00053         LeverAngle = LeverAngle * PI / 180;//引数のdegデータをradに変換
00054         currentYawAngle=currentYawAngle * PI / 180;//同上
00055         double Move_Vx = cos(LeverAngle),Move_Vy = sin(LeverAngle);//レバー入力方向を2軸に分解
00056         //複数回行う計算をここでまとめる
00057         double WheelID_1and3_calcresult = _MoveRatio*(cos(PI/4 - currentYawAngle)*Move_Vx - sin(PI/4 - currentYawAngle)*Move_Vy);
00058         double WheelID_2and4_calcresult = _MoveRatio*(sin(PI/4 - currentYawAngle)*Move_Vx + cos(PI/4 - currentYawAngle)*Move_Vy);
00059 
00060         //各ホイールの目標rpsを計算
00061         _WheelTGTRPS[0] += WheelID_1and3_calcresult;
00062         _WheelTGTRPS[1] += WheelID_2and4_calcresult;
00063         _WheelTGTRPS[2] -= WheelID_1and3_calcresult;
00064         _WheelTGTRPS[3] -= WheelID_2and4_calcresult;
00065     }
00066 
00067     //速度段階調整
00068     for(int i =0; i<4; i++)_WheelTGTRPS[i] *= _SpeedStage[_SpeedState];
00069 }
00070 
00071 //ホイールの擬似目標RPS値の計算
00072 void TetraOmni::FakeRPScalc()
00073 {
00074     for(int i=0; i<4; i++) {
00075         double checkdiff = _WheelTGTRPS[i]-FakeTargetRPS[i];
00076         if(fabs(_preWheelTGTRPS[i] - _WheelTGTRPS[i])>1)diff[i]=checkdiff;
00077         if(fabs(checkdiff)>_rps_FakeDiff && fabs(FakeTargetRPS[i]) < fabs(_WheelTGTRPS[i]) )
00078             FakeTargetRPS[i] += fabs(diff[i]/TGTRPS_SwitchingStage)*((diff[i]>0)-(diff[i]<0));//(diff>0)-(diff<0)は正負判定
00079         else FakeTargetRPS[i] = _WheelTGTRPS[i];
00080 
00081         _preWheelTGTRPS[i] = _WheelTGTRPS[i];
00082     }
00083 }
00084 
00085 void TetraOmni::init()
00086 {
00087     for(int i =0; i<4; i++) _WheelTGTRPS[i]=0;
00088     for(int i =0; i<4; i++) _preWheelTGTRPS[i]=0;
00089     for(int i =0; i<4; i++)FakeTargetRPS[i]=0;
00090     _StandardYaw = 0;
00091     for(int i =0; i<4; i++)diff[i] = 0;
00092 }
00093 
00094 //速度段階の設定.0~100の値.初期設定は33,67,100の状態
00095 void TetraOmni::SpeedStageSetting(double low,double middle,double high)
00096 {
00097     _SpeedStage[0] = middle * _MaxRPS/100;
00098     _SpeedStage[1] = low * _MaxRPS/100;
00099     _SpeedStage[2] = high * _MaxRPS/100;
00100 }