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:
- 57:02b7178cd083
- Parent:
- 56:78bd99bf995a
- Child:
- 58:ac3d182732c9
--- a/TVDCTRL.cpp Mon Jan 08 10:21:11 2018 +0000
+++ b/TVDCTRL.cpp Tue Jan 09 06:14:28 2018 +0000
@@ -622,8 +622,9 @@
//Constant variables
const double TGT_SLIP_RATIO = 0.3;
const double TGT_VEHICLE_SPEED = 20.0 / 3.6; //この車速相当の回転数まで空転を制限する
- const double CTRL_START_VEHICLE_SPEED = 0.0 / 3.6; //トラコンONとなる車速[m/s]
- const double KP = 10.0;
+ const double CTRL_START_VEHICLE_SPEED = 10.0 / 3.6; //トラコンONとなる車速[m/s]
+ const double STRAIGHT_ROAD_THRESHOLD = 200.0; //直進判定閾値
+ const double KP = 20.0;
const double KI = 0.0;
const double KD = 0.0;
//------------------------------
@@ -635,6 +636,7 @@
static double e[2][2] = {0.0}; //1つ前の偏差まで保持
static double integral = 0.0;
double tmpTcsMotorTrq; //
+ bool straightFlag = false;
wheelRpsRR = getWheelRps(RR_MOTOR);
wheelRpsRL = getWheelRps(RL_MOTOR);
@@ -642,30 +644,42 @@
steeringAngle = getSteerAngle()/127.0 * M_PI*STEER_RATIO; //実舵角取得
Vb = getVelocity(); //前輪回転方向における車速換算値
- Vb = 15.0/3.6;
+// 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) > 100.0)
- R = mySign(steeringAngle) * 100.0;
+
+ 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
+ } else {
+ straightFlag = false;
+ }
for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
+ if(straightFlag == true) {
+ Vbw = Vb; //対地車速設定[m/s]
+ } else {
+ if(rlFlag == 0) {
+ Vbw = Vb*(1.0 + TREAD/(2.0*R)); //旋回半径を考慮した対地車速設定[m/s]
+ } else {
+ Vbw = Vb*(1.0 - TREAD/(2.0*R)); //旋回半径を考慮した対地車速設定[m/s
+ }
+ }
+
if(rlFlag == 0) {
- Vbw = Vb*(1.0 + TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s]
Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRR; //駆動輪速度[m/s]
} else {
- Vbw = Vb*(1.0 - TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s
Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL; //駆動輪速度[m/s]
}
if(Vb < CTRL_START_VEHICLE_SPEED) { //対地車速が一定値より小さい場合(停車状態)
e[rlFlag][0] = TGT_VEHICLE_SPEED - Vw; //空転抑制制御
} else {
- e[rlFlag][0] = Vbw/(1.0 - TGT_SLIP_RATIO) - Vw; //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
+ e[rlFlag][0] = Vbw/(1.0 - TGT_SLIP_RATIO) - Vw; //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
}
-
+
integral = integral + (e[rlFlag][0] + e[rlFlag][1]);
lastMotorTrq[rlFlag] = KP * e[rlFlag][0] + KI*integral + KD*(e[rlFlag][1] - e[rlFlag][0]); //PIDコントローラへどーーーん
e[rlFlag][1] = e[rlFlag][0];
@@ -679,27 +693,25 @@
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] < 0.0)
+ outputTrq[rlFlag] = 0.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(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へ制限
+ }
for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
-// if(e[rlFlag][0] < 0.0) //駆動時のみ制御ON
- i_motorTrq[rlFlag] = (int)(outputTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5);
+ 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)