|
|
@@ -228,16 +228,17 @@ int16_t SpeedSet = 0; //
|
|
|
uint32_t accStep = 0; // 加速时间步进
|
|
|
uint32_t decStep = 0; // 减速时间步进
|
|
|
int16_t SpeedSetReal = 0; // 速度设定真实值
|
|
|
-
|
|
|
+int32_t Ref_Speed_Temp_End = 0; //速度环最终输出
|
|
|
+FlagStatus ExitGasModeFlag = RESET;
|
|
|
/*指拨模式相关变量*/
|
|
|
|
|
|
//指拨模式处理
|
|
|
MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, MC_GearSt_Struct_t GearSt)
|
|
|
{
|
|
|
int32_t Tmp;
|
|
|
- int16_t TorQueBySpd = 0;
|
|
|
+ static int16_t TorQueBySpd = 0;
|
|
|
int32_t Ref_Speed_Temp;
|
|
|
- int16_t SpdMotorByIdc = 0;
|
|
|
+ static int16_t SpdMotorByIdc = 0;
|
|
|
|
|
|
MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0, 0, RESET};
|
|
|
|
|
|
@@ -245,16 +246,15 @@ MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, MC_GearSt
|
|
|
|
|
|
/*车轮速度使用原始数据，滤波后的数据有滞后，影响控制回路*/
|
|
|
wheelSpeed = (int16_t)MC_SpeedSensorData.Speed_Data;
|
|
|
-
|
|
|
- /*实时计算电机转速与车轮速的比值*/
|
|
|
- SpdMotorDivWheel = (uint32_t)(MC_RunInfo.MotorSpeed * 100) / wheelSpeed ;
|
|
|
- Tmp = SpdMotorDivWheel;
|
|
|
- SpdMotorDivWheelFlt += ((Tmp << 8) - SpdMotorDivWheelFlt) >> 6;
|
|
|
- SpdMotorDivWheelFlted = SpdMotorDivWheelFlt >> 8;
|
|
|
-
|
|
|
+
|
|
|
/*在电机转速与车轮速比值，与实际速比一致时，更新速比*/
|
|
|
if(( MC_RunInfo.MotorSpeed > 100 ) && ( wheelSpeed > 0 ))
|
|
|
{
|
|
|
+ /*实时计算电机转速与车轮速的比值*/
|
|
|
+ SpdMotorDivWheel = (uint32_t)(MC_RunInfo.MotorSpeed * 100) / wheelSpeed ;
|
|
|
+ Tmp = SpdMotorDivWheel;
|
|
|
+ SpdMotorDivWheelFlt += ((Tmp << 8) - SpdMotorDivWheelFlt) >> 6;
|
|
|
+ SpdMotorDivWheelFlted = SpdMotorDivWheelFlt >> 8;
|
|
|
/*加速时，更新速比，比较法*/
|
|
|
if((wheelSpeed - wheelSpeedPre ) > 5)
|
|
|
{
|
|
|
@@ -296,12 +296,9 @@ MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, MC_GearSt
|
|
|
|
|
|
/*电机最高速度,上位机配置参数*/
|
|
|
SpeedMax = MC_MotorParam.Rate_Speed;
|
|
|
- Tmp = SensorData + 50 ; //加50偏移量，确保能达到最大值2048
|
|
|
+ Tmp = SensorData-50; //指拨100启动，减去50能控低速
|
|
|
Tmp = Tmp > 2048 ? 2048 : Tmp;
|
|
|
-
|
|
|
- /*调试用，根据车速限速值，换算指拨对应的设定车速*/
|
|
|
- dbSpdWheelSet = (Tmp * MC_ConfigParam1.SpeedLimit * 10 )>> 11;
|
|
|
-
|
|
|
+ Tmp = (Tmp*Tmp)/2048; //指拨值改为抛物线，低速控制行程变大
|
|
|
/*电机转速设定，根据指拨大小、车轮限速值和速比，换算*/
|
|
|
SpeedSet = ((Tmp * MC_ConfigParam1.SpeedLimit * SpdProportion) / 10 >> 11); //(Tmp >> 11) * cd_Speedlimit * ( SpdProportion / 10)
|
|
|
|
|
|
@@ -318,25 +315,24 @@ MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, MC_GearSt
|
|
|
switch (GearSt & 0x0F)
|
|
|
{
|
|
|
case 0x01:
|
|
|
- accStep = StepCalc(SpeedMax, 1, 6500);
|
|
|
+ accStep = StepCalc(SpeedMax, 1, 2000);
|
|
|
break;
|
|
|
case 0x02:
|
|
|
- accStep = StepCalc(SpeedMax, 1, 6000);
|
|
|
+ accStep = StepCalc(SpeedMax, 1, 1500);
|
|
|
break;
|
|
|
case 0x03:
|
|
|
- accStep = StepCalc(SpeedMax, 1, 5500);
|
|
|
+ accStep = StepCalc(SpeedMax, 1, 1000);
|
|
|
break;
|
|
|
case 0x04:
|
|
|
- accStep = StepCalc(SpeedMax, 1, 5000);
|
|
|
+ accStep = StepCalc(SpeedMax, 1, 500);
|
|
|
break;
|
|
|
default:
|
|
|
/*计算周期1ms, 加减速时间为 5.00s 加减速步进计算*/
|
|
|
- accStep = StepCalc(SpeedMax, 1, 5000);
|
|
|
+ accStep = StepCalc(SpeedMax, 1, 1000);
|
|
|
break;
|
|
|
}
|
|
|
-
|
|
|
/*减速步进*/
|
|
|
- decStep = StepCalc(SpeedMax, 1, 5500);
|
|
|
+ decStep = StepCalc(SpeedMax, 1, 1000);
|
|
|
|
|
|
/* 跟踪启动 */
|
|
|
if(MC_CalParam.Foc_Flag == RESET)
|
|
|
@@ -377,58 +373,15 @@ MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, MC_GearSt
|
|
|
|
|
|
#endif
|
|
|
|
|
|
- /*限制母线电流*/
|
|
|
- #if 0 //根据档位设定最大电流，电流环在内环，不稳定
|
|
|
-
|
|
|
- static uint16_t CurrentLimitPresent; //限流实际值，做升降速处理
|
|
|
- uint16_t CurrentLimitSet; //限流设置值，不同助力档位更新
|
|
|
-
|
|
|
- switch(GearSt & 0x0F)
|
|
|
- {
|
|
|
- case 0x01://ECO
|
|
|
- {
|
|
|
- CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_ECO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17);
|
|
|
- CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
|
|
|
- break;
|
|
|
- }
|
|
|
- case 0x02://NORM
|
|
|
- {
|
|
|
- CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_NORM.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17);
|
|
|
- CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
|
|
|
- break;
|
|
|
- }
|
|
|
- case 0x03://SPORT
|
|
|
- {
|
|
|
- CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_SPORT.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17);
|
|
|
- CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
|
|
|
- break;
|
|
|
- }
|
|
|
- case 0x04://TURBO
|
|
|
- {
|
|
|
- CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_TURBO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17);
|
|
|
- CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
|
|
|
- break;
|
|
|
- }
|
|
|
- default://SMART
|
|
|
- {
|
|
|
- CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_SMART.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17);
|
|
|
- CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- SpdMotorByIdc = PID_Regulator(CurrentLimitPresent, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower);
|
|
|
-
|
|
|
- #else
|
|
|
-
|
|
|
- SpdMotorByIdc = PID_Regulator((MC_ConfigParam1.CurrentLimit * 1000) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower);
|
|
|
-
|
|
|
- #endif
|
|
|
-
|
|
|
/* 电机速度闭环 */
|
|
|
//最大力矩为4档的力矩参数
|
|
|
- TorQueBySpd = PID_Regulator(SpeedSetReal, MC_HallSensorData.motorspeed_RCFlt, &PID_MotorSpd);
|
|
|
-
|
|
|
- TorQueBySpd += SpdMotorByIdc;
|
|
|
+ PID_MotorSpd.hLower_Limit_Output = -200;
|
|
|
+ PID_MotorSpd.hUpper_Limit_Output = 2100;
|
|
|
+ TorQueBySpd = PID_Regulator(SpeedSetReal>>1, MC_HallSensorData.motorspeed_RCFlt>>1, &PID_MotorSpd);
|
|
|
+ PID_MotorSpd.hLower_Limit_Output = -100;
|
|
|
+ PID_MotorSpd.hUpper_Limit_Output = 1050;
|
|
|
+ /*限制母线电流*/
|
|
|
+ SpdMotorByIdc = PID_Regulator((MC_ConfigParam1.CurrentLimit * 1000) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower);
|
|
|
|
|
|
#if 1
|
|
|
static uint16_t K_ByVoltage_Set_Old = 1024;
|
|
|
@@ -455,6 +408,29 @@ MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, MC_GearSt
|
|
|
|
|
|
#endif
|
|
|
|
|
|
+
|
|
|
+
|
|
|
+ Ref_Speed_Temp = ((int32_t)TorQueBySpd * K_ByVoltage_Result) >> 10;
|
|
|
+ Ref_Speed_Temp = ((int32_t)Ref_Speed_Temp * K_ByTemperature_Result) >> 10;
|
|
|
+
|
|
|
+ if(ExitGasModeFlag == RESET)
|
|
|
+ {
|
|
|
+ if((Ref_Speed_Temp - Ref_Speed_Temp_End) > 2)
|
|
|
+ {
|
|
|
+ Ref_Speed_Temp_End += 2;
|
|
|
+ }
|
|
|
+ else if((Ref_Speed_Temp - Ref_Speed_Temp_End) < (-3))
|
|
|
+ {
|
|
|
+ Ref_Speed_Temp_End -= 3;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if( Ref_Speed_Temp_End > 10) Ref_Speed_Temp_End -= 9;
|
|
|
+ else ExitGasModeFlag = RESET;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(Ref_Speed_Temp_End < -200) Ref_Speed_Temp_End=-200;
|
|
|
//速度环控制量为0时停机，防止电机出现异响
|
|
|
if(SpeedSetReal == 0)
|
|
|
{
|
|
|
@@ -466,10 +442,7 @@ MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, MC_GearSt
|
|
|
MC_MotorStar(&MC_StarFlag);
|
|
|
}
|
|
|
|
|
|
- Ref_Speed_Temp = ((int32_t)TorQueBySpd * K_ByVoltage_Result) >> 10;
|
|
|
- Ref_Speed_Temp = ((int32_t)Ref_Speed_Temp * K_ByTemperature_Result) >> 10;
|
|
|
-
|
|
|
- p_MC_CalParam.Ref_Speed = (int16_t)(Ref_Speed_Temp);
|
|
|
+ p_MC_CalParam.Ref_Speed = (int16_t)((Ref_Speed_Temp_End+SpdMotorByIdc)>>1);
|
|
|
p_MC_CalParam.Foc_Flag = SET;
|
|
|
p_MC_CalParam.AssistRunMode = MC_AssistRunMode_GAS;
|
|
|
|
|
|
@@ -1268,8 +1241,26 @@ void MC_CalParam_Cal(MC_WorkMode_Struct_t p_MC_WorkMode, \
|
|
|
MC_AssistRunMode_Temp = MC_AssistRunMode_INVALID;
|
|
|
MC_AssistRunMode_ShiftFlag = SET;
|
|
|
|
|
|
- SpeedSetMiddle = 0; //指拨模式，清零速度中间量
|
|
|
- SpdMotorDivWheelFlt = 0;
|
|
|
+
|
|
|
+ if(p_MC_CalParam->AssistRunMode==MC_AssistRunMode_GAS) //退出指拨模式
|
|
|
+ {
|
|
|
+ if(Ref_Speed_Temp_End > 20)
|
|
|
+ {
|
|
|
+ MC_AssistRunMode_Temp = MC_AssistRunMode_GAS;
|
|
|
+ MC_AssistRunMode_ShiftFlag = RESET;
|
|
|
+ ExitGasModeFlag = SET;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ SpdMotorDivWheelFlt = 0;
|
|
|
+ SpeedSetMiddle = 0;
|
|
|
+ SpeedSetReal = 0;
|
|
|
+ Ref_Speed_Temp_End=0;
|
|
|
+ ExitGasModeFlag = RESET;
|
|
|
+ MC_AssistRunMode_Temp = MC_AssistRunMode_INVALID;
|
|
|
+ MC_AssistRunMode_ShiftFlag = SET;
|
|
|
+ }
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
//助力模式处理
|
white.xu