Diff: TVDCTRL.cpp

Revision:

7:ad013d88a539

Parent:

6:26fa8c78500e

Child:

8:a22aec357a64

--- a/TVDCTRL.cpp	Tue Jul 26 05:12:15 2016 +0000
+++ b/TVDCTRL.cpp	Wed Jul 27 03:30:35 2016 +0000
@@ -32,11 +32,6 @@
 volatile int gApsP=0, gApsS=0, gBrake=0;        //現在のセンサ値
 volatile int rawApsP=0, rawApsS=0, rawBrake=0;  //現在の補正無しのセンサ値
 
-float convertVoltage(int count)
-{
-    return (count*(3.3f/0xFFFF));
-}
-
 void getCurrentErrCount(struct errCounter_t *ptr)
 {
     ptr->apsUnderVolt = errCounter.apsUnderVolt;
@@ -49,7 +44,7 @@
     ptr->brakeOverRide = errCounter.brakeOverRide;
 }
 
-int loadCurrentSensor(int sensor)
+int getCurrentSensor(int sensor)
 {
     switch (sensor) {
         case APS_PRIMARY:
@@ -63,7 +58,7 @@
     }
 }
 
-int loadRawSensor(int sensor)
+int getRawSensor(int sensor)
 {
     switch (sensor) {
         case APS_PRIMARY:
@@ -204,7 +199,7 @@
 }
 
 volatile int gRightPulseTime=100000, gLeftPulseTime=100000;
-volatile bool loadVelocityFlag = false;
+volatile bool pulseTimeISRFlag = false;
 
 void countRightPulseISR(void)
 {
@@ -244,17 +239,17 @@
     }
 }
 
-void loadVelocityISR(void)
+void getPulseTimeISR(void)
 {
-    loadVelocityFlag = true;
+    pulseTimeISRFlag = true;
 }
 
 int getPulseTime(SelectMotor rl)
 {
     static int preRightPulse, preLeftPulse;
 
-    if(loadVelocityFlag == true) {
-        loadVelocityFlag = false;
+    if(pulseTimeISRFlag == true) {
+        pulseTimeISRFlag = false;
 
         preRightPulse = (int)(gRightPulseTime*ratioLPF_V + preRightPulse*(1.0f-ratioLPF_V));
         preLeftPulse = (int)(gLeftPulseTime*ratioLPF_V + preLeftPulse*(1.0f-ratioLPF_V));
@@ -312,9 +307,9 @@
 }
 
 //トルク値線形補間関数
-int interpolateLinear(int torque, int currentMaxTorque)
+inline int interpolateLinear(int torque, int currentMaxTorque)
 {
-    return (int)((((float)DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/((float)currentMaxTorque-LINEAR_REGION_TORQUE)) * torque) + LINEAR_REGION_VOLTAGE;
+    return (int)(((double)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(currentMaxTorque-LINEAR_REGION_TORQUE)) * torque) + LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN;
 }
 
 unsigned int calcTorqueToVoltage(int torque, SelectMotor rl)
@@ -324,11 +319,11 @@
     int currentMaxTorque=0;
 
     if(torque <= LINEAR_REGION_TORQUE) {         //要求トルク<=2.5Nmの時
-        outputVoltage = (int)((double)LINEAR_REGION_VOLTAGE/LINEAR_REGION_TORQUE * torque);
+        outputVoltage = (int)((double)(LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN)/LINEAR_REGION_TORQUE * torque);
     } else {
         rpm = (int)(1.0/getPulseTime(rl)*1000000.0 * 60.0);  //pulseTime:[us]
 
-        rpm = 3001;
+        rpm = 12000;
 
         if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がないフラットな領域
             outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
@@ -400,7 +395,7 @@
 {
     int requestTorque=0;    //ドライバー要求トルク
     int distributionTrq=0;  //分配トルク
-    float torqueHigh, torqueLow;    //トルクの大きい方小さい方
+    int torqueHigh, torqueLow;    //トルクの大きい方小さい方
 
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
@@ -408,13 +403,15 @@
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
-    distributionTrq = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
+    //distributionTrq = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
     distributionTrq /= 2.0f;
 
     //配分の制御はなしとする
     //デバッグ終わったらこの行は消すこと!!!!!
     distributionTrq=0;
+    torqueHigh = torqueLow = requestTorque;
 
+    /*
     if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
         torqueHigh = MAX_OUTPUT_TORQUE;
     else
@@ -425,6 +422,7 @@
         torqueHigh = (int)(requestTorque*2.0);   //片モーター下限値時，反対のモーターも出力クリップ
     } else
         torqueLow = requestTorque - distributionTrq;
+    */
 
     if(getSteerDirection()) {
         //steer left
@@ -454,5 +452,5 @@
 
     ticker1.attach(&loadSensorsISR, 0.01f);    //サンプリング周期10msec
     //ticker2.attach(&generatePulse, 0.03f);
-    ticker3.attach(&loadVelocityISR, 0.01f);
+    ticker3.attach(&getPulseTimeISR, 0.01f);
 }