yosuke kitajima / TetraOmni_verKi

自分用4オムニ

Dependents:   FourOmniMecha

TetraOmni.cpp@3:5b1a26c34fdd, 2020-03-23 (annotated)

Committer:
ttrist
Date:
Mon Mar 23 08:04:36 2020 +0000
Revision:
3:5b1a26c34fdd
Parent:
2:7c3745bfc8ae 
don't worry

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ttrist 1:3783de4eeaff 1 #include "mbed.h"
ttrist 0:9e9bd3849402 2 #include "TetraOmni.h"
ttrist 0:9e9bd3849402 3
ttrist 0:9e9bd3849402 4 #define PI 3.141592653589793
ttrist 0:9e9bd3849402 5
ttrist 2:7c3745bfc8ae 6 //引数は(直進の速度比率,回転の速度比率,モータの最大RPS,擬似目標値の一度の増減rps値)
ttrist 2:7c3745bfc8ae 7 TetraOmni::TetraOmni(double MoveRatio,double RotationRatio,double MaxRPS,double rps_FakeDiff)
ttrist 2:7c3745bfc8ae 8 {
ttrist 2:7c3745bfc8ae 9 _MaxRPS = MaxRPS;
ttrist 2:7c3745bfc8ae 10 _MoveRatio = MoveRatio/(MoveRatio + RotationRatio);
ttrist 2:7c3745bfc8ae 11 _RotationRatio = RotationRatio/(MoveRatio + RotationRatio);
ttrist 2:7c3745bfc8ae 12 _rps_FakeDiff = rps_FakeDiff;
ttrist 2:7c3745bfc8ae 13 FlameSize = 1;
ttrist 3:5b1a26c34fdd 14 TGTRPS_SwitchingStage=75;
ttrist 2:7c3745bfc8ae 15 SpeedStageSetting(33,67,100);
ttrist 2:7c3745bfc8ae 16 init();
ttrist 2:7c3745bfc8ae 17 }
ttrist 0:9e9bd3849402 18
ttrist 0:9e9bd3849402 19
ttrist 0:9e9bd3849402 20
ttrist 2:7c3745bfc8ae 21 //ホイールの擬似目標値を返す．IDは1〜4
ttrist 2:7c3745bfc8ae 22 double TetraOmni::Wheel_FakeTGTRPS(int Wheel_ID)
ttrist 2:7c3745bfc8ae 23 {
ttrist 2:7c3745bfc8ae 24 return FakeTargetRPS[Wheel_ID];
ttrist 2:7c3745bfc8ae 25 }
ttrist 0:9e9bd3849402 26
ttrist 0:9e9bd3849402 27
ttrist 2:7c3745bfc8ae 28 //ホイールの正しい目標値を返す．IDは1〜4
ttrist 2:7c3745bfc8ae 29 double TetraOmni::Wheel_TrueTGTRPS(int Wheel_ID)
ttrist 2:7c3745bfc8ae 30 {
ttrist 2:7c3745bfc8ae 31 return _WheelTGTRPS[Wheel_ID];
ttrist 2:7c3745bfc8ae 32 }
ttrist 2:7c3745bfc8ae 33
ttrist 2:7c3745bfc8ae 34 //コントローラの値からホイール速度を計算
ttrist 2:7c3745bfc8ae 35 //引数は(レバー入力の有無,レバー角度(deg),ジャイロ角度,回転入力の有無と方向,速度段階)
ttrist 2:7c3745bfc8ae 36 //回転方向については2進数で00=not,01=right,10=left回転
ttrist 2:7c3745bfc8ae 37 //speedについては00=middle,01=low,10=highの三段階
ttrist 2:7c3745bfc8ae 38 void TetraOmni::wheelTGTRPScalc(bool LeverState,double LeverAngle,double currentYawAngle,int RotState,int _SpeedState)
ttrist 2:7c3745bfc8ae 39 {
ttrist 2:7c3745bfc8ae 40 //ホイール目標値リセット
ttrist 2:7c3745bfc8ae 41 for(int i =0; i<4; i++)_WheelTGTRPS[i]=0;
ttrist 2:7c3745bfc8ae 42
ttrist 2:7c3745bfc8ae 43 //回転処理
ttrist 2:7c3745bfc8ae 44 if(RotState==0b10) {
ttrist 2:7c3745bfc8ae 45 for(int i =0; i<4; i++)_WheelTGTRPS[i]+=1*_RotationRatio*FlameSize;
ttrist 2:7c3745bfc8ae 46 } else if(RotState==0b01) {
ttrist 2:7c3745bfc8ae 47 for(int i =0; i<4; i++)_WheelTGTRPS[i]+=-1*_RotationRatio*FlameSize;
ttrist 0:9e9bd3849402 48 }
ttrist 0:9e9bd3849402 49
ttrist 2:7c3745bfc8ae 50 //レバー入力あり
ttrist 2:7c3745bfc8ae 51 if(LeverState) {
ttrist 2:7c3745bfc8ae 52 //データを整える
ttrist 2:7c3745bfc8ae 53 LeverAngle = LeverAngle * PI / 180;//引数のdegデータをradに変換
ttrist 2:7c3745bfc8ae 54 currentYawAngle=currentYawAngle * PI / 180;//同上
ttrist 2:7c3745bfc8ae 55 double Move_Vx = cos(LeverAngle),Move_Vy = sin(LeverAngle);//レバー入力方向を2軸に分解
ttrist 2:7c3745bfc8ae 56 //複数回行う計算をここでまとめる
ttrist 2:7c3745bfc8ae 57 double WheelID_1and3_calcresult = _MoveRatio*(cos(PI/4 - currentYawAngle)*Move_Vx - sin(PI/4 - currentYawAngle)*Move_Vy);
ttrist 2:7c3745bfc8ae 58 double WheelID_2and4_calcresult = _MoveRatio*(sin(PI/4 - currentYawAngle)*Move_Vx + cos(PI/4 - currentYawAngle)*Move_Vy);
ttrist 0:9e9bd3849402 59
ttrist 2:7c3745bfc8ae 60 //各ホイールの目標rpsを計算
ttrist 2:7c3745bfc8ae 61 _WheelTGTRPS[0] += WheelID_1and3_calcresult;
ttrist 2:7c3745bfc8ae 62 _WheelTGTRPS[1] += WheelID_2and4_calcresult;
ttrist 2:7c3745bfc8ae 63 _WheelTGTRPS[2] -= WheelID_1and3_calcresult;
ttrist 2:7c3745bfc8ae 64 _WheelTGTRPS[3] -= WheelID_2and4_calcresult;
ttrist 0:9e9bd3849402 65 }
ttrist 0:9e9bd3849402 66
ttrist 2:7c3745bfc8ae 67 //速度段階調整
ttrist 2:7c3745bfc8ae 68 for(int i =0; i<4; i++)_WheelTGTRPS[i] *= _SpeedStage[_SpeedState];
ttrist 2:7c3745bfc8ae 69 }
ttrist 0:9e9bd3849402 70
ttrist 2:7c3745bfc8ae 71 //ホイールの擬似目標RPS値の計算
ttrist 2:7c3745bfc8ae 72 void TetraOmni::FakeRPScalc()
ttrist 2:7c3745bfc8ae 73 {
ttrist 2:7c3745bfc8ae 74 for(int i=0; i<4; i++) {
ttrist 2:7c3745bfc8ae 75 double checkdiff = _WheelTGTRPS[i]-FakeTargetRPS[i];
ttrist 2:7c3745bfc8ae 76 if(fabs(_preWheelTGTRPS[i] - _WheelTGTRPS[i])>1)diff[i]=checkdiff;
ttrist 2:7c3745bfc8ae 77 if(fabs(checkdiff)>_rps_FakeDiff && fabs(FakeTargetRPS[i]) < fabs(_WheelTGTRPS[i]) )
ttrist 2:7c3745bfc8ae 78 FakeTargetRPS[i] += fabs(diff[i]/TGTRPS_SwitchingStage)*((diff[i]>0)-(diff[i]<0));//(diff>0)-(diff<0)は正負判定
ttrist 2:7c3745bfc8ae 79 else FakeTargetRPS[i] = _WheelTGTRPS[i];
ttrist 0:9e9bd3849402 80
ttrist 2:7c3745bfc8ae 81 _preWheelTGTRPS[i] = _WheelTGTRPS[i];
ttrist 1:3783de4eeaff 82 }
ttrist 2:7c3745bfc8ae 83 }
ttrist 0:9e9bd3849402 84
ttrist 2:7c3745bfc8ae 85 void TetraOmni::init()
ttrist 2:7c3745bfc8ae 86 {
ttrist 2:7c3745bfc8ae 87 for(int i =0; i<4; i++) _WheelTGTRPS[i]=0;
ttrist 2:7c3745bfc8ae 88 for(int i =0; i<4; i++) _preWheelTGTRPS[i]=0;
ttrist 2:7c3745bfc8ae 89 for(int i =0; i<4; i++)FakeTargetRPS[i]=0;
ttrist 2:7c3745bfc8ae 90 _StandardYaw = 0;
ttrist 2:7c3745bfc8ae 91 for(int i =0; i<4; i++)diff[i] = 0;
ttrist 2:7c3745bfc8ae 92 }
ttrist 2:7c3745bfc8ae 93
ttrist 2:7c3745bfc8ae 94 //速度段階の設定.0~100の値．初期設定は33,67,100の状態
ttrist 2:7c3745bfc8ae 95 void TetraOmni::SpeedStageSetting(double low,double middle,double high)
ttrist 2:7c3745bfc8ae 96 {
ttrist 2:7c3745bfc8ae 97 _SpeedStage[0] = middle * _MaxRPS/100;
ttrist 2:7c3745bfc8ae 98 _SpeedStage[1] = low * _MaxRPS/100;
ttrist 2:7c3745bfc8ae 99 _SpeedStage[2] = high * _MaxRPS/100;
ttrist 2:7c3745bfc8ae 100 }