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:
- 59:7fdf1168c5c3
- Parent:
- 58:ac3d182732c9
diff -r ac3d182732c9 -r 7fdf1168c5c3 TVDCTRL.cpp
--- a/TVDCTRL.cpp Tue Jan 09 10:09:01 2018 +0000
+++ b/TVDCTRL.cpp Tue Feb 27 04:35:43 2018 +0000
@@ -460,7 +460,7 @@
int distributeTorque(float steeringWheelAngle)
{
- float deadband = 0.0;
+ const float deadband = M_PI * 5.0 / 180.0;
double steeringSign = 1.0;
int disTrq = 0;
@@ -603,10 +603,10 @@
float limitRate;
float currentVelocity = getVelocity() * 3.6f; //km/hで車速取得
- if(currentVelocity < 15.0f)
+ if(currentVelocity < 5.0f)
limitRate = 0.0f;
else if(currentVelocity < 30.0f)
- limitRate = (currentVelocity - 15.0f) / (30.0f - 15.0f);
+ limitRate = (currentVelocity - 5.0f) / (30.0f - 5.0f);
else
limitRate = 1.0f;
@@ -620,9 +620,9 @@
{
//------------------------------
//Constant variables
- const double TGT_SLIP_RATIO = 0.3;
+ const double TGT_SLIP_RATIO = 0.2;
const double TGT_VEHICLE_SPEED = 20.0 / 3.6; //この車速相当の回転数まで空転を制限する
- const double CTRL_START_VEHICLE_SPEED = 10.0 / 3.6; //トラコンONとなる車速[m/s]
+ const double CTRL_START_VEHICLE_SPEED = 5.0 / 3.6; //トラコンONとなる車速[m/s]
const double STRAIGHT_ROAD_THRESHOLD = 200.0; //直進判定閾値
const double KP = 20.0;
const double KI = 0.0;
@@ -632,7 +632,6 @@
double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE}; //実数値へ変換
double outputTrq[2] = {0.0};
double steeringAngle, R, Vb, Vbw, Vw, wheelRpsRR, wheelRpsRL;
- static double lastMotorTrq[2] = {0.0}; //前回の出力トルク
static double e[2][2] = {0.0}; //1つ前の偏差まで保持
static double integral = 0.0;
double tmpTcsMotorTrq; //
@@ -647,9 +646,10 @@
// Vb = 15.0/3.6;
R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.001); //理論旋回半径取得
-
+
if(myAbs(R) < 1.0) //旋回半径の最小値
R = mySign(steeringAngle) * 1.0; //旋回半径最小値設定
+
if(myAbs(R) > STRAIGHT_ROAD_THRESHOLD) { //直進判定
R = mySign(steeringAngle) * STRAIGHT_ROAD_THRESHOLD; //旋回半径設定
straightFlag = true; //直進判定フラグON
@@ -681,37 +681,35 @@
}
integral = integral + (e[rlFlag][0] + e[rlFlag][1]);
- lastMotorTrq[rlFlag] = KP * e[rlFlag][0] + KI*integral + KD*(e[rlFlag][1] - e[rlFlag][0]); //PIDコントローラへどーーーん
+ outputTrq[rlFlag] = KP * e[rlFlag][0] + KI*integral + KD*(e[rlFlag][1] - e[rlFlag][0]); //PIDコントローラへどーーーん
e[rlFlag][1] = e[rlFlag][0];
- }
-// printf("%f %f\r\n", e[0][0], e[1][0]);
+ outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], outputTrq[rlFlag]); //ちっさい方を出力トルクとして採用
- outputTrq[0] = myMin(reqMotorTrq[0], lastMotorTrq[0]); //ちっさい方を出力トルクとして採用
- outputTrq[1] = myMin(reqMotorTrq[1], lastMotorTrq[1]); //ちっさい方を出力トルクとして採用
-
- for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
if(outputTrq[rlFlag] > 45.0)
outputTrq[rlFlag] = 45.0;
- if(outputTrq[rlFlag] < -10.0)
- outputTrq[rlFlag] = -10.0;
+ if(outputTrq[rlFlag] < -15.0)
+ outputTrq[rlFlag] = -15.0;
}
- if(myAbs(e[0][0]) > myAbs(e[1][1])) { //右輪の空転が大きい場合
- tmpTcsMotorTrq = outputTrq[1] * (ALPHA-1.0)/(ALPHA+1.0);
- if(tmpTcsMotorTrq > outputTrq[0]) //空転輪側車軸トルクを下げ過ぎていた場合
- if(reqMotorTrq[0] > tmpTcsMotorTrq)
- outputTrq[0] = tmpTcsMotorTrq; //右車軸トルクを0へ制限
- } else if(myAbs(e[0][0]) < myAbs(e[1][1])) { //左輪の空転が大きい場合
- tmpTcsMotorTrq = outputTrq[0] * (ALPHA-1.0)/(ALPHA+1.0);
- if(tmpTcsMotorTrq > outputTrq[1]) //空転輪側車軸トルクを下げ過ぎていた場合
- if(reqMotorTrq[1] > tmpTcsMotorTrq)
- outputTrq[1] = tmpTcsMotorTrq; //左車軸トルクを0へ制限
+ if(outputTrq[0] < outputTrq[1]) { //右輪のモータトルクが小さい場合(空転輪)
+ tmpTcsMotorTrq = outputTrq[1] * ZERO_AXLE_TORQUE_FACTOR; //右車軸トルクが0となるモータトルクを演算
+ if(tmpTcsMotorTrq > outputTrq[0]) //空転輪側車軸トルクを下げ過ぎていた場合
+ outputTrq[0] = tmpTcsMotorTrq; //右車軸トルクを0へ設定
+ if(reqMotorTrq[0] < outputTrq[0]) //トラコン演算前のモータトルクを超過した場合
+ outputTrq[0] = reqMotorTrq[0]; //駆動力を増やし過ぎないように制限
+ } else if(outputTrq[0] > outputTrq[1]) { //左輪のモータトルクが小さい場合(空転輪)
+ tmpTcsMotorTrq = outputTrq[0] * ZERO_AXLE_TORQUE_FACTOR; //左車軸トルクが0となるモータトルクを演算
+ if(tmpTcsMotorTrq > outputTrq[1]) //空転輪側車軸トルクを下げ過ぎていた場合
+ outputTrq[1] = tmpTcsMotorTrq; //左車軸トルクを0へ設定
+ if(reqMotorTrq[1] < outputTrq[1]) //トラコン演算前のモータトルクを超過した場合
+ outputTrq[1] = reqMotorTrq[1]; //駆動力を増やし過ぎないように制限
}
for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
i_motorTrq[rlFlag] = (int)(outputTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5);
}
+// printf("%f %f\r\n", outputTrq[0], outputTrq[1]);
}
void driveTVD(int TVDmode, bool isRedyToDrive)
@@ -726,7 +724,7 @@
loadSteerAngle(); //舵角更新
loadRps(); //従動輪・モータ回転数更新
-// printf("%f %f\r\n", getWheelRps(RR_MOTOR), getWheelRps(RL_MOTOR));
+ printf("%f %f\r\n", getWheelRps(RR_MOTOR), getWheelRps(RL_MOTOR));
if(isRedyToDrive && isBrakeOn())
readyToDriveFlag = 0;
@@ -750,7 +748,7 @@
if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
requestTorque = 0;
- distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0f); //片モーターのトルク分配量計算
+ distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) * limitTorqueDistribution() / 2.0f); //片モーターのトルク分配量計算
disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f); //微分制御
distributionTrq = 0;