feat(assist): 停机逻辑优化。

User project/3.BasicFunction/Include/AssistCurve.h

@@ -43,7 +43,7 @@
 #define TORQUE_START_THRESHOLD   80 // 0.1Nm
 #define TORQUE_STOP_THRESHOLD    20 // 0.1Nm
 #define TORQUE_SWITCH2_THRESHOLD 120 // 0.1Nm
-#define TORQUE_SWITCH1_THRESHOLD 40 // 0.1Nm
+#define TORQUE_SWITCH1_THRESHOLD 60 // 0.1Nm
 
 #define BIKE_ASS_MOTOR_CURRENT_MAX      5500 // 0.01A
 #define BIKE_ASS_MOTOR_TORQUE_MAX       20   // 0.1Nm
@@ -65,7 +65,7 @@
 
 #define ASS_LINER_TORQUE_DEFAULT             \
     {                                  \
-        2867, 6096, 9000, 14288, 4096 \
+        2867, 4096, 6144, 8192, 4096 \
     } // Q12
 //#define TORQUE_ASSIST_DEFAULT                                                                                                                      \
 //    {                                                                                                                                              \

User project/3.BasicFunction/Source/AssistCurve.c

@@ -1182,7 +1182,7 @@ SWORD   AssitCuvApplPerVolt(void)
     }
    
     Te_Tor_Assit_tempPu = (SLONG)(Polynomial(&ass_CalCoef.uwTorqueAssGain[ass_CalIn.uwGearSt], &TorqCmd, 14)); // Q14  转矩助力曲线
-    Te_Tor_Assit_LinerPu = (SLONG)(((TorqCmd * LinerAssist[ass_CalIn.uwGearSt] )>> 12) + 136);
+    Te_Tor_Assit_LinerPu = (SLONG)(((TorqCmd * LinerAssist[ass_CalIn.uwGearSt -1] )>> 12) + 136);
     if (Te_Tor_Assit_tempPu < Te_Tor_Assit_LinerPu)
     {
       Te_Tor_Assit_tempPu = Te_Tor_Assit_LinerPu;
@@ -1271,54 +1271,6 @@ SWORD   AssitCuvApplPerVolt(void)
         }
         
         
