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:
- 48:45614aa0db15
- Parent:
- 47:949e6c2e69fc
- Child:
- 49:c97740265324
diff -r 949e6c2e69fc -r 45614aa0db15 TVDCTRL.cpp
--- a/TVDCTRL.cpp Wed Dec 13 09:20:32 2017 +0000
+++ b/TVDCTRL.cpp Fri Dec 15 04:02:58 2017 +0000
@@ -633,87 +633,92 @@
//--------------------------------------------------
//参考文献：モデルを用いたトラクションコントロールの基礎研究
//--------------------------------------------------
-//rlFlag : 0 ---> Right motor
-// 1 ---> Left motor
-//Vb : Follower wheel
-//Vb : Drive wheel
-int getTractionCtrl(int i_motorTrq, bool rlFlag = false)
+void getTractionCtrl(int* i_motorTrq)
{
//------------------------------
//Constant variables
const double TGT_SLIP_RATIO = 0.5;
+ const double TGT_VEHICLE_SPEED = 1.0 / 3.6; //トラコンONとなる車速[m/s](これ未満は空転を抑える制御をする)
//------------------------------
- double nowMotorTrq = i_motorTrq * LSB_MOTOR_TORQUE; //実数値へ変換
- double outputTrq;
+ double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE}; //実数値へ変換
+ double outputTrq[2] = {0.0};
double steeringAngle, R, V, Vb, Vw, wheelRpsRR, wheelRpsRL;
- static double lastMotorTrq[2] = {0}; //前回の出力トルク
- static double e[2][3] = {0}; //3つ前の偏差まで保持
+ static double lastMotorTrq[2] = {0.0}; //前回の出力トルク
+ static double e[2][3] = {0.0}; //3つ前の偏差まで保持
V = getVelocity(); //前輪回転方向における車速換算値
-
-// V = 5.6;
-
wheelRpsRR = getWheelRps(RR_MOTOR);
wheelRpsRL = getWheelRps(RL_MOTOR);
- if(V <= 0.0 || wheelRpsRR <= 0.0 || wheelRpsRL <= 0.0)
- return i_motorTrq; //車体が止まっている時はトラコンOFF
+// if(V <= 0.0 || wheelRpsRR <= 0.0 || wheelRpsRL <= 0.0)
+// return i_motorTrq; //車体が止まっている時はトラコンOFF
steeringAngle = (getSteerAngle() - 127)/127.0 * M_PI*STEER_RATIO; //実舵角取得
- steeringAngle = 0;
- Vb = V * cos(steeringAngle); //2輪モデルにおける車体進行方向速度取得
+// steeringAngle = 0;
+ Vb = V * cos(steeringAngle); //2輪モデルにおける車体進行方向速度取得
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(rlFlag == 0) {
- Vb = Vb*(1.0 + TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s]
- Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRR; //駆動輪速度[m/s]
- } else {
- Vb = Vb*(1.0 - TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s
- Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL; //駆動輪速度[m/s]
+ for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
+ if(rlFlag == 0) {
+ Vb = Vb*(1.0 + TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s]
+ Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRR; //駆動輪速度[m/s]
+ } else {
+ Vb = Vb*(1.0 - TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s
+ Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL; //駆動輪速度[m/s]
+ }
+
+ if(Vb < TGT_VEHICLE_SPEED) { //対地車速が一定値より小さい場合(停車状態)
+ e[rlFlag][0] = TGT_VEHICLE_SPEED - Vw; //Rev limit制御(一定回転数以上回転させない)
+ } else {
+ e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw; //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
+ }
+
+ lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]); //PIDコントローラへどーーーん
+ e[rlFlag][2] = e[rlFlag][1];
+ e[rlFlag][1] = e[rlFlag][0];
}
- e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw; //現在の偏差取得(目標スリップ率における車輪速と実駆動輪速度の差分)
+ if((lastMotorTrq[0] < reqMotorTrq[0]) && (lastMotorTrq[1] > reqMotorTrq[1])) { //TCS R:L => ON:OFF
+ reqMotorTrq[1] += (reqMotorTrq[0] - lastMotorTrq[0]); //TCSで制限された分を左へベクタリング
+ } else if((lastMotorTrq[1] < reqMotorTrq[1]) && (lastMotorTrq[0] > reqMotorTrq[0])) { //TCS R:L => OFF:ON
+ reqMotorTrq[0] += (reqMotorTrq[1] - lastMotorTrq[1]); //TCSで制限された分を右へベクタリング
+ } else {
+ //Do nothing
+ }
- if(rlFlag == false)
- printf("%2.5f\r\n", e[rlFlag][0]);
-
- lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]); //PIDコントローラへどーーーん
-
- e[rlFlag][2] = e[rlFlag][1];
- e[rlFlag][1] = e[rlFlag][0];
+ for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
+ outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], lastMotorTrq[rlFlag]); //ちっさい方を採用
- outputTrq = myMin(nowMotorTrq, lastMotorTrq[rlFlag]); //ちっさい方を採用
+ if(outputTrq[rlFlag] < 0.0) //現状、マイナストルクは無しで
+ outputTrq[rlFlag] = 0.0;
- if(outputTrq < 0.0) //現状、マイナストルクは無しで
- outputTrq = 0.0;
+ if(outputTrq[rlFlag] > MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE)
+ outputTrq[rlFlag] = MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE;
- if(outputTrq > MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE)
- outputTrq = MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE;
+ if(outputTrq[rlFlag] < MAX_MOTOR_TORQUE_REGENERATIVE*LSB_MOTOR_TORQUE)
+ outputTrq[rlFlag] = MAX_MOTOR_TORQUE_REGENERATIVE*LSB_MOTOR_TORQUE;
- if(outputTrq < MAX_MOTOR_TORQUE_REGENERATIVE*LSB_MOTOR_TORQUE)
- outputTrq = MAX_MOTOR_TORQUE_REGENERATIVE*LSB_MOTOR_TORQUE;
-
- return (int)(outputTrq / LSB_MOTOR_TORQUE + 0.5); //四捨五入してreturn
+ i_motorTrq[rlFlag] = (int)(outputTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5);
+ }
}
void driveTVD(int TVDmode, bool isRedyToDrive)
{
- int requestTorque=0; //ドライバー要求トルク
- int distributionTrq=0; //分配トルク
- int disTrq_omega=0;
- int torqueRight, torqueLeft; //トルクの右左
- static unsigned int preMcpA=0, preMcpB=0;
+ int requestTorque = 0; //ドライバー要求トルク
+ int distributionTrq = 0; //分配トルク
+ int disTrq_omega = 0;
+ int motorTrq[2] = {0}; //左右モータトルク
+ static unsigned int preMcpA = 0, preMcpB = 0;
- loadSensors(); //APS,BRAKE更新
- loadSteerAngle(); //舵角更新
- loadRps(); //従動輪・モータ回転数更新
+ loadSensors(); //APS,BRAKE更新
+ loadSteerAngle(); //舵角更新
+ loadRps(); //従動輪・モータ回転数更新
-// if(isRedyToDrive && isBrakeOn())
- if(isRedyToDrive)
+ if(isRedyToDrive && isBrakeOn())
readyToDriveFlag = 0;
if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION)
@@ -741,15 +746,13 @@
distributionTrq = 0;
disTrq_omega = 0;
- torqueRight = requestTorque + distributionTrq;
- torqueLeft = requestTorque - distributionTrq;
+ motorTrq[0] = requestTorque + distributionTrq;
+ motorTrq[1] = requestTorque - distributionTrq;
- torqueRight += disTrq_omega;
- torqueLeft -= disTrq_omega;
+ motorTrq[0] += disTrq_omega;
+ motorTrq[1] -= disTrq_omega;
- torqueRight = getTractionCtrl(torqueRight, 0);
- torqueLeft = torqueRight;
-// torqueLeft = getTractionCtrl(torqueLeft, 1);
+ getTractionCtrl(motorTrq);
//現在バグあり
//アクセル全開で旋回後、舵を中立に戻していくと加速する。旋回を優先するモード
@@ -779,11 +782,11 @@
// }
// }
- gRightMotorTorque = torqueRight;
- gLeftMotorTorque = torqueLeft;
+ gRightMotorTorque = motorTrq[0];
+ gLeftMotorTorque = motorTrq[1];
- McpData.valA = calcTorqueToVoltage(torqueRight, getRps(RR_MOTOR) * 60.0f);
- McpData.valB = calcTorqueToVoltage(torqueLeft, getRps(RL_MOTOR) * 60.0f);
+ McpData.valA = calcTorqueToVoltage(motorTrq[0], getRps(RR_MOTOR) * 60.0f);
+ McpData.valB = calcTorqueToVoltage(motorTrq[1], getRps(RL_MOTOR) * 60.0f);
preMcpA = McpData.valA;
preMcpB = McpData.valB;