2014 sift
/
TVDctrller2017_brdRev1_PandA
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
TVDctrller2017_brdRev1_ver6
by 
2014 sift
Diff: TVDCTRL.cpp
- Revision:
- 46:16f1a7a01f5f
- Parent:
- 45:8e5d35beb957
- Child:
- 47:949e6c2e69fc
--- a/TVDCTRL.cpp Wed Dec 06 08:02:28 2017 +0000
+++ b/TVDCTRL.cpp Tue Dec 12 11:37:33 2017 +0000
@@ -133,7 +133,7 @@
tmpRawApsP = (int)apsP.read_u16();
tmpRawApsS = (int)apsS.read_u16();
tmpRawBrake = (int)brake.read_u16();
-
+
//Low Pass Filter
tmpApsP = (int)(tmpRawApsP * ratioLPF_ACC_BRK + preApsP * (1.0-ratioLPF_ACC_BRK));
tmpApsS = (int)(tmpRawApsS * ratioLPF_ACC_BRK + preApsS * (1.0-ratioLPF_ACC_BRK));
@@ -247,7 +247,7 @@
int dT2;
bool preInputState;
int stopCounter;
- float preRps;
+ double preRps;
} rps_t;
//パルス数カウンタ
@@ -285,13 +285,6 @@
}
//***********************************
-////回転数構造体初期化関数
-//rps_t initRps(void)
-//{
-// rps_t initResult = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f};
-// return initResult;
-//}
-//
//RPS読み込み許可設定関数
void loadRpsISR(void)
{
@@ -301,8 +294,7 @@
//RPS読み込み関数
void loadRps(void)
{
- const rps_t INITRPS = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f}; //構造体初期化用定数
-// static rps_t rps[4] = {initRps(), initRps(), initRps(), initRps()};
+ const rps_t INITRPS = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0}; //構造体初期化用定数
static rps_t rps[4] = {INITRPS, INITRPS, INITRPS, INITRPS};
static int currentTime[4] = {0};
float pulseNumPerRev;
@@ -351,11 +343,11 @@
if(rps[i].stopCounter < 50) //低回転数時、急に0rpsと演算しないように前回値保持(設定値はだいたい)
rps[i].stopCounter++;
else
- gRps[i] = 0.0f;
+ gRps[i] = 0.0;
} else {
//RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
- gRps[i] = ((float)rps[i].counter / ((currentTime[i] + rps[i].dT1 - rps[i].dT2) / 1000000.0f)) / pulseNumPerRev;
- gRps[i] = gRps[i] * ratioLPF_RPS + (1.0f-ratioLPF_RPS)*rps[i].preRps;
+ gRps[i] = ((double)rps[i].counter / ((currentTime[i] + rps[i].dT1 - rps[i].dT2) / 1000000.0)) / pulseNumPerRev;
+ gRps[i] = gRps[i] * ratioLPF_RPS + (1.0-ratioLPF_RPS)*rps[i].preRps;
rps[i].stopCounter = 0;
}
@@ -373,22 +365,22 @@
}
//車輪RPS取得関数
-float getWheelRps(select_t position)
+double getWheelRps(select_t position)
{
- float deltaN; //左右モータ回転数差
- float aveN; //左右モータ回転数平均値
+ double deltaN; //左右モータ回転数差
+ double aveN; //左右モータ回転数平均値
if(position <= FL_WHEEL) //前輪の場合
return gRps[position]; //演算結果をそのまま適用
else { //後輪の場合,モータ回転数から速度線図に従い演算
//右車輪回転数が大きいと仮定
- aveN = ((gRps[RR_MOTOR] + gRps[RL_MOTOR]) / GEAR_RATIO) / 2.0f; //平均回転数計算
+ aveN = ((gRps[RR_MOTOR] + gRps[RL_MOTOR]) / GEAR_RATIO) / 2.0; //平均回転数計算
deltaN = ((gRps[RR_MOTOR] - gRps[RL_MOTOR]) / GEAR_RATIO) / ALPHA; //速度線図上の車輪回転数差計算
if(position == RR_MOTOR)
- return aveN + deltaN / 2.0f; //右車輪回転数
+ return aveN + deltaN / 2.0; //右車輪回転数
else
- return aveN - deltaN / 2.0f; //左車輪回転数
+ return aveN - deltaN / 2.0; //左車輪回転数
}
}
@@ -419,9 +411,9 @@
//車速取得関数[m/s]
//左右従動輪回転数の平均値から車速を演算
-float getVelocity(void)
+double getVelocity(void)
{
- return (0.5f*TIRE_DIAMETER*2.0f*M_PI*(getWheelRps(FR_WHEEL) + getWheelRps(FL_WHEEL))*0.5f);
+ return (0.5*TIRE_DIAMETER*2.0*M_PI*(getWheelRps(FR_WHEEL) + getWheelRps(FL_WHEEL))*0.5);
}
int distributeTorque_omega(float steeringWheelAngle)
@@ -507,7 +499,6 @@
return (int)(disTrq * steeringSign);
}
-
//rpm +++++ モータ回転数
//regSwitch +++++ 回生=1 力行=0
inline int calcMaxTorque(int rpm, bool regSwitch)
@@ -641,6 +632,74 @@
return limitRate;
}
+//------------------------------
+//参考文献：デジタルPI制御と離散化
+//------------------------------
+//速度アルゴリズム型PID制御器
+//lastOutput : 前回の制御出力
+//*e : 過去3つ分の偏差
+double calcPID(double lastOutput, double* e)
+{
+ //------------------------------
+ //Constant variables
+ const double KP = 1.0;
+ const double KI = 0.0;
+ const double KD = 0.0;
+ //------------------------------
+
+ return lastOutput + KP*(e[0] - e[1]) + KI*(e[1] + e[0]) + KD*(e[0] - 2*e[1] + e[2]); //PID controller
+}
+
+//--------------------------------------------------
+//参考文献：モデルを用いたトラクションコントロールの基礎研究
+//--------------------------------------------------
+//rlFlag : 0 ---> Right motor
+// 1 ---> Left motor
+//Vb : Follower wheel
+//Vb : Drive wheel
+int getTractionCtrl(int i_motorTrq, bool rlFlag = false)
+{
+ //------------------------------
+ //Constant variables
+ const double TGT_SLIP_RATIO = 0.3;
+ //------------------------------
+
+ double nowMotorTrq = i_motorTrq * LSB_MOTOR_TORQUE; //実数値へ変換
+ double steeringAngle, R, Vb, Vw;
+ static double lastMotorTrq[2] = {0}; //前回の出力トルク
+ static double e[2][3] = {0}; //3つ前の偏差まで保持
+
+ steeringAngle = (getSteerAngle() - 127)/127.0 * M_PI*STEER_RATIO; //実舵角取得
+
+ Vb = getVelocity() * cos(steeringAngle); //2輪モデルにおける車体進行方向速度取得
+
+ R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMin(myAbs(steeringAngle), 0.001); //理論旋回半径取得
+
+ if(rlFlag) {
+ Vb = Vb*(1.0 + TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s]
+ Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*getWheelRps(RR_MOTOR); //駆動輪速度[m/s]
+ } else {
+ Vb = Vb*(1.0 - TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s
+ Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*getWheelRps(RL_MOTOR); //駆動輪速度[m/s]
+ }
+
+ e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw; //現在の偏差取得(目標スリップ率における車輪速と実駆動輪速度の差分)
+
+ lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]); //PIDコントローラへどーーーん
+
+ if(lastMotorTrq[rlFlag] < 0.0) //現状、マイナストルクは無しで
+ lastMotorTrq[rlFlag] = 0.0;
+
+ e[rlFlag][2] = e[rlFlag][1];
+ e[rlFlag][1] = e[rlFlag][0];
+
+ if(lastMotorTrq[rlFlag] > nowMotorTrq) {
+ lastMotorTrq[rlFlag] = nowMotorTrq; //要求トルクより大きいのはどーーーんなのでリミット
+ }
+
+ return (int)(lastMotorTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5); //四捨五入してreturn
+}
+
void driveTVD(int TVDmode, bool isRedyToDrive)
{
int requestTorque=0; //ドライバー要求トルク
@@ -686,6 +745,9 @@
torqueRight += disTrq_omega;
torqueLeft -= disTrq_omega;
+
+ torqueRight = getTractionCtrl(torqueRight, 0);
+ torqueLeft = getTractionCtrl(torqueLeft, 1);
//現在バグあり
//アクセル全開で旋回後、舵を中立に戻していくと加速する。旋回を優先するモード