-//        if(ass_CalCoef.StartFlag ==0 )
-//        { 
-//          
-//           VoltCnt++;
-//           ass_CalOut.swVoltLimitPu = tmpVoltargetPu;
-//           if(VoltCnt>=30)
-//           {
-//             ass_CalCoef.StartFlag=1;
-//             VoltCnt=0;
-//           }
-////           if (ass_CalOut.swVoltLimitPu < (tmpVoltargetPu-200))
-////           {
-////              ass_CalOut.swVoltLimitPu +=200;
-////              
-////           }
-////           else
-////           {
-////               ass_CalOut.swVoltLimitPu = tmpVoltargetPu;
-////               ass_CalCoef.StartFlag=1;
-////           }
-//        }
-//        else if(ass_CalCoef.StartFlag ==1)
-//        {
-//           ass_CalOut.swVoltLimitPu += ass_CalCoef.uwStartUpGainAddStep;
-//           if(abs(ass_CalIn.swCurRefPu - ass_CalIn.swCurFdbPu)<200)
-//           {
-//              VoltCnt++;
-//           }
-//           else
-//           {
-//              VoltCnt--;
-//              if (VoltCnt<0)
-//              {
-//                VoltCnt=0;
-//              }
-//           }
-//           if(VoltCnt > 30)
-//           {
-//              Ass_FSM = TorqueAssit;
-//              ass_CalCoef.StartFlag=0;
-//              VoltCnt = (((SLONG)abs(ass_CalOut.swVoltLimitPu - tmpVoltargetPu))<<12)/scm_swVsDcpLimPu;
-//           }
-//            else
-//           {
-//           }
-//        }
-//        
-
         /*启动停机判断，瞬时转矩小于停机值持续1/4圈*/
         if(ass_CalIn.uwcadancePer == 0)
         {
@@ -1384,11 +1336,11 @@ SWORD   AssitCuvApplPerVolt(void)
         
           ///////////test/////////
           
-          if(ass_CalIn.uwtorquePer >= ass_CalCoef.uwSwitch1TorqThreshold)
+          if(ass_CalIn.uwtorque >= ass_CalCoef.uwSwitch1TorqThreshold)
           {             
               ass_CalOut.swVoltLimitPu +=  ass_CalCoef.uwStartUpGainAddStep;
           }
-          else if (ass_CalIn.uwtorquePer <= ass_CalCoef.uwSwitch1TorqThreshold)
+          else if (ass_CalIn.uwtorque <= ass_CalCoef.uwSwitch1TorqThreshold)
           {
               ass_CalOut.swVoltLimitPu -=  ass_CalCoef.uwSpeedConstantCommand;
           }
@@ -1638,7 +1590,7 @@ SWORD   AssitCuvApplPerVolt(void)
         break;
 
     case  ReduceCurrent:
-        if ((ass_CalCoef.swSmoothGain <= 0) && (abs(ass_CalIn.swSpdFbkPu)< 400))
+       if (abs(ass_CalIn.swSpdFbkPu)< 500)
         {
             ass_CalCoef.swSmoothGain = 0;
             ass_CalCoef.swTorqFilterGain = 0;
@@ -1647,22 +1599,31 @@ SWORD   AssitCuvApplPerVolt(void)
             ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu; // Q15  Q10 (9.625)
             asr_stTorqSpdPIOut.slIqRefPu = ((SLONG)ass_CalOut.swTorAssistCurrent) << 16;
             asr_stTorqSpdPIOut.swIqRefPu = ass_CalOut.swTorAssistCurrent;
-            Ass_FSM = StopAssit;
-            
+            Ass_FSM = StopAssit;     
         }  
         else
         {
-            ass_CalCoef.swSmoothGain -= 40;
-            ass_CalOut.swVoltLimitPu -= 20;
-            if(ass_CalOut.swVoltLimitPu <= 0)
+            if(ass_CalCoef.swSmoothGain >=40)
+            {
+                ass_CalCoef.swSmoothGain -=40;
+            }
+            else
+            {
+                ass_CalCoef.swSmoothGain=0;
+            }
+
+            if(ass_CalOut.swVoltLimitPu >=20)
+            {
+                ass_CalOut.swVoltLimitPu -= 20;
+            }
+            else
             {
                 ass_CalOut.swVoltLimitPu = 0;
             }
-            
         }
         break;
+       
     case StopAssit:
- 
         ass_CalOut.swTorSpdLoopCurrentTemp = 0;
         /* 启动判断*/
        if (ass_CalIn.uwbikespeed < 449) // 0.3Hz,  (2.19m轮径下  2.36km/h )
@@ -1692,22 +1653,6 @@ SWORD   AssitCuvApplPerVolt(void)
             }
         }
        
