Diff: TVDCTRL.cpp

Revision:

25:c21d35c7f0de

Parent:

24:1de0291bc5eb

Child:

26:331e77bb479b

--- a/TVDCTRL.cpp	Thu Jun 29 01:58:49 2017 +0000
+++ b/TVDCTRL.cpp	Sat Jul 01 00:26:28 2017 +0000
@@ -107,7 +107,7 @@
     loadSensorFlag = true;
 }
 
-//関数内処理時間より短い時間のタイマーのセットは禁止
+//センサ読み込み関数
 void loadSensors(void)
 {
     if(true == loadSensorFlag) {
@@ -219,88 +219,59 @@
     }
 }
 
-volatile int gRightPulseTime=100000, gLeftPulseTime=100000;
+volatile int gRightMotorPulseCounter = 0, gLeftMotorPulseCounter = 0;
 volatile bool pulseTimeISRFlag = false;
 
-void countRightPulseISR(void)
+void countRightMotorPulseISR(void)
 {
-    //Do not use "printf" in interrupt!!!
-    static int preTime=0;
-    int currentTime = RightPulseTimer.read_us();
-
-    gRightPulseTime = currentTime - preTime;
-
-    if(gRightPulseTime < MIN_PULSE_TIME)  //12000rpm上限より早い場合
-        gRightPulseTime = MIN_PULSE_TIME;
-
-    if(currentTime < 1800000000) {
-        preTime = currentTime;
-    } else {    //30分経過後
-        RightPulseTimer.reset();
-        preTime = 0;
-    }
+    gRightMotorPulseCounter++;
 }
 
-void countLeftPulseISR(void)
+void countLeftMotorPulseISR(void)
 {
-    //Do not use "printf" in interrupt!!!
-    static int preTime=0;
-    int currentTime = LeftPulseTimer.read_us();
-
-    gLeftPulseTime = currentTime - preTime;
-
-    if(gLeftPulseTime < MIN_PULSE_TIME)  //12000rpm上限より早い場合
-        gLeftPulseTime = MIN_PULSE_TIME;
-
-    if(currentTime < 1800000000) {
-        preTime = currentTime;
-    } else {    //30分経過後
-        LeftPulseTimer.reset();
-        preTime = 0;
-    }
+    gLeftMotorPulseCounter++;
 }
 
-void getPulseTimeISR(void)
+void getPulseCounterISR(void)
 {
     pulseTimeISRFlag = true;
 }
 
-int getPulseTime(SelectMotor rl)
+float getRPS(SelectMotor rl)
 {
-    static int preRightPulse, preLeftPulse;
+    static int rightMotorPulse[100]= {0}, leftMotorPulse[100]= {0};     //過去1秒間のパルス数格納
+    static int sumRightMotorPulse, sumLeftMotorPulse;
 
     if(pulseTimeISRFlag == true) {
-        pulseTimeISRFlag = false;
+        for(int i=99; i>0; i--) {
+            rightMotorPulse[i] = rightMotorPulse[i-1];
+            leftMotorPulse[i] = leftMotorPulse[i-1];
+        }
+
+        rightMotorPulse[0] = gRightMotorPulseCounter;
+        leftMotorPulse[0] = gLeftMotorPulseCounter;
 
-        if(gRightPulseTime > MAX_PULSE_TIME)    //最大パルス時間にクリップ
-            gRightPulseTime = MAX_PULSE_TIME;
-        if(gLeftPulseTime > MAX_PULSE_TIME)
-            gLeftPulseTime = MAX_PULSE_TIME;
+        gRightMotorPulseCounter = 0;
+        gLeftMotorPulseCounter = 0;
 
-        preRightPulse = (int)(gRightPulseTime*ratioLPF_V + preRightPulse*(1.0f-ratioLPF_V));
-        preLeftPulse = (int)(gLeftPulseTime*ratioLPF_V + preLeftPulse*(1.0f-ratioLPF_V));
+        sumRightMotorPulse = 0;
+        sumLeftMotorPulse = 0;
+
+        for(int i=0; i<100; i++) {
+            sumRightMotorPulse += rightMotorPulse[i];
+            sumLeftMotorPulse += leftMotorPulse[i];
+        }
     }
 
     if(rl == RIGHT_MOTOR)
-        return preRightPulse;
+        return ((float)sumRightMotorPulse / MOTOR_PULSE_NUM) * 60.0;    //過去1秒間のモータパルス数を使ってRPM算出
     else
-        return preLeftPulse;
+        return ((float)sumLeftMotorPulse / MOTOR_PULSE_NUM) * 60.0;     //過去1秒間のモータパルス数を使ってRPM算出
 }
 
 float getVelocity(void)
 {
-    int rightPulse=0, leftPulse=0;
-    int avePulseTime;
-
-    rightPulse = getPulseTime(RIGHT_MOTOR);
-    leftPulse = getPulseTime(LEFT_MOTOR);
-
-    avePulseTime = (int)((rightPulse+leftPulse)/2.0);
-
-    if(avePulseTime < MIN_PULSE_TIME)    //最低パルス時間にクリップ
-        avePulseTime = MIN_PULSE_TIME;
-
-    return (float)(M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0)*TVD_GEAR_RATIO));
+    return TIRE_DIAMETER*M_PI*(getRPS(RIGHT_MOTOR) + getRPS(LEFT_MOTOR))/2.0;
 }
 
 int distributeTorque_omega(float steering)
