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:
- 55:e9ca699bec57
- Parent:
- 54:f466964ceaa5
- Child:
- 56:78bd99bf995a
--- a/TVDCTRL.cpp Mon Jan 08 05:29:14 2018 +0000
+++ b/TVDCTRL.cpp Mon Jan 08 10:06:12 2018 +0000
@@ -341,7 +341,7 @@
//パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定 (ピーク値を保存したい)
if(rps[i].counter == 0) {
- if(rps[i].stopCounter < 50) //低回転数時、急に0rpsと演算しないように前回値保持(設定値はだいたい)
+ if(rps[i].stopCounter < 5) //低回転数時、急に0rpsと演算しないように前回値保持(設定値はだいたい)
rps[i].stopCounter++;
else
gRps[i] = 0.0;
@@ -528,39 +528,39 @@
double startPoint = 0; //b
double intercept = 0; //c
- int outputVoltage=0;
+ double outputVoltage=0;
if(reqTorque > LINEAR_REGION_TORQUE_POWER) { //力行トルクがrpmに対して非線形となる領域
- slope = (double)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/(DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER);
+ slope = (double)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/((double)DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER);
startPoint = LINEAR_REGION_VOLTAGE_POWER;
intercept = LINEAR_REGION_TORQUE_POWER;
- outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
+ outputVoltage = ((reqTorque + slope*startPoint - intercept) / slope);
} else if(reqTorque > 0) { //力行トルクがrpmに対して線形となる領域
- slope = (double)LINEAR_REGION_TORQUE_POWER/(LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P);
+ slope = (double)LINEAR_REGION_TORQUE_POWER/((double)LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P);
startPoint = ZERO_TORQUE_VOLTAGE_P;
intercept = 0;
- outputVoltage = (int)(reqTorque/slope + startPoint);
+ outputVoltage = (reqTorque/slope + startPoint);
} else if(0 == reqTorque) {
outputVoltage = ZERO_TORQUE_VOLTAGE_NEUTRAL; //ニュートラル信号
} else if(reqTorque > LINEAR_REGION_TORQUE_REGENERATIVE) { //回生トルクがrpmに対して線形となる領域
- slope = (double)(0.0 - LINEAR_REGION_TORQUE_REGENERATIVE)/(ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE);
+ slope = (double)(0.0 - LINEAR_REGION_TORQUE_REGENERATIVE)/((double)ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE);
startPoint = LINEAR_REGION_VOLTAGE_REGENERATIVE;
intercept = LINEAR_REGION_TORQUE_REGENERATIVE;
- outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
+ outputVoltage = ((reqTorque + slope*startPoint - intercept) / slope);
} else { //回生トルクがrpmに対して非線形となる領域
- slope = (double)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
+ slope = (double)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/((double)LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
startPoint = DACOUTPUT_MIN;
intercept = calcMaxTorque(rpm, 1);
- outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
+ outputVoltage = ((reqTorque + slope*startPoint - intercept) / slope);
}
if(outputVoltage > DACOUTPUT_MAX)
@@ -568,7 +568,7 @@
if(outputVoltage < DACOUTPUT_MIN)
outputVoltage = DACOUTPUT_MIN;
- return (unsigned int)(0xFFF*((double)outputVoltage/0xFFFF)); //DACの分解能に適応(16bit->12bit)
+ return (unsigned int)(0xFFF*(outputVoltage/0xFFFF)); //DACの分解能に適応(16bit->12bit)
}
int calcRequestTorque(void)
@@ -640,24 +640,22 @@
//Constant variables
const double TGT_SLIP_RATIO = 0.3;
const double TGT_VEHICLE_SPEED = 20.0 / 3.6; //この車速相当の回転数まで空転を制限する
- const double CTRL_START_VEHICLE_SPEED = 5 / 3.6; //トラコンONとなる車速[m/s]
+ const double CTRL_START_VEHICLE_SPEED = 0.0 / 3.6; //トラコンONとなる車速[m/s]
//------------------------------
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, Vbw, Vw, wheelRpsRR, wheelRpsRL;
+ double steeringAngle, R, Vb, Vbw, Vw, wheelRpsRR, wheelRpsRL;
static double lastMotorTrq[2] = {0.0}; //前回の出力トルク
static double e[2][3] = {0.0}; //3つ前の偏差まで保持
- double tmpTcsMotorTrq; //
-
- V = getVelocity(); //前輪回転方向における車速換算値
+ double tmpTcsMotorTrq; //
wheelRpsRR = getWheelRps(RR_MOTOR);
wheelRpsRL = getWheelRps(RL_MOTOR);
- steeringAngle = getSteerAngle()/127.0 * M_PI*STEER_RATIO; //実舵角取得
- Vb = V * cos(steeringAngle); //2輪モデルにおける車体進行方向速度取得
+ steeringAngle = getSteerAngle()/127.0 * M_PI*STEER_RATIO; //実舵角取得
+ Vb = getVelocity(); //前輪回転方向における車速換算値
Vb = 15.0/3.6;
R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.001); //理論旋回半径取得
@@ -699,27 +697,23 @@
outputTrq[rlFlag] = -10.0;
}
- if(e[0][0] > e[1][1]) { //右輪の空転が大きい場合
+ if(myAbs(e[0][0]) > myAbs(e[1][1])) { //右輪の空転が大きい場合
tmpTcsMotorTrq = outputTrq[1] * (ALPHA-1.0)/(ALPHA+1.0);
- if(reqMotorTrq[0] > tmpTcsMotorTrq)
- if(tmpTcsMotorTrq > outputTrq[0]) //空転輪側車軸トルクを下げ過ぎていた場合
+ if(tmpTcsMotorTrq > outputTrq[0]) //空転輪側車軸トルクを下げ過ぎていた場合
+ if(reqMotorTrq[0] > tmpTcsMotorTrq)
outputTrq[0] = tmpTcsMotorTrq; //右車軸トルクを0へ制限
- } else if(e[0][0] < e[1][1]) { //左輪の空転が大きい場合
+ } else if(myAbs(e[0][0]) < myAbs(e[1][1])) { //左輪の空転が大きい場合
tmpTcsMotorTrq = outputTrq[0] * (ALPHA-1.0)/(ALPHA+1.0);
- if(reqMotorTrq[1] > tmpTcsMotorTrq)
- if(tmpTcsMotorTrq > outputTrq[1]) //空転輪側車軸トルクを下げ過ぎていた場合
+ if(tmpTcsMotorTrq > outputTrq[1]) //空転輪側車軸トルクを下げ過ぎていた場合
+ if(reqMotorTrq[1] > tmpTcsMotorTrq)
outputTrq[1] = tmpTcsMotorTrq; //左車軸トルクを0へ制限
}
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;
-
- i_motorTrq[rlFlag] = (int)(outputTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5);
+ if(e[rlFlag][0] < 0.0) //駆動時のみ制御ON
+ 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)
@@ -734,6 +728,8 @@
loadSteerAngle(); //舵角更新
loadRps(); //従動輪・モータ回転数更新
+// printf("%f %f\r\n", getWheelRps(RR_MOTOR), getWheelRps(RL_MOTOR));
+
if(isRedyToDrive && isBrakeOn())
readyToDriveFlag = 0;