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:
- 51:640198055ed6
- Parent:
- 50:b542658924df
- Child:
- 52:6209efecde6f
--- a/TVDCTRL.cpp Tue Dec 19 08:11:06 2017 +0000
+++ b/TVDCTRL.cpp Wed Dec 20 16:34:37 2017 +0000
@@ -2,6 +2,7 @@
#include "MCP4922.h"
#include "Steering.h"
#include "Global.h"
+#include "float.h"
extern AnalogIn apsP;
extern AnalogIn apsS;
@@ -523,21 +524,21 @@
//x = 1/a * (y + ab - c)
unsigned int calcTorqueToVoltage(int reqTorque, int rpm)
{
- float slope = 0; //a
- int startPoint = 0; //b
- int intercept = 0; //c
+ double slope = 0; //a
+ double startPoint = 0; //b
+ double intercept = 0; //c
int outputVoltage=0;
if(reqTorque > LINEAR_REGION_TORQUE_POWER) { //力行トルクがrpmに対して非線形となる領域
- slope = (float)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/(DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER);
+ slope = (double)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/(DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER);
startPoint = LINEAR_REGION_VOLTAGE_POWER;
intercept = LINEAR_REGION_TORQUE_POWER;
outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
} else if(reqTorque > 0) { //力行トルクがrpmに対して線形となる領域
- slope = (float)LINEAR_REGION_TORQUE_POWER/(LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P);
+ slope = (double)LINEAR_REGION_TORQUE_POWER/(LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P);
startPoint = ZERO_TORQUE_VOLTAGE_P;
intercept = 0;
@@ -548,14 +549,14 @@
outputVoltage = ZERO_TORQUE_VOLTAGE_NEUTRAL; //ニュートラル信号
} else if(reqTorque > LINEAR_REGION_TORQUE_REGENERATIVE) { //回生トルクがrpmに対して線形となる領域
- slope = (float)(0 - LINEAR_REGION_TORQUE_REGENERATIVE)/(ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE);
+ slope = (double)(0.0 - LINEAR_REGION_TORQUE_REGENERATIVE)/(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);
} else { //回生トルクがrpmに対して非線形となる領域
- slope = (float)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
+ slope = (double)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
startPoint = DACOUTPUT_MIN;
intercept = calcMaxTorque(rpm, 1);
@@ -622,12 +623,12 @@
{
//------------------------------
//Constant variables
- const double KP = 40.0;
+ const double KP = 10.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.0*e[1] + e[2]); //PID controller
+ return lastOutput + KP*(e[0] - e[1]) + KI*e[0] + KD*(e[0] - 2.0*e[1] + e[2]); //PID controller
}
//--------------------------------------------------
@@ -638,7 +639,7 @@
//------------------------------
//Constant variables
const double TGT_SLIP_RATIO = 0.1;
- const double TGT_VEHICLE_SPEED = 0.0 / 3.6; //トラコンONとなる車速[m/s](これ未満は空転を抑える制御をする)
+ const double TGT_VEHICLE_SPEED = 20.0 / 3.6; //トラコンONとなる車速[m/s](これ未満は空転を抑える制御をする)
//------------------------------
double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE}; //実数値へ変換
@@ -647,25 +648,22 @@
static double lastMotorTrq[2] = {0.0}; //前回の出力トルク
double motorTrq_wTCS[2] = {0.0}; //TCSトルクベクタリングを含めたトルク
static double e[2][3] = {0.0}; //3つ前の偏差まで保持
+ double vectoringAmount = 0.0;
V = getVelocity(); //前輪回転方向における車速換算値
- V = 2.8;
-
wheelRpsRR = getWheelRps(RR_MOTOR);
wheelRpsRL = getWheelRps(RL_MOTOR);
steeringAngle = getSteerAngle()/127.0 * M_PI*STEER_RATIO; //実舵角取得
Vb = V * cos(steeringAngle); //2輪モデルにおける車体進行方向速度取得
- R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.001); //理論旋回半径取得
+ R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.01); //理論旋回半径取得
- if(myAbs(R) < 1.0)
- R = mySign(steeringAngle) * 1.0;
+ if(myAbs(R) < 5.0)
+ R = mySign(steeringAngle) * 5.0;
if(myAbs(R) > 100.0)
R = mySign(steeringAngle) * 100.0;
-// printf("%f\r\n", R);
-
for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
if(rlFlag == 0) {
Vb = Vb*(1.0 + TREAD/(2.0*R)); //トレッドを考慮した従動輪速度[m/s]
@@ -686,35 +684,42 @@
e[rlFlag][1] = e[rlFlag][0];
}
- if((lastMotorTrq[0] < reqMotorTrq[0]) && (lastMotorTrq[1] > reqMotorTrq[1])) { //TCS R:L => ON:OFF
- motorTrq_wTCS[1] = reqMotorTrq[1] + (reqMotorTrq[0] - lastMotorTrq[0]); //TCSで制限された分を左へベクタリング
- motorTrq_wTCS[0] = reqMotorTrq[0];
- } else if((lastMotorTrq[1] < reqMotorTrq[1]) && (lastMotorTrq[0] > reqMotorTrq[0])) { //TCS R:L => OFF:ON
- motorTrq_wTCS[0] = reqMotorTrq[0] + (reqMotorTrq[1] - lastMotorTrq[1]); //TCSで制限された分を右へベクタリング
- motorTrq_wTCS[1] = reqMotorTrq[1];
- } else {
- motorTrq_wTCS[0] = reqMotorTrq[0];
- motorTrq_wTCS[1] = reqMotorTrq[1];
- }
+// if((lastMotorTrq[0] < reqMotorTrq[0]) && (lastMotorTrq[1] >= reqMotorTrq[1])) { //TCS R:L => ON:OFF
+// vectoringAmount = reqMotorTrq[0] - lastMotorTrq[0];
+//
+// if(myAbs(vectoringAmount) > 10.0)
+// vectoringAmount = mySign(vectoringAmount) * 10.0;
+//
+// motorTrq_wTCS[1] = reqMotorTrq[1] + vectoringAmount; //TCSで制限された分を左へベクタリング
+// motorTrq_wTCS[0] = reqMotorTrq[0];
+//
+// } else if((lastMotorTrq[1] < reqMotorTrq[1]) && (lastMotorTrq[0] >= reqMotorTrq[0])) { //TCS R:L => OFF:ON
+// vectoringAmount = reqMotorTrq[0] - lastMotorTrq[0];
+//
+// if(myAbs(vectoringAmount) > 10.0)
+// vectoringAmount = mySign(vectoringAmount) * 10.0;
+//
+// motorTrq_wTCS[0] = reqMotorTrq[0] + vectoringAmount; //TCSで制限された分を右へベクタリング
+// motorTrq_wTCS[1] = reqMotorTrq[1];
+//
+// } else {
+// motorTrq_wTCS[0] = reqMotorTrq[0];
+// motorTrq_wTCS[1] = reqMotorTrq[1];
+// }
motorTrq_wTCS[0] = reqMotorTrq[0];
motorTrq_wTCS[1] = reqMotorTrq[1];
- printf("%f %f\r\n", e[0][0], e[1][0]);
+// printf("%f %f\r\n", e[0][0], e[1][0]);
for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], myMin(lastMotorTrq[rlFlag], motorTrq_wTCS[rlFlag])); //ちっさい方を採用
-// outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], lastMotorTrq[rlFlag]); //ちっさい方を採用
- if(outputTrq[rlFlag] < 0.0) //現状、マイナストルクは無しで
- outputTrq[rlFlag] = 0.0;
-
- if(outputTrq[rlFlag] > MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE)
- outputTrq[rlFlag] = 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[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);
}
@@ -749,11 +754,11 @@
indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
- requestTorque=calcRequestTorque(); //ドライバー要求トルク取得
+ requestTorque = calcRequestTorque(); //ドライバー要求トルク取得
if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
requestTorque = 0;
-
+
distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0f); //片モーターのトルク分配量計算
disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f); //微分制御
@@ -768,34 +773,6 @@
getTractionCtrl(motorTrq);
- //現在バグあり
- //アクセル全開で旋回後、舵を中立に戻していくと加速する。旋回を優先するモード
-// if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
-// torqueRight = torqueLeft = requestTorque; //内輪側モーター最低トルクより小さい要求トルクなら等配分
-// } else {
-// if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
-// torqueLeft = MAX_OUTPUT_TORQUE_POWER;
-//
-// if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
-// torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
-// }
-// }
-// if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
-// torqueRight = MAX_OUTPUT_TORQUE_POWER;
-// if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
-// torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
-// }
-// }
-// if(torqueLeft < MIN_INNERWHEEL_MOTOR_TORQUE) { //内輪最低トルク時
-// torqueLeft = MIN_INNERWHEEL_MOTOR_TORQUE; //内輪最低トルクにクリップ
-// torqueRight = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE; //片モーター下限値時，トルク高側のモーターも出力クリップ
-// }
-// if(torqueRight < MIN_INNERWHEEL_MOTOR_TORQUE) { //内輪最低トルク時
-// torqueRight = MIN_INNERWHEEL_MOTOR_TORQUE; //内輪最低トルクにクリップ
-// torqueLeft = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE; //片モーター下限値時，トルク高側のモーターも出力クリップ
-// }
-// }
-
gRightMotorTorque = motorTrq[0];
gLeftMotorTorque = motorTrq[1];