Diff: TVDCTRL.cpp

Revision:

49:c97740265324

Parent:

48:45614aa0db15

Child:

50:b542658924df

--- a/TVDCTRL.cpp	Fri Dec 15 04:02:58 2017 +0000
+++ b/TVDCTRL.cpp	Tue Dec 19 04:30:13 2017 +0000
@@ -552,7 +552,7 @@
         startPoint = LINEAR_REGION_VOLTAGE_REGENERATIVE;
         intercept = LINEAR_REGION_TORQUE_REGENERATIVE;
 
-        outputVoltage = (int)(reqTorque/slope + startPoint);
+        outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
 
     } else {                                            //回生トルクがrpmに対して非線形となる領域
         slope = (float)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
@@ -637,25 +637,25 @@
 {
     //------------------------------
     //Constant variables
-    const double TGT_SLIP_RATIO = 0.5;
+    const double TGT_SLIP_RATIO = 0.1;
     const double TGT_VEHICLE_SPEED = 1.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, Vw, wheelRpsRR, wheelRpsRL;
-    static double lastMotorTrq[2] = {0.0};                        //前回の出力トルク
-    static double e[2][3] = {0.0};                                //3つ前の偏差まで保持
+    static double lastMotorTrq[2] = {0.0};                          //前回の出力トルク
+    double motorTrq_wTCS[2] = {0.0};                                //TCSトルクベクタリングを含めたトルク
+    static double e[2][3] = {0.0};                                  //3つ前の偏差まで保持
 
-    V = getVelocity();                                          //前輪回転方向における車速換算値
+    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
-
-    steeringAngle   = (getSteerAngle() - 127)/127.0 * M_PI*STEER_RATIO; //実舵角取得
-//    steeringAngle   = 0;
+    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);  //理論旋回半径取得
 
@@ -671,8 +671,8 @@
             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制御(一定回転数以上回転させない)
+        if(Vb < TGT_VEHICLE_SPEED) {                                //対地車速が一定値より小さい場合(停車状態)
+            e[rlFlag][0] = TGT_VEHICLE_SPEED/(1.0 - TGT_SLIP_RATIO) - Vw;   //Rev limit制御(一定回転数以上回転させない)
         } else {
             e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw;          //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
         }
@@ -683,18 +683,23 @@
     }
 
     if((lastMotorTrq[0] < reqMotorTrq[0]) && (lastMotorTrq[1] > reqMotorTrq[1])) {          //TCS R:L => ON:OFF
-        reqMotorTrq[1] += (reqMotorTrq[0] - lastMotorTrq[0]);                               //TCSで制限された分を左へベクタリング
+        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
-        reqMotorTrq[0] += (reqMotorTrq[1] - lastMotorTrq[1]);                               //TCSで制限された分を右へベクタリング
+        motorTrq_wTCS[0] = reqMotorTrq[0] + (reqMotorTrq[1] - lastMotorTrq[1]);                               //TCSで制限された分を右へベクタリング
+        motorTrq_wTCS[1] = reqMotorTrq[1];
     } else {
-        //Do nothing
+        motorTrq_wTCS[0] = reqMotorTrq[0];
+        motorTrq_wTCS[1] = reqMotorTrq[1];
     }
+    
+    printf("%f %f\r\n", e[0][0], e[1][0]);
 
     for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
-        outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], lastMotorTrq[rlFlag]);    //ちっさい方を採用
+        outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], myMin(lastMotorTrq[rlFlag], motorTrq_wTCS[rlFlag]));    //ちっさい方を採用
 
-        if(outputTrq[rlFlag] < 0.0)                              //現状、マイナストルクは無しで
-            outputTrq[rlFlag] = 0.0;
+//        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;