@@ -332,64 +303,107 @@
 {
     int disTrq = 0;
     const float deadband = (M_PI/180.0f)*5.0f;
+    int steeringSign;
+
+    if(steering > 0)
+        steeringSign = 1;
+    else
+        steeringSign = -1;
+
+    steering = myAbs(steering);
 
     if(steering < deadband)
         disTrq = 0;
     else if(steering < M_PI*0.5) {
         steering -= deadband;
         disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI*0.5 - deadband) * steering);
-    } else
+    } else {
         disTrq = MAX_DISTRIBUTION_TORQUE;
-    /*else {
-            steering -= deadband;
-            disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI - deadband) * steering);
-        }
-    */
-    //pc.printf("%1.2f\r\n", 45.0/0xffff*disTrq);
+    }
 
-    return disTrq;
+    return disTrq * steeringSign;
 }
 
-//トルク値線形補間関数
-inline int interpolateLinear(int torque, int currentMaxTorque)
+//rpm +++++ モータ回転数
+//regSwitch +++++ 回生=1 力行=0
+inline int calcMaxTorque(int rpm, bool regSwitch)
 {
-    return (int)(((double)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(currentMaxTorque-LINEAR_REGION_TORQUE)) * (torque-LINEAR_REGION_TORQUE)) + LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN;
+    int maxTrq=0;
+
+    //後で削除
+    rpm = 2000;
+
+    //++++++++++++++++++++
+    if(regSwitch == 0) {
+        if(rpm <3000)
+            maxTrq = MAX_MOTOR_TORQUE_POWER;
+        else if(rpm <= 11000)
+            maxTrq = maxTorqueMap[(int)(rpm / 10.0)];
+        else
+            maxTrq = MAX_REVOLUTION_TORQUE_POWER;
+    } else {
+        if(rpm < 1250) {
+            maxTrq = 0;
+        } else if(rpm <= 6000) {
+            //+++++++++++++++++++++++++++++++++++++
+            //暫定処理 今後回生トルクマップも要作成
+            maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE;
+            //+++++++++++++++++++++++++++++++++++++
+        } else {
+            maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
+        }
+    }
+    return maxTrq;
 }
 
