Diff: TVDCTRL.cpp

Revision:

10:87ad65eef0e9

Parent:

9:220e4e77e056

Child:

11:88701a5e7eae

--- a/TVDCTRL.cpp	Wed Jul 27 05:42:33 2016 +0000
+++ b/TVDCTRL.cpp	Sat Aug 06 05:39:00 2016 +0000
@@ -252,6 +252,11 @@
     if(pulseTimeISRFlag == true) {
         pulseTimeISRFlag = false;
 
+        if(gRightPulseTime > 100000)
+            gRightPulseTime = 100000;
+        if(gLeftPulseTime > 100000)
+            gLeftPulseTime = 100000;
+
         preRightPulse = (int)(gRightPulseTime*ratioLPF_V + preRightPulse*(1.0f-ratioLPF_V));
         preLeftPulse = (int)(gLeftPulseTime*ratioLPF_V + preLeftPulse*(1.0f-ratioLPF_V));
     }
@@ -288,20 +293,18 @@
 int distributeTorque(float velocity, float steering)
 {
     int disTrq = 0;
-    float sqrtVelocity = velocity*velocity;
-    float Gy=0;
+    double sqrtVelocity = (double)velocity*velocity;
+    double Gy=0;
 
-    Gy = (sqrtVelocity*steering) / ((1.0f+STABIRITY_FACTOR*sqrtVelocity)*WHEEL_BASE);
-    
-    printf("%f\r\n", Gy);
+    Gy = (sqrtVelocity*steering) / ((1.0+STABIRITY_FACTOR*sqrtVelocity)*WHEEL_BASE);
 
-    if(Gy > 9.8f)
-        Gy = 9.8f;
+    if(Gy > 9.8)
+        Gy = 9.8;
 
-    if(Gy < 0.98f) {
+    if(Gy < 0.98) {
         disTrq = 0;
-    } else if(Gy < 4.9f) {
-        disTrq = ((float)MAX_DISTRIBUTION_TORQUE / (4.9f-0.98f) * (Gy-0.98f));
+    } else if(Gy < 4.9) {
+        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (4.9-0.98) * (Gy-0.98));
     } else {    //0.5G以上は配分一定
         disTrq = MAX_DISTRIBUTION_TORQUE;
     }
@@ -388,7 +391,11 @@
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
     distributionTrq = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
+    //distributionTrq = distributeTorque(50.0f, MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
     distributionTrq /= 2.0;
+    
+    //デバッグ中
+    distributionTrq = 0;
 
     if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
         torqueHigh = MAX_OUTPUT_TORQUE;
@@ -430,4 +437,8 @@
     ticker1.attach(&loadSensorsISR, 0.01f);    //サンプリング周期10msec
     //ticker2.attach(&generatePulse, 0.03f);
     ticker3.attach(&getPulseTimeISR, 0.01f);
+
+    mcp.writeA(0);   //右モーター
+    mcp.writeB(0);   //左モーター
+
 }