Diff: TVDCTRL.cpp

Revision:

38:11753ee9734f

Parent:

37:ba10cf09c151

Child:

39:c05074379713

--- a/TVDCTRL.cpp	Wed Aug 30 10:18:23 2017 +0000
+++ b/TVDCTRL.cpp	Tue Oct 24 10:19:51 2017 +0000
@@ -298,7 +298,14 @@
 
 float getVelocity(void)
 {
+//    static float debugVelocity = 0.0f;
+//    debugVelocity += 0.002;
+//
+//    printf("%1.2f\r\n", debugVelocity);
+//
+//    return debugVelocity;
     return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*0.5f)*LSB_MOTORSPEED;
+//    return 15.0f;
 }
 
 int distributeTorque_omega(float steeringWheelAngle)
@@ -344,7 +351,7 @@
 
     steeringAngle = myAbs(steeringAngle);
 
-    if(steeringAngle <= 0.0)
+    if(steeringAngle <= 0.0001)
         steeringAngle = 0.0001;
 
     R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle;   //理論旋回半径 計算
@@ -488,19 +495,10 @@
     float limitRate;
     float currentVelocity = getVelocity() * 3.6f;   //km/hで車速取得
 
-    /*
-    if(currentVelocity < 5.0f)
-            limitRate = 0.0f;
-        else if(currentVelocity < 15.0f)
-            limitRate = (currentVelocity - 5.0f) / (15.0f - 5.0f);
-        else
-            limitRate = 1.0f;
-    */
-
     if(currentVelocity < 15.0f)
         limitRate = 0.0f;
     else if(currentVelocity < 30.0f)
-        limitRate = (currentVelocity - 15.0f) / (30.0f - 10.0f);
+        limitRate = (currentVelocity - 15.0f) / (30.0f - 15.0f);
     else
         limitRate = 1.0f;
 
@@ -534,22 +532,17 @@
         readyToDriveFlag = 1;
     }
 
-    //+++++++++++++++++++
-    //後で削除すること！
-    //readyToDriveFlag = 0;
-    //+++++++++++++++++++
-
     indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
     LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
     distributionTrq = (int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f);  //片モーターのトルク分配量計算
-    //disTrq_omega = (int)(distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0f);      //微分制御
+    disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f);      //微分制御
 
     //printf("%d\r\n", distributionTrq);
 
-    //distributionTrq = 0;
+//    distributionTrq = 0;
     disTrq_omega = 0;
 
     torqueRight = requestTorque + distributionTrq;
@@ -558,6 +551,25 @@
     torqueRight += disTrq_omega;
     torqueLeft -= disTrq_omega;
 
+    if(torqueRight < 0) {
+        if((getRPS(RIGHT) * LSB_MOTORSPEED * 60.0f) < 600.0f) {
+            torqueLeft = requestTorque + torqueRight;
+            torqueRight = 0;
+        } else if((getRPS(RIGHT) * LSB_MOTORSPEED * 60.0f) <= 1250.0f) {
+            torqueLeft = requestTorque + torqueRight*((getRPS(RIGHT) * GEAR_RATIO * LSB_MOTORSPEED * 60.0f - 600.0f)/(1250.0f - 600.0f));
+            torqueRight = torqueRight*((getRPS(RIGHT)-600.0f)/(1250.0f - 600.0f));
+        }
+    }
+    if(torqueLeft < 0) {
+        if((getRPS(LEFT) * LSB_MOTORSPEED * 60.0f) < 600.0f) {
+            torqueRight = requestTorque + torqueLeft;
+            torqueLeft = 0;
+        } else if((getRPS(LEFT) * LSB_MOTORSPEED * 60.0f) <= 1250.0f) {
+            torqueRight = requestTorque + torqueLeft*((getRPS(LEFT) * GEAR_RATIO * LSB_MOTORSPEED * 60.0f - 600.0f)/(1250.0f - 600.0f));
+            torqueLeft = torqueLeft*((getRPS(LEFT)-600.0f)/(1250.0f - 600.0f));
+        }
+    }
+
     //アクセルべた踏みでトルクMAX、旋回より駆動を優先(加速番長モード)
     if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
         torqueLeft = MAX_OUTPUT_TORQUE_POWER;
@@ -574,8 +586,10 @@
     McpData.valA = calcTorqueToVoltage(torqueRight, getRPS(RIGHT));
     McpData.valB = calcTorqueToVoltage(torqueLeft, getRPS(LEFT));
 
-    preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544);
-    preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544);
+//    preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544);
+//    preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544);
+    preMcpA = McpData.valA;
+    preMcpB = McpData.valB;
 
     mcp.writeA(preMcpA);   //右モーター
     mcp.writeB(preMcpB);   //左モーター