-unsigned int calcTorqueToVoltage(int torque, SelectMotor rl)
+//演算方法
+//y = a(x - b) + c
+//x = 1/a * (y + ab - c)
+unsigned int calcTorqueToVoltage(int reqTorque, int rpm)
 {
-    int outputVoltage=0;
-    int rpm=0;
-    int currentMaxTorque=0;
+    float slope = 0;        //a
+    int startPoint = 0;     //b
+    int intercept = 0;      //c
 
-    if(torque <= LINEAR_REGION_TORQUE) {         //要求トルク<=2.5Nmの時
-        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]
+    int outputVoltage=0;
 
-        rpm = 0;
+    if(reqTorque > LINEAR_REGION_TORQUE_POWER) {        //力行トルクがrpmに対して非線形となる領域
+        slope = (float)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/(DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER);
+        startPoint = LINEAR_REGION_VOLTAGE_POWER;
+        intercept = LINEAR_REGION_TORQUE_POWER;
+
+        outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
 
-        if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がないフラットな領域
-            outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
-        } else {
-            if(rpm <= 11000) {
-                int index = (int)((rpm - 3000)/10.0);    //マップは10rpm刻みに作成
-                currentMaxTorque = calcMaxTorque[index];
-            } else {
-                currentMaxTorque = MAX_REVOLUTION_TORQUE;      //回転数上限時の最大トルク
-            }
+    } else if(reqTorque > 0) {                          //力行トルクがrpmに対して線形となる領域
+        slope = (float)LINEAR_REGION_TORQUE_POWER/(LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P);
+        startPoint = ZERO_TORQUE_VOLTAGE_P;
+        intercept = 0;
+
+        outputVoltage = (int)(reqTorque/slope + startPoint);
+
+    } else if(0 == reqTorque) {
+
+        outputVoltage = ZERO_TORQUE_VOLTAGE_NEUTRAL;    //ニュートラル信号
 
-            if(currentMaxTorque < torque) {     //要求トルクが現在の回転数での最大値を超えている時
-                outputVoltage = DACOUTPUT_VALID_RANGE;  //現在の回転数での最大トルクにクリップ
-            } else {
-                outputVoltage = interpolateLinear(torque, currentMaxTorque);
-            }
-        }
+    } else if(reqTorque > LINEAR_REGION_TORQUE_REGENERATIVE) {  //回生トルクがrpmに対して線形となる領域
+        slope = (float)(0 - LINEAR_REGION_TORQUE_REGENERATIVE)/(ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE);
+        startPoint = LINEAR_REGION_VOLTAGE_REGENERATIVE;
+        intercept = LINEAR_REGION_TORQUE_REGENERATIVE;
+
+        outputVoltage = (int)(reqTorque/slope + startPoint);
+
+    } else {                                            //回生トルクがrpmに対して非線形となる領域
+        slope = (float)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
+        startPoint = DACOUTPUT_MIN;
+        intercept = calcMaxTorque(rpm, 1);
+
+        outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
     }
 
-    outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
-
-    //printf("%d\r\n", (int)(0xFFF*((double)outputVoltage/0xFFFF)));
+    if(outputVoltage > DACOUTPUT_MAX)
+        outputVoltage = DACOUTPUT_MAX;
+    if(outputVoltage < DACOUTPUT_MIN)
+        outputVoltage = DACOUTPUT_MIN;
 
     return (unsigned int)(0xFFF*((double)outputVoltage/0xFFFF));  //DACの分解能に適応(16bit->12bit)
 }
@@ -406,15 +420,15 @@
     else
         currentAPS -= APS_MIN_POSITION;     //オフセット修正
 
-    if(currentAPS < APS_DEADBAND)   //デッドバンド内であれば要求トルク->0
-        requestTorque = 0;
+    if(currentAPS <= APS_REG_RANGE)   //デッドバンド内であれば要求トルク->0
+        requestTorque = (int)((float)(-MAX_OUTPUT_TORQUE_REGENERATIVE) / APS_REG_RANGE * currentAPS + MAX_OUTPUT_TORQUE_REGENERATIVE);
     else
-        requestTorque = (int)(((double)MAX_OUTPUT_TORQUE / APS_VALID_RANGE) * (currentAPS - APS_DEADBAND));
+        requestTorque = (int)((float)MAX_OUTPUT_TORQUE_POWER / APS_PWR_RANGE * (currentAPS - APS_REG_RANGE));
 
-    if(requestTorque > MAX_OUTPUT_TORQUE)
-        requestTorque = MAX_OUTPUT_TORQUE;
-    else if(requestTorque < 0)
-        requestTorque = 0;
+    if(requestTorque > MAX_OUTPUT_TORQUE_POWER)
+        requestTorque = MAX_OUTPUT_TORQUE_POWER;
+    else if(requestTorque < MAX_OUTPUT_TORQUE_REGENERATIVE)
+        requestTorque = MAX_OUTPUT_TORQUE_REGENERATIVE;
 
     if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
         requestTorque = 0;
@@ -450,7 +464,6 @@
     int distributionTrq=0;  //分配トルク
     int disTrq_omega=0;
     int torqueRight, torqueLeft;    //トルクの右左
-
     static unsigned int preMcpA=0, preMcpB=0;
 
     loadSensors();      //APS,BRAKE更新
