自分用4オムニ
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 20 2022 00:45:50 by
1.7.2