-//        if (ass_CalIn.uwtorquelpf > ass_CalCoef.uwAssThreshold && ass_CalIn.uwcadance > 0)
-//        {
-////            hw_voPWMOn();
-//            if (ass_CalIn.uwbikespeed == 0)
-//            {
-//                Ass_FSM = Startup;
-//                ass_CalCoef.swTorqFilterGain = 0;
-//            }
-//            else
-//            {
-//                ass_CalCoef.swTorqFilterGain = 0;
-//                Ass_FSM = Spd2Torq;
-//                ass_CalOut.swSpeedRef = ass_CalIn.uwSpdFbkAbsPu;
-//            }
-//            ass_CalCoef.sw2StopCNT = 0;
-//        }
         /* 退出助力判断*/
         if (ass_CalIn.uwcadance == 0 || ass_CalIn.uwtorquelpf < ass_CalCoef.uwAssStopThreshold)
         {
@@ -1766,16 +1711,42 @@ SWORD   AssitCuvApplPerVolt(void)
         {
             ass_CalOut.swCadAss2CurrentTemp = ass_CalCoef.swCurrentmax_cadAssPu;
         }
-       ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;
-       ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
-       if (ass_CalCoef.StartFlag==0)
-       {
-            ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * 13000;
-       }
-       else
-       {
+       ass_CalOut.swTorRefTarget = ass_CalOut.swTorAss2CurrentTemp + ass_CalOut.swCadAss2CurrentTemp;      
+        if((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) > 2)
+        {
+            TorquAccCnt++;
+            if(TorquAccCnt >= 2)
+            {
+                ass_CalOut.swTorRefEnd += TorqueAccStep; 
+                TorquAccCnt = 0;
+            }          
+        }  
+        else if(((ass_CalOut.swTorRefTarget - ass_CalOut.swTorRefEnd) < (-1)))
+        {
+            if (ass_CalIn.uwcadance != ass_CalIn.uwcadancelast)
+            {
+                ass_CalOut.swTorRefEnd -= TorqueDecStep;               
+            }
+//            TorquDecCnt++;
+//            if(TorquDecCnt >= 10)
+//            {
+//                ass_CalOut.swTorRefEnd += TorqueAccStep; 
+//                TorquDecCnt = 0;
+//            }    
+        }
+        else
+        {
+           ass_CalOut.swTorRefEnd = ass_CalOut.swTorRefTarget;
+        }
+
+//       if (ass_CalCoef.StartFlag==0)
+//       {
+//            ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection * 13000;
+//       }
+//       else
+//       {
             ass_CalOut.swTorAssistCurrentTemp = ass_CalIn.swDirection *ass_CalOut.swTorRefEnd;
-       }
+//       }
        
        
     break;  

User project/3.BasicFunction/Source/torquesensor.c

@@ -235,6 +235,8 @@ void torsensor_voTorSensorCof(void)
  Reference: N/A
 ****************************************************************/
 LPF_OUT tst_dynOffsetLpf;
+UWORD tsttorqCadCnt,tsttorqMin=4096,tstdynOffset;
+
 void torsensor_voTorSensorInit(void)
 {
     torsensor_stTorSensorOut.uwTorqueReg = 0;
@@ -244,6 +246,7 @@ void torsensor_voTorSensorInit(void)
     torsensor_stTorSensorOut.blTorqueCaliFlg = FALSE;
     torsensor_stTorSensorOut.blTorqueErrorFlg = FALSE;
     mth_voLPFilterCoef(1000000 / 1, EVENT_1MS_HZ, &tst_dynOffsetLpf.uwKx); //25Hz  
+    tstdynOffset= hw_uwADC0[7];
     tst_dynOffsetLpf.slY.sw.hi =  hw_uwADC0[7];
         /* Torque Sensor limit coef */
    
@@ -252,7 +255,6 @@ void torsensor_voTorSensorInit(void)
 /*************************************************************************
  Local Functions (N/A)
 *************************************************************************/
-UWORD tsttorqCadCnt,tsttorqMin=4096,tstdynOffset;
 BOOL tstDynCalibflg= TRUE;
 UWORD tstTorqOffset,tstSensitiveset,TorqValue,TorqValuePu, TorqReg;
 SWORD tstTorqTemp,tstTorqTemp111,tstSencitiveOrig;
@@ -299,7 +301,7 @@ void torsensor_voTorADCwithTemp(void)
 {
     tstTorqTemp = temp_swTorqTempCal(hw_uwADC0[4]); // 0.1 C
 //    tstTorqOffset = torsensor_uwTorqOffsetCal(tstTorqTemp); // Torque AD 
-    tstTorqOffset= 1478; //tst_dynOffsetLpf.slY.sw.hi;    //tstdynOffset;
+    tstTorqOffset= tst_dynOffsetLpf.slY.sw.hi;//  1478;   //tstdynOffset;
     tstSensitiveset = torsensor_uwTorqSencitiveCal(tstTorqTemp/10, 250); //Q12
 
     torsensor_stTorSensorOut.uwTorqueReg = hw_uwADC0[7];