@@ -463,55 +476,50 @@
             || (errCounter.apsExceedVolt > ERRCOUNTER_DECISION)
             || (errCounter.apsErrorTolerance > ERRCOUNTER_DECISION)
 //            || (errCounter.apsStick > ERRCOUNTER_DECISION)
-            || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION)
-            || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
-            || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
+//            || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION)
+//            || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
+//            || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
       ) {
         readyToDriveFlag = 1;
     }
 
+    //+++++++++++++++++++
+    //後で削除すること！
+    readyToDriveFlag = 0;
+    //+++++++++++++++++++
+
     indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
     LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
-    //デバッグ中!!!
-    //requestTorque = (int)(MAX_OUTPUT_TORQUE/2.0f);
 
-    distributionTrq = (int)(distributeTorque(myAbs(getSteerAngle())) / 2.0);  //片モーターのトルク分配量計算
-    disTrq_omega = (int)(distributeTorque_omega(getSteerAngle()) / 2.0);
-    
-    //printf("%d\r\n", disTrq_omega);
-
-    //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());   //トルク配分の最低車速制限
+    distributionTrq = (int)(distributeTorque(getSteerAngle()) / 2.0);  //片モーターのトルク分配量計算
+    //disTrq_omega = (int)(distributeTorque_omega(getSteerAngle()) / 2.0);      //微分制御
+    //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());     //トルク配分の最低車速制限
 
-    //デバッグ中
-    //distributionTrq = 0;
+    printf("%1.2f\r\n", 45.0/0xffff*distributionTrq);
 
-    if(getSteerDirection()) { //steer left
-        torqueRight = requestTorque + distributionTrq;
-        torqueLeft = requestTorque - distributionTrq;
-    } else {    //steer right
-        torqueRight = requestTorque - distributionTrq;
-        torqueLeft = requestTorque + 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) { //片モーター上限時最大値にクリップ
-            torqueLeft = MAX_OUTPUT_TORQUE;
+        if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
+            torqueLeft = MAX_OUTPUT_TORQUE_POWER;
 
             if(((torqueRight + torqueLeft)/2.0) > requestTorque) {
-                torqueRight = requestTorque - (MAX_OUTPUT_TORQUE-requestTorque);
+                torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
             }
         }
-        if(torqueRight > MAX_OUTPUT_TORQUE) { //片モーター上限時最大値にクリップ
-            torqueRight = MAX_OUTPUT_TORQUE;
+        if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
+            torqueRight = MAX_OUTPUT_TORQUE_POWER;
             if(((torqueRight + torqueLeft)/2.0) > requestTorque) {
-                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE-requestTorque);
+                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
             }
         }
         if(torqueLeft < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
@@ -523,14 +531,11 @@
             torqueLeft = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
         }
     }
-
-    //printf("%d %d %f\r\n", torqueRight, torqueLeft, getSteerAngle());
+    */
 
     McpData.valA = calcTorqueToVoltage(torqueRight, RIGHT_MOTOR);
     McpData.valB = calcTorqueToVoltage(torqueLeft, LEFT_MOTOR);
 
-    //pc.printf("%u %u\r\n", McpData.valA, McpData.valB);
-
     preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544);
     preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544);
 
@@ -540,23 +545,22 @@
 
 void initTVD(void)
 {
-    rightMotorPulse.mode(PullUp);
-    leftMotorPulse.mode(PullUp);
-    rightMotorPulse.fall(&countRightPulseISR);
-    leftMotorPulse.fall(&countLeftPulseISR);
-
     RightPulseTimer.reset();
     LeftPulseTimer.reset();
     RightPulseTimer.start();
     LeftPulseTimer.start();
 
-    ticker1.attach(&loadSensorsISR, 0.01f);    //サンプリング周期10msec
-    ticker2.attach(&getPulseTimeISR, 0.01f);
+    rightMotorPulse.fall(&countRightMotorPulseISR);
+    leftMotorPulse.fall(&countLeftMotorPulseISR);
+
+    ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S);   //制御周期毎にデータ読み込み(LPF演算のため)
+    ticker2.attach(&getPulseCounterISR, CONTROL_CYCLE_S);  //
 
     mcp.writeA(0);   //右モーター
     mcp.writeB(0);   //左モーター
 
-    printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE);
+    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);
     printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MIN_INNERWHEEL_MOTOR_TORQUE);
 }