Diff: TVDCTRL.cpp

Revision:

28:47e9531a3a9d

Parent:

27:37a8b4f6d28d

Child:

29:a51cb2cf22ae

--- a/TVDCTRL.cpp	Sat Jul 01 08:45:37 2017 +0000
+++ b/TVDCTRL.cpp	Mon Jul 03 12:03:34 2017 +0000
@@ -21,7 +21,6 @@
 extern AnalogOut STR2AN;
 extern CAN can;
 
-
 #define indicateSystem(x)       (indicatorLed.write(x))
 
 Timer RightPulseTimer;
@@ -48,6 +47,13 @@
 int gApsP=0, gApsS=0, gBrake=0;        //現在のセンサ値
 int rawApsP=0, rawApsS=0, rawBrake=0;  //現在の補正無しのセンサ値
 
+int gRightMotorTorque=0, gLeftMotorTorque=0;
+
+int getMotorTorque(Select rl)
+{
+    return ((rl==LEFT) ? gLeftMotorTorque : gRightMotorTorque);
+}
+
 //エラーカウンタ外部参照用関数
 //errCounter_t型変数のポインタを引数に取る
 void getCurrentErrCount(struct errCounter_t *ptr)
@@ -245,10 +251,12 @@
     pulseTimeISRFlag = true;
 }
 
-float getRPS(SelectMotor rl)
+//default argument  : switchWheel=false
+int getRPS(Select rl, bool switchWheel)
 {
     static int rightMotorPulse[100]= {0}, leftMotorPulse[100]= {0};     //過去1秒間のパルス数格納
     static int sumRightMotorPulse, sumLeftMotorPulse;
+    float rps;
 
     if(pulseTimeISRFlag == true) {
         for(int i=99; i>0; i--) {
@@ -269,17 +277,28 @@
             sumRightMotorPulse += rightMotorPulse[i];
             sumLeftMotorPulse += leftMotorPulse[i];
         }
+        pulseTimeISRFlag = false;
     }
 
-    if(rl == RIGHT_MOTOR)
-        return ((float)sumRightMotorPulse / MOTOR_PULSE_NUM) * 60.0f;    //過去1秒間のモータパルス数を使ってRPM算出
-    else
-        return ((float)sumLeftMotorPulse / MOTOR_PULSE_NUM) * 60.0f;     //過去1秒間のモータパルス数を使ってRPM算出
+    if(switchWheel == 0) {
+        if(rl == RIGHT)
+            rps = (float)sumRightMotorPulse / MOTOR_PULSE_NUM;    //過去1秒間のモータパルス数を使ってRPS算出
+        else
+            rps = (float)sumLeftMotorPulse / MOTOR_PULSE_NUM;     //過去1秒間のモータパルス数を使ってRPS算出
+    } else {
+        //こっちはタイヤ回転数
+        //そのうち対応
+        if(rl == RIGHT)
+            rps = (float)sumRightMotorPulse / MOTOR_PULSE_NUM;    //過去1秒間のモータパルス数を使ってRPS算出
+        else
+            rps = (float)sumLeftMotorPulse / MOTOR_PULSE_NUM;     //過去1秒間のモータパルス数を使ってRPS算出
+    }
+    return (int)(rps / LSB_MOTORSPEED);     //LSB変換
 }
 
 float getVelocity(void)
 {
-    return TIRE_DIAMETER*M_PI*(getRPS(RIGHT_MOTOR) + getRPS(LEFT_MOTOR))/2.0f;
+    return TIRE_DIAMETER*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*LSB_MOTORSPEED/2.0f;
 }
 
 int distributeTorque_omega(float steering)
@@ -350,8 +369,13 @@
         else
             maxTrq = MAX_REVOLUTION_TORQUE_POWER;
     } else {
-        if(rpm < 1250) {
+        if(rpm < 600) {
             maxTrq = 0;
+        } else if(rpm < 1250) {
+            //+++++++++++++++++++++++++++++++++++++
+            //暫定処理 今後回生トルクマップも要作成
+            maxTrq = 0;
+            //+++++++++++++++++++++++++++++++++++++
         } else if(rpm <= 6000) {
             maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE;
         } else if(rpm <= 11000) {
@@ -498,8 +522,8 @@
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
-    distributionTrq = (int)(distributeTorque(getSteerAngle()) / 2.0);  //片モーターのトルク分配量計算
-    disTrq_omega = (int)(distributeTorque_omega(getSteerAngle()) / 2.0);      //微分制御
+    distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0);  //片モーターのトルク分配量計算
+    disTrq_omega = (int)(distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0);      //微分制御
     //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());     //トルク配分の最低車速制限
 
     torqueRight = requestTorque + distributionTrq;
@@ -518,8 +542,8 @@
         torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
     }
 
-    McpData.valA = calcTorqueToVoltage(torqueRight, RIGHT_MOTOR);
-    McpData.valB = calcTorqueToVoltage(torqueLeft, LEFT_MOTOR);
+    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);
@@ -543,7 +567,7 @@
 
     mcp.writeA(0);   //右モーター
     mcp.writeB(0);   //左モーター
-    
+
     printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE_POWER);
     printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE_REGENERATIVE);
     printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_DISTRIBUTION_TORQUE);