Diff: TVDCTRL.cpp

Revision:

26:331e77bb479b

Parent:

25:c21d35c7f0de

Child:

27:37a8b4f6d28d

--- a/TVDCTRL.cpp	Sat Jul 01 00:26:28 2017 +0000
+++ b/TVDCTRL.cpp	Sat Jul 01 07:24:50 2017 +0000
@@ -16,6 +16,10 @@
 extern Serial pc;
 extern AnalogOut STR2AN;
 
+extern DigitalOut indicatorLed;
+
+#define indicateSystem(x)       (indicatorLed.write(x))
+
 Timer RightPulseTimer;
 Timer LeftPulseTimer;
 Ticker ticker1;
@@ -264,14 +268,14 @@
     }
 
     if(rl == RIGHT_MOTOR)
-        return ((float)sumRightMotorPulse / MOTOR_PULSE_NUM) * 60.0;    //過去1秒間のモータパルス数を使ってRPM算出
+        return ((float)sumRightMotorPulse / MOTOR_PULSE_NUM) * 60.0f;    //過去1秒間のモータパルス数を使ってRPM算出
     else
-        return ((float)sumLeftMotorPulse / MOTOR_PULSE_NUM) * 60.0;     //過去1秒間のモータパルス数を使ってRPM算出
+        return ((float)sumLeftMotorPulse / MOTOR_PULSE_NUM) * 60.0f;     //過去1秒間のモータパルス数を使ってRPM算出
 }
 
 float getVelocity(void)
 {
-    return TIRE_DIAMETER*M_PI*(getRPS(RIGHT_MOTOR) + getRPS(LEFT_MOTOR))/2.0;
+    return TIRE_DIAMETER*M_PI*(getRPS(RIGHT_MOTOR) + getRPS(LEFT_MOTOR))/2.0f;
 }
 
 int distributeTorque_omega(float steering)
@@ -332,8 +336,8 @@
 
     //後で削除
     rpm = 2000;
+    //++++++++++++++++++++
 
-    //++++++++++++++++++++
     if(regSwitch == 0) {
         if(rpm <3000)
             maxTrq = MAX_MOTOR_TORQUE_POWER;
@@ -345,9 +349,11 @@
         if(rpm < 1250) {
             maxTrq = 0;
         } else if(rpm <= 6000) {
+            maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE;
+        } else if(rpm <= 11000) {
             //+++++++++++++++++++++++++++++++++++++
             //暫定処理 今後回生トルクマップも要作成
-            maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE;
+            maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
             //+++++++++++++++++++++++++++++++++++++
         } else {
             maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
@@ -453,12 +459,7 @@
     return limitRate;
 }
 
-extern DigitalIn RTDSW;
-#define isPressedRTD(void)      (!RTDSW.read())
-extern DigitalOut indicatorLed;
-#define indicateSystem(x)       (indicatorLed.write(x))
-
-void driveTVD(void)
+void driveTVD(int TVDmode, bool isRedyToDrive)
 {
     int requestTorque=0;    //ドライバー要求トルク
     int distributionTrq=0;  //分配トルク
@@ -469,7 +470,7 @@
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
 
-    if(isPressedRTD() && isBrakeOn())
+    if(isRedyToDrive && isBrakeOn())
         readyToDriveFlag = 0;
 
     if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION)
@@ -494,44 +495,24 @@
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
     distributionTrq = (int)(distributeTorque(getSteerAngle()) / 2.0);  //片モーターのトルク分配量計算
-    //disTrq_omega = (int)(distributeTorque_omega(getSteerAngle()) / 2.0);      //微分制御
+    disTrq_omega = (int)(distributeTorque_omega(getSteerAngle()) / 2.0);      //微分制御
     //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());     //トルク配分の最低車速制限
 
-    printf("%1.2f\r\n", 45.0/0xffff*distributionTrq);
-
     torqueRight = requestTorque + distributionTrq;
     torqueLeft = requestTorque - distributionTrq;
 
     torqueRight += disTrq_omega;
     torqueLeft -= disTrq_omega;
 
-    /*
-    if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
-        torqueRight = torqueLeft = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
-    } else {
-        if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
-            torqueLeft = MAX_OUTPUT_TORQUE_POWER;
-
-            if(((torqueRight + torqueLeft)/2.0) > requestTorque) {
-                torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
-            }
-        }
-        if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
-            torqueRight = MAX_OUTPUT_TORQUE_POWER;
-            if(((torqueRight + torqueLeft)/2.0) > requestTorque) {
-                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
-            }
-        }
-        if(torqueLeft < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
-            torqueLeft = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
-            torqueRight = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
-        }
-        if(torqueRight < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
-            torqueRight = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
-            torqueLeft = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
-        }
+    //アクセルべた踏みでトルクMAX、旋回より駆動を優先(加速番長モード)
+    if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
+        torqueLeft = MAX_OUTPUT_TORQUE_POWER;
+        torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
     }
-    */
+    if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
+        torqueRight = MAX_OUTPUT_TORQUE_POWER;
+        torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
+    }
 
     McpData.valA = calcTorqueToVoltage(torqueRight, RIGHT_MOTOR);
     McpData.valB = calcTorqueToVoltage(torqueLeft, LEFT_MOTOR);