Diff: TVDCTRL.cpp

Revision:

50:b542658924df

Parent:

49:c97740265324

Child:

51:640198055ed6

--- a/TVDCTRL.cpp	Tue Dec 19 04:30:13 2017 +0000
+++ b/TVDCTRL.cpp	Tue Dec 19 08:11:06 2017 +0000
@@ -638,7 +638,7 @@
     //------------------------------
     //Constant variables
     const double TGT_SLIP_RATIO = 0.1;
-    const double TGT_VEHICLE_SPEED = 1.0 / 3.6;                 //トラコンONとなる車速[m/s](これ未満は空転を抑える制御をする)
+    const double TGT_VEHICLE_SPEED = 0.0 / 3.6;                 //トラコンONとなる車速[m/s](これ未満は空転を抑える制御をする)
     //------------------------------
 
     double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE};         //実数値へ変換
@@ -649,9 +649,9 @@
     static double e[2][3] = {0.0};                                  //3つ前の偏差まで保持
 
     V = getVelocity();                                              //前輪回転方向における車速換算値
-    
-//    V = 5.6;
-    
+
+    V = 2.8;
+
     wheelRpsRR = getWheelRps(RR_MOTOR);
     wheelRpsRL = getWheelRps(RL_MOTOR);
 
@@ -661,7 +661,11 @@
 
     if(myAbs(R) < 1.0)
         R = mySign(steeringAngle) * 1.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]
@@ -692,14 +696,19 @@
         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]);
 
     for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
-        outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], myMin(lastMotorTrq[rlFlag], motorTrq_wTCS[rlFlag]));    //ちっさい方を採用
 
-//        if(outputTrq[rlFlag] < 0.0)                              //現状、マイナストルクは無しで
-//            outputTrq[rlFlag] = 0.0;
+        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;