Diff: TVDCTRL.cpp

Revision:

32:9688c30ac38b

Parent:

31:042c08a7434f

Child:

33:8bebba17a749

diff -r 042c08a7434f -r 9688c30ac38b TVDCTRL.cpp
--- a/TVDCTRL.cpp	Thu Jul 27 12:34:06 2017 +0000
+++ b/TVDCTRL.cpp	Thu Jul 27 15:29:22 2017 +0000
@@ -298,7 +298,7 @@
 
 float getVelocity(void)
 {
-    return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))/2.0f)*LSB_MOTORSPEED;
+    return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*0.5f)*LSB_MOTORSPEED;
 }
 
 int distributeTorque_omega(float steering)
@@ -333,7 +333,7 @@
     double R = 0.0;     //旋回半径
     double Gy = 0.0;    //横G
     double deadband = 0.0;
-    double steeringAngle = steeringWheelAngle / 7.0;
+    double steeringAngle = (double)steeringWheelAngle * STEER_RATIO;
     double steeringSign = 1.0;
     int disTrq = 0;
 
@@ -343,12 +343,12 @@
         steeringSign = -1;
 
     steeringAngle = myAbs(steeringAngle);
-    
+
     if(steeringAngle <= 0.0)
         steeringAngle = 0.0001;
 
     R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle;   //理論旋回半径 計算
-    Gy = V2 / R;                                    //理論横G
+    Gy = V2 / R / 9.81;                                    //理論横G
 
     if(Gy < deadband)
         disTrq = 0;
@@ -537,7 +537,7 @@
     //disTrq_omega = (int)(distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0);      //微分制御
     //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());     //トルク配分の最低車速制限
 
-    distributionTrq = 0;
+    distributionTrq = 100;
 
     torqueRight = requestTorque + distributionTrq;
     torqueLeft = requestTorque - distributionTrq;
@@ -554,6 +554,8 @@
         torqueRight = MAX_OUTPUT_TORQUE_POWER;
         torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
     }
+    
+    printf("%d %d\r\n", torqueRight, torqueLeft);
 
     McpData.valA = calcTorqueToVoltage(torqueRight, getRPS(RIGHT));
     McpData.valB = calcTorqueToVoltage(torqueLeft, getRPS(LEFT));