motor_ctl-2/User/Src/motor_control.c

#include "motor_control.h"
#include "MC_FOC_driver.h"
#include "MC_PID_regulators.h"
#include "MC_Globals.h"
#include "stm32f10x_svpwm_3shunt.h"
#include "hall_sensor.h"
#include "cadence_sensor.h"
#include "speed_sensor.h"
#include "torque_sensor.h"
#include "gas_sensor.h"
#include "key_driver.h"
#include "pwm_driver.h"
#include "math_tools.h"
#include "power12V_driver.h"
/************************全局变量定义************************/
//工作模式
MC_WorkMode_Struct_t MC_WorkMode = MC_WorkMode_Run; 
MC_WorkMode_Struct_t MC_WorkMode_Back = ~MC_WorkMode_Run; 
//MC_CTL控制指令
MC_ControlCode_Struct_t MC_ControlCode= {MC_GearSt_OFF, MC_LightSwitch_OFF}; 
MC_ControlCode_Struct_t MC_ControlCode_Back = {(MC_GearSt_Struct_t)~MC_GearSt_OFF, (MC_LightSwitch_Struct_t)~MC_LightSwitch_OFF};
//电机控制计算参数
MC_CalParam_Struct_t MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET}; 
MC_CalParam_Struct_t MC_CalParam_Back = {(MC_AssistRunMode_Struct_t)~MC_AssistRunMode_INVALID, ~0, ~0, (FlagStatus)~RESET}; 
//踏频限流系数
uint8_t MC_CadenceLimit_K = 100;
//力矩助力控制参数
MC_TorqueProcess_Param_Struct_t MC_TorqueProcess_Param = {SET, 0, 0, 0};
//推行助力控制参数
MC_WalkProcess_Param_Struct_t MC_WalkProcess_Param = {FALSE, 0};  
//电机启动标志
FlagStatus MC_StarFlag = RESET;  

/*************************局部函数定义***********************/
//设定值线性变化处理
uint16_t MC_DataSet_Linear_Process(uint16_t SetData, uint16_t PresentData, uint16_t AddCnt, uint16_t DecCnt)
{
  int16_t ErrorData;
	uint16_t Result;
	
	ErrorData = SetData - PresentData;
	if(ErrorData > 0)  //升速
	{
	  if(ErrorData >= AddCnt)
		{
		  Result = PresentData + AddCnt;
		}
		else
		{
		  Result = SetData;
		}
	}
	else if(ErrorData < 0)  //降速
	{
	  if((-ErrorData) >= DecCnt)
		{
		  Result = PresentData - DecCnt;
		}
		else
		{
		  Result = SetData;
		}
	}
	else
	{
	  Result = SetData;
	}
	
	return Result;
}

//随电压计算助力衰减系数
uint16_t MC_Cal_K_ByVoltage(uint16_t Voltage, uint16_t DesignVol, uint16_t K_Voltage_Old)
{
 	uint32_t Cal_Temp;
	uint16_t SetVol_Th = 0;
	uint16_t ResetVol_Th = 0;
	uint16_t Result = 1024;
	
	//根据马达额定电压设定衰减点、衰减系数和恢复点
	switch(DesignVol)
	{
		case 24:
		{
			SetVol_Th = 33 * 7;
			ResetVol_Th = 36 * 7;
			Cal_Temp = (Voltage > (SetVol_Th * 100)) ? 1024 : (uint16_t)((uint32_t)(Voltage) / SetVol_Th);//Voltage单位0.001V, SetVol_Th单位0.1V, 系数放大100倍 
		  break;
		}
		case 36:
		{
			SetVol_Th = 33 * 10;
			ResetVol_Th = 36 * 10;
			Cal_Temp = (Voltage > SetVol_Th * 100) ? 1024 : (uint16_t)((uint32_t)(Voltage) / SetVol_Th);//Voltage单位0.001V, SetVol_Th单位0.1V, 系数放大100倍
		  break;
		}
		case 48:
		{
			SetVol_Th = 33 * 13;
			ResetVol_Th = 36 * 13;
			Cal_Temp = (Voltage > SetVol_Th * 100) ? 1024 : (uint16_t)((uint32_t)(Voltage) / SetVol_Th);//Voltage单位0.001V, SetVol_Th单位0.1V, 系数放大100倍
		  break;
		}
		default:
		{
		  Cal_Temp = 100;
			break;
		}
	}
	Cal_Temp = Cal_Temp * Cal_Temp / 100 * Cal_Temp / 100 * Cal_Temp * 1024 / 10000; //f(x) = x^4
	Cal_Temp = (Cal_Temp < 820) ? 820 : Cal_Temp;
	
	//系数仅衰减
	if(Cal_Temp < K_Voltage_Old)
	{
	  Result = Cal_Temp;
	}
	else
	{
	  Result = K_Voltage_Old;
	}

	//高于设定恢复电压后，恢复系数
	if(Voltage > (ResetVol_Th * 100))
	{
	  Result = 1024;
	}

	return(Result);
}

//随SOC计算限流衰减系数
uint16_t MC_LimitCurrent_Cal_K_BySOC(uint16_t SOC)
{
  uint16_t Limit_K_BySOC = 1024;
	if(SOC <= 2)
		Limit_K_BySOC = 256;
	else if(SOC <= 6)
		Limit_K_BySOC = 256 + (SOC - 2) * 64;
	else if(SOC <= 22)
	  Limit_K_BySOC = 512 + (SOC - 6) * 24;
	else if(SOC <= 30)
		Limit_K_BySOC = 896 + (SOC - 22) * 16;
	else
		Limit_K_BySOC = 1024;
	return Limit_K_BySOC;
}

//随SOC计算限速衰减系数
uint16_t MC_LimitSpeed_Cal_K_BySOC(uint16_t SOC)
{
  uint16_t Limit_K_BySOC = 1024;
	if(SOC <= 2)
		Limit_K_BySOC = 512;
	else if(SOC <= 6)
		Limit_K_BySOC = 512 + (SOC - 2) * 32;
	else if(SOC <= 22)
	  Limit_K_BySOC = 640 + (SOC - 6) * 20;
	else if(SOC <= 30)
		Limit_K_BySOC = 960 + (SOC - 22) * 8;
	else
		Limit_K_BySOC = 1024;
	return Limit_K_BySOC;
}

//随温度计算助力衰减系数
uint16_t MC_Cal_K_ByTemperature(uint16_t CoilTemp, uint16_t AlarmTempTH)
{
  uint32_t CalTemp;
	uint16_t Result = 1024;
	
	if(CoilTemp > AlarmTempTH)
	{
	  CalTemp = (uint32_t)AlarmTempTH * AlarmTempTH * 1024;
		Result = (uint16_t)(CalTemp / CoilTemp / CoilTemp);
	}
	else
	{
	  Result = 1024;
	}
	
	return(Result);
}

//助力模式判断处理
MC_AssistRunMode_Struct_t MC_JudgeAsistRunMode_Process(MC_GasMode_Struct_t GasMode_Param, uint16_t GasSensorData, MC_GearSt_Struct_t GearSt, TrueOrFalse_Flag_Struct_t StopFlag)
{
  MC_AssistRunMode_Struct_t MC_AssistRunMode_Result;
	
	if(MC_ErrorCode.Code == 0)  // 无故障
	{
	  if((GearSt != MC_GearSt_OFF) && (StopFlag == FALSE) && (HAL_GetTick() > 3000))
		{
		  //进入指拨模式
			if((GasSensorData > 200) && (GasMode_Param.Mode != (uint8_t)MC_SUPPORT_DISABLE)) //转把启动电压 = 零点电压 + (TH / 4096 * 3.3) / 10 * 15.6，5V转把0.3V启动
			{
				MC_AssistRunMode_Result = MC_AssistRunMode_GAS;
			}
			//退出指拨模式
			else if(GasSensorData < 100)  
			{
				//进入推行模式
				if(GearSt == MC_GearSt_WALK)
				{
					MC_AssistRunMode_Result = MC_AssistRunMode_WALK;
				}
				else 
				{
					//进入踏频模式
					if(((GearSt & 0xF0) != 0) && (GearSt != MC_GearSt_SMART))
					{
						MC_AssistRunMode_Result = MC_AssistRunMode_CADENCE;
					}
					//进入力矩模式
					else
					{
						MC_AssistRunMode_Result = MC_AssistRunMode_TORQUE;
					}
				}
		  }
		}
		else
		{
	  	MC_AssistRunMode_Result = MC_AssistRunMode_INVALID;
		}
		Power12V_Driver_Process(SET);
	}
	else  //存在故障
	{
	  MC_AssistRunMode_Result = MC_AssistRunMode_INVALID;
		MC_ControlCode.GearSt = MC_GearSt_OFF;
		MC_ControlCode_Back.GearSt = (MC_GearSt_Struct_t)~MC_ControlCode.GearSt;
		#if 0
		Power12V_Driver_Process(RESET);
		#endif
	}
	
	return MC_AssistRunMode_Result;
}

/*指拨模式相关变量*/

static int32_t SpdMotorDivWheelFlt=0;
int16_t SpdProportion=640; //车轮电机速度比
static uint16_t SpdProportion_buff_CNT=0;
uint8_t  SpdProportion_CAL_flag=0;
static uint16_t SpdProportion_Save_CNT=0;
uint16_t SpdProportion_buff[100]={0};
float SpdProportion_StandardDeviation=0;
int32_t test_StandardDeviation=0;
uint16_t test_SpdProportionAver=0;
int32_t SpeedSetMiddle=0;
int16_t dbSpdWheelSet=0;  //调试用

int16_t wheelSpeed=0;
static int16_t DbSpdMotorPre=0;
static int16_t wheelSpeedPre=0;
int16_t SpdMotorDivWheel=0;
int16_t SpdMotorDivWheelFlted=0;
int16_t SpeedMax = 0;	// 最高时速
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_GasSpeed_Process(MC_GasMode_Struct_t GasMode_Param, uint16_t SensorData, MC_GearSt_Struct_t GearSt)
{
	int32_t Tmp;
	static int16_t TorQueBySpd = 0;
	int32_t Ref_Speed_Temp;
	static int16_t SpdMotorByIdc = 0;
	uint8_t SpeedLimit = 0;
	
	MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};

	/*车轮速度使用原始数据，滤波后的数据有滞后，影响控制回路*/
	wheelSpeed = (int16_t)MC_SpeedSensorData.Speed_Data;

	/*在电机转速与车轮速比值，与实际速比一致时，更新速比*/
	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)
		{
			if(( MC_RunInfo.MotorSpeed - DbSpdMotorPre ) > 0)
			{
				SpdProportion = SpdMotorDivWheel ;
			}
			DbSpdMotorPre = MC_RunInfo.MotorSpeed;
		}
		wheelSpeedPre = wheelSpeed;
			
		/*求标准差,速比稳定后，更新速比*/
		/*此处将数据保存到数组中，标准差计算，时间较长，放在主循环进行*/
		if((SpdProportion_CAL_flag==0) && (MC_CalParam.Ref_Speed > 25))  //电机力矩控制量低于25时，认为是空载，此时不更新速比
		{
			SpdProportion_Save_CNT++;
			/*40ms保存一次数据到数组*/
			if(SpdProportion_Save_CNT >= 40 )
			{
				SpdProportion_Save_CNT = 0;
				
				SpdProportion_buff[SpdProportion_buff_CNT] = SpdMotorDivWheelFlted;
				SpdProportion_buff_CNT++;
				if( SpdProportion_buff_CNT >=50 )
				{
					SpdProportion_buff_CNT = 0;
					
					/*标志位置1，主循环里求标准差*/
					SpdProportion_CAL_flag = 1;
				}
			}
		}
	}
	else
	{
	  wheelSpeedPre = wheelSpeed;
		DbSpdMotorPre = MC_RunInfo.MotorSpeed;
	}

	/*电机最高速度,上位机配置参数*/
	SpeedMax = MC_MotorParam.Rate_Speed;
	Tmp = SensorData - 100;  //指拨200启动，减去100能控低速
	Tmp = Tmp > 2048 ? 2048 : Tmp; 
	Tmp = (Tmp * Tmp) / 2048;   //指拨值改为抛物线，低速控制行程变大
	
	/*电机转速设定，根据指拨大小、车轮限速值和速比，换算，以0xAA >> 3*为中心根据整车限速对称偏移，调整上限±15km/h*/
	if(MC_GasMode_Param.Mode_bit.SpeedLimit <= 5)
	  SpeedLimit = ((MC_ConfigParam1.SpeedLimit + (int8_t)MC_ConfigParam2.SpeedLimitAdj + MC_GasMode_Param.Mode_bit.SpeedLimit + 0x1F - (0xAA >> 3)) * MC_LimitSpeed_Cal_K_BySOC(MC_RunInfo.SOC)) >> 10;
	else
		SpeedLimit = ((MC_ConfigParam1.SpeedLimit + (int8_t)MC_ConfigParam2.SpeedLimitAdj + MC_GasMode_Param.Mode_bit.SpeedLimit - (0xAA >> 3)) * MC_LimitSpeed_Cal_K_BySOC(MC_RunInfo.SOC)) >> 10;
	//根据SOC限制最大车速
	SpeedSet = Tmp * (SpeedLimit * SpdProportion) / 10  >> 11; //(Tmp >> 11) * cd_Speedlimit * ( SpdProportion / 10)  	
	
	//超过限速值，设定电机转速为0
	if(wheelSpeed > (SpeedLimit * 10 + 20))
	{
		SpeedSet = 0;
	}
	if(MC_GasMode_Param.Mode_bit.StartMode == 1) //带速启动
	{
	  if(wheelSpeed <= 60)
			SpeedSet = 0;
	}
	
	SpeedSet = (SpeedSet > 0) ? SpeedSet : 0;
	SpeedSet = (SpeedSet < SpeedMax) ? SpeedSet : SpeedMax;
	
	//设定加速度
	if(GasMode_Param.Mode_bit.PowerLimitFlag == 1) //根据档位调整加速度
	{
	  switch (GearSt & 0x0F)
		{
			case 0x01:
				accStep = StepCalc(SpeedMax, 1, 2000);
				break;
			case 0x02:
				accStep = StepCalc(SpeedMax, 1, 1500);
				break;
			case 0x03:
				accStep = StepCalc(SpeedMax, 1, 1000);
				break;
			case 0x04:
				accStep = StepCalc(SpeedMax, 1, 500);
				break;
			default:
				/*计算周期1ms, 加减速时间为 5.00s  加减速步进计算*/
				accStep = StepCalc(SpeedMax, 1, 1000);
				break;
		}
	}
  else //采用Turbo
	{
    accStep = StepCalc(SpeedMax, 1, 500);
	}
	
	/*减速步进*/
	decStep = StepCalc(SpeedMax, 1, 1000);

	/* 跟踪启动 */
	if(MC_CalParam.Foc_Flag  == RESET)			
	{
		//MotorStartFlg = 1;
		if(MC_RunInfo.MotorSpeed > 100)
		{
			SpeedSetReal = MC_RunInfo.MotorSpeed;
			SpeedSetMiddle = SpeedSetReal << 16;
		}
	}
		
	/*速度指令的加减速处理*/
	if(GasMode_Param.Mode_bit.PowerLimitFlag == 1) //根据档位设置目标速度
	{
		switch(GearSt & 0x0F)
	  {
			case 0x01:
				SpeedSetReal = accDecProcess((SpeedSet * 9) >> 4, accStep, decStep, &SpeedSetMiddle);  
				break;
			case 0x02:
				SpeedSetReal = accDecProcess((SpeedSet * 11) >> 4, accStep, decStep, &SpeedSetMiddle);  
				break;
			case 0x03:
				SpeedSetReal = accDecProcess((SpeedSet * 13) >> 4, accStep, decStep, &SpeedSetMiddle);  
				break;
			case 0x04:
				SpeedSetReal = accDecProcess(SpeedSet, accStep, decStep, &SpeedSetMiddle);  
				break;
			default:
				SpeedSetReal = accDecProcess(SpeedSet, accStep, decStep, &SpeedSetMiddle);  
				break;
	  }
	}
  else //采用Turbo
	{
	  SpeedSetReal = accDecProcess(SpeedSet, accStep, decStep, &SpeedSetMiddle);  
	}

	/* 电机速度闭环 */
	//最大力矩为4档的力矩参数
	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 * MC_LimitCurrent_Cal_K_BySOC(MC_RunInfo.SOC)) >> 17, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower);
	
	#if 1
	static uint16_t K_ByVoltage_Set_Old = 1024;
	uint16_t K_ByVoltage_Set;
	static uint16_t K_ByVoltage_Result;
	uint32_t K_ByTemperature_Set, K_ByTemperature_Set1, K_ByTemperature_Set2;
	static uint16_t K_ByTemperature_Result;

	//根据电压调节输出
  K_ByVoltage_Set = MC_Cal_K_ByVoltage(MC_RunInfo.BusVoltage, MC_MotorParam.Rate_Voltage, K_ByVoltage_Set_Old);//根据母线电压计算衰减比例，递减
	K_ByVoltage_Set_Old = K_ByVoltage_Set;
  K_ByVoltage_Result = MC_DataSet_Linear_Process(K_ByVoltage_Set, K_ByVoltage_Result, 1, 1); //设定值与给定值线性处理
	
	//根据温度调节输出
	K_ByTemperature_Set1 = MC_Cal_K_ByTemperature(MC_RunInfo.T_Coil, MC_ConfigParam1.TempTH_Alarm); //根据温度计算衰减比例
	K_ByTemperature_Set2 = MC_Cal_K_ByTemperature(MC_RunInfo.T_PCB, (MC_ConfigParam1.TempTH_Alarm-15)); //根据温度计算衰减比例
	K_ByTemperature_Set  = (K_ByTemperature_Set1 * K_ByTemperature_Set2)>>10;		
	K_ByTemperature_Result = MC_DataSet_Linear_Process(K_ByTemperature_Set, K_ByTemperature_Result, 1, 1); //设定值与给定值线性处理
	
	#else
	
	uint16_t K_ByVoltage_Result = 1024;
	uint16_t K_ByTemperature_Result = 1024;
	
	#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 > 19) Ref_Speed_Temp_End -= 16;
		else ExitGasModeFlag = RESET;
	}
	
	if(Ref_Speed_Temp_End < -200) Ref_Speed_Temp_End=-200;
	//速度环控制量为0时停机，防止电机出现异响
	if(SpeedSetReal == 0)
	{
		MC_MotorStop(&MC_StarFlag);
	}
	else
	{
		//电机启动
		MC_MotorStar(&MC_StarFlag);
	}
	
	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;
	
	return (p_MC_CalParam);
}

//指拨模式力矩控制处理
MC_CalParam_Struct_t MC_AssistRunMode_GasTorque_Process(MC_GasMode_Struct_t GasMode_Param, uint16_t GasSensorData, uint16_t TorqueSensorData, MC_GearSt_Struct_t GearSt)
{
  MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};
  uint8_t TorqueAccStep = 0;//力矩上升斜率
	uint8_t TorqueDecStep = 0;//力矩下降斜率
	int16_t Torque_Temp;
	int32_t Torque_Ref_Temp;
	static int16_t IqRefByInPower;           //限流计算结果
  static uint16_t CurrentLimitPresent;     //限流实际值，做升降速处理
	uint16_t CurrentLimitSet;                //限流设置值，不同助力档位更新	
	static uint8_t TorqueRefEndUpdateCount = 0;
	
	
	//踩踏力矩输入
  MC_TorqueProcess_Param.TorqueApp = (GasSensorData < TorqueSensorData) ? TorqueSensorData : GasSensorData;
	MC_TorqueProcess_Param.TorqueApp = (MC_TorqueProcess_Param.TorqueApp > 2048) ? 2048 : MC_TorqueProcess_Param.TorqueApp;

  //输出目标力矩
  if(GasMode_Param.Mode_bit.PowerLimitFlag == 1) //根据挡位限制功率
	{
		switch(GearSt)
		{
			case MC_GearSt_Torque_ECO:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_ECO.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_ECO.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_ECO.DecCnt;	
				//根据输入调节力矩环给定
				Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_AssisParam.Gear_ECO.Upper_Iq) >> 11;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_ECO.Upper_Iq) ? MC_AssisParam.Gear_ECO.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_ECO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_ECO.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_ECO.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_NORM:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_NORM.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_NORM.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_NORM.DecCnt;	
				//根据输入调节力矩环给定
				Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_AssisParam.Gear_NORM.Upper_Iq) >> 11;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_NORM.Upper_Iq) ? MC_AssisParam.Gear_NORM.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_NORM.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_NORM.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_NORM.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_SPORT:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_SPORT.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_SPORT.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_SPORT.DecCnt;	
				//根据输入调节力矩环给定
				Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_AssisParam.Gear_SPORT.Upper_Iq) >> 11;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_SPORT.Upper_Iq) ? MC_AssisParam.Gear_SPORT.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_SPORT.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_SPORT.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_SPORT.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_TURBO: case MC_GearSt_SMART:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_TURBO.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_TURBO.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_TURBO.DecCnt;	
				//根据输入调节力矩环给定
				Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_AssisParam.Gear_TURBO.Upper_Iq) >> 11;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_TURBO.Upper_Iq) ? MC_AssisParam.Gear_TURBO.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_TURBO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_TURBO.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_TURBO.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			default: 
			{
				TorqueAccStep = 0;
				TorqueDecStep = 0;
				Torque_Temp = 0;
				break;
			}
		}
	}
	else if(GasMode_Param.Mode_bit.PowerLimitFlag == 0) //不根据挡位限制功率，按照Turbo参数
	{
	  //控制输入给定加速斜率
		TorqueAccStep = (MC_AssisParam.Gear_SMART.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_SMART.AccCnt;
		//控制输入给定减速斜率
		TorqueDecStep = MC_AssisParam.Gear_TURBO.DecCnt;	
		//根据输入调节力矩环给定
		Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_AssisParam.Gear_TURBO.Upper_Iq) >> 11;
		//给定上限
		Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_TURBO.Upper_Iq) ? MC_AssisParam.Gear_TURBO.Upper_Iq : Torque_Temp;
		//限流参数设置
		CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_TURBO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
		CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
		PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_TURBO.Upper_Iq);   //Lower Limit for Output limitation
		PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
		PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_TURBO.Upper_Iq << 10); // 放大1024
		PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
	}
	
	//限速值设定
	static uint16_t speedLimitStart = 250, speedLimitEnd = 272, slewRate = 52;
	if((MC_ConfigParam2.SpeedLimitStartAdj > 50) || (MC_ConfigParam2.SpeedLimitStartAdj < -50)) MC_ConfigParam2.SpeedLimitStartAdj=0;
	if((MC_ConfigParam2.SpeedLimitEndAdj > 50) || (MC_ConfigParam2.SpeedLimitEndAdj < -50)) MC_ConfigParam2.SpeedLimitEndAdj=0;
	
  if(MC_GasMode_Param.Mode_bit.SpeedLimit <= 5)
	{
	  speedLimitStart = (((MC_ConfigParam1.SpeedLimit + (int8_t)MC_ConfigParam2.SpeedLimitAdj + GasMode_Param.Mode_bit.SpeedLimit + 0x1F - (0xAA >> 3) - 2) * 10 + MC_ConfigParam2.SpeedLimitStartAdj) * MC_LimitSpeed_Cal_K_BySOC(MC_RunInfo.SOC)) >> 10;
	  speedLimitEnd = (((MC_ConfigParam1.SpeedLimit + (int8_t)MC_ConfigParam2.SpeedLimitAdj + GasMode_Param.Mode_bit.SpeedLimit + 0x1F - (0xAA >> 3)) * 10 + MC_ConfigParam2.SpeedLimitEndAdj + 22) * MC_LimitSpeed_Cal_K_BySOC(MC_RunInfo.SOC)) >> 10;
	  slewRate = 1024 / (speedLimitEnd - speedLimitStart - 3);		
	}
	else
	{
	  speedLimitStart = (((MC_ConfigParam1.SpeedLimit + (int8_t)MC_ConfigParam2.SpeedLimitAdj + GasMode_Param.Mode_bit.SpeedLimit - (0xAA >> 3) - 2) * 10 + MC_ConfigParam2.SpeedLimitStartAdj) * MC_LimitSpeed_Cal_K_BySOC(MC_RunInfo.SOC)) >> 10;
	  speedLimitEnd = (((MC_ConfigParam1.SpeedLimit + (int8_t)MC_ConfigParam2.SpeedLimitAdj + GasMode_Param.Mode_bit.SpeedLimit - (0xAA >> 3)) * 10 + MC_ConfigParam2.SpeedLimitEndAdj + 22) * MC_LimitSpeed_Cal_K_BySOC(MC_RunInfo.SOC)) >> 10;
	  slewRate = 1024 / (speedLimitEnd - speedLimitStart - 3);		
	}	
	
	//随车速调节助力比
	if(MC_GasMode_Param.Mode_bit.StartMode == 0) //零速启动
	  Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Function_Linear_3Stage(0, 0, speedLimitStart, slewRate, MC_SpeedSensorData.Speed_Data)) >> 10);
	else //带速启动
		Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Function_Linear_3Stage(60, 0xFF, speedLimitStart, slewRate, MC_SpeedSensorData.Speed_Data)) >> 10);
	
	//助力输出		
	MC_TorqueProcess_Param.TorqueRef = Torque_Temp;
	if(MC_TorqueProcess_Param.TorqueRef <= 0)
	{
		MC_TorqueProcess_Param.TorqueRef = 0;
	}
			
	//升降速曲线计算
	if( MC_SpeedSensorData.Speed_Data > speedLimitStart ) //限速处理
	{
		if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
		{
			MC_TorqueProcess_Param.TorqueRefEnd += 1;
		}
		else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
		{
			MC_TorqueProcess_Param.TorqueRefEnd -= 1; 
		}
	}
	else if((0)&&(MC_SpeedSensorData.Speed_Data < 100))  //上坡处理，取消了坡度传感器，这里不执行			  
	{
		if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
		{
			MC_TorqueProcess_Param.TorqueRefEnd += TorqueAccStep;
		}
		else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
		{
			TorqueRefEndUpdateCount++;			
			if(TorqueRefEndUpdateCount >=3)
			{
				TorqueRefEndUpdateCount = 0;
				MC_TorqueProcess_Param.TorqueRefEnd -= TorqueDecStep;
			}
		}		
	}	
	else //正常骑行
	{
		if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
		{
			MC_TorqueProcess_Param.TorqueRefEnd += TorqueAccStep;
		}
		else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
		{
			MC_TorqueProcess_Param.TorqueRefEnd -= TorqueDecStep;
		}
	}
	MC_TorqueProcess_Param.TorqueRefEnd = (MC_TorqueProcess_Param.TorqueRefEnd < 6) ? 6 : MC_TorqueProcess_Param.TorqueRefEnd;
	
	//限速点处理
	if( MC_SpeedSensorData.Speed_Data > speedLimitEnd ) //超速断电
	{
		MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
		MC_TorqueProcess_Param.TorqueRefEnd = 0;
		//停机处理
		MC_MotorStop(&MC_StarFlag);
	}
	else
	{
		MC_MotorStar(&MC_StarFlag);
	}
	
	//根据电压和温度衰减	
	#if 1
	static uint16_t K_ByVoltage_Set_Old = 1024;
	uint16_t K_ByVoltage_Set;
	static uint16_t K_ByVoltage_Result;
	uint32_t K_ByTemperature_Set, K_ByTemperature_Set1, K_ByTemperature_Set2;
	static uint16_t K_ByTemperature_Result;

	//根据电压调节输出
  K_ByVoltage_Set = MC_Cal_K_ByVoltage(MC_RunInfo.BusVoltage, MC_MotorParam.Rate_Voltage, K_ByVoltage_Set_Old);//根据母线电压计算衰减比例，递减
	K_ByVoltage_Set_Old = K_ByVoltage_Set;
  K_ByVoltage_Result = MC_DataSet_Linear_Process(K_ByVoltage_Set, K_ByVoltage_Result, 1, 1); //设定值与给定值线性处理
	
	//根据温度调节输出
	K_ByTemperature_Set1 = MC_Cal_K_ByTemperature(MC_RunInfo.T_Coil, MC_ConfigParam1.TempTH_Alarm); //根据温度计算衰减比例
	K_ByTemperature_Set2 = MC_Cal_K_ByTemperature(MC_RunInfo.T_PCB, (MC_ConfigParam1.TempTH_Alarm-15)); //根据温度计算衰减比例
	K_ByTemperature_Set  = (K_ByTemperature_Set1 * K_ByTemperature_Set2)>>10;	
	K_ByTemperature_Result = MC_DataSet_Linear_Process(K_ByTemperature_Set, K_ByTemperature_Result, 1, 1); //设定值与给定值线性处理
	
	#else
	
	uint16_t K_ByVoltage_Result = 1024;
	uint16_t K_ByTemperature_Result = 1024;
	#endif
	
	//根据SOC计算限流
	IqRefByInPower = PID_Regulator((CurrentLimitPresent * MC_LimitCurrent_Cal_K_BySOC(MC_RunInfo.SOC) / 100) >> 10, (MC_RunInfo.BusCurrent >> 7), &PID_IMax);
		
	Torque_Ref_Temp = ((int32_t)MC_TorqueProcess_Param.TorqueRefEnd * K_ByVoltage_Result) >> 10;
	Torque_Ref_Temp = (Torque_Ref_Temp * K_ByTemperature_Result) >> 10;
	Torque_Ref_Temp = (Torque_Ref_Temp + IqRefByInPower) >> 1;
	
	//Iq输出
	p_MC_CalParam.Ref_Torque = (int16_t)Torque_Ref_Temp;
	p_MC_CalParam.Foc_Flag = SET;
	p_MC_CalParam.AssistRunMode = MC_AssistRunMode_GAS;
	
	return (p_MC_CalParam);
	
}

//推行模式处理
MC_CalParam_Struct_t MC_AssistRunMode_Walk_Process(MC_WorkMode_Struct_t p_MC_WorkMode)
{
	MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};
	int16_t TorQueBySpd = 0;
	int32_t Ref_Speed_Temp;
	int16_t SpdMotorByIdc = 0;
	uint8_t StepData = 0;
	
	//配置模式，设定转速 = 最高转速
	if(p_MC_WorkMode == MC_WorkMode_Config)
	{
	  StepData = (MC_MotorParam.Rate_Speed << 5) / 5000;//设计5s加速到最大值
		StepData = (StepData < 1) ? 1 : StepData;
		if(MC_WalkProcess_Param.MotorSpeedSetBigin < (MC_MotorParam.Rate_Speed << 5) * MC_WalkMode_Persent / 100 - 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin += StepData;
		}
		else if(MC_WalkProcess_Param.MotorSpeedSetBigin > (MC_MotorParam.Rate_Speed << 5) * MC_WalkMode_Persent / 100 + 10)
		{
		  if(MC_WalkProcess_Param.MotorSpeedSetBigin > StepData)
			{
			  MC_WalkProcess_Param.MotorSpeedSetBigin -= StepData;
			}
			else
			{
			  MC_WalkProcess_Param.MotorSpeedSetBigin = 0;
			}
		}
		else
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin = (MC_MotorParam.Rate_Speed << 5 ) * MC_WalkMode_Persent / 100;
		}
		SpdMotorByIdc = PID_Regulator((MC_ConfigParam1.CurrentLimit * 1000) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower); // 母线电流闭环
	}
	
	//运行模式，设定转速 = 设置值
	else
	{
		uint8_t WalkMode_MotorSpeedSet = 0;
		
		//新增配置项，兼容旧电机
		WalkMode_MotorSpeedSet = (MC_ConfigParam1.WalkMode_MotorSpeedSet == 0) ? 135 : MC_ConfigParam1.WalkMode_MotorSpeedSet;
		
		if(MC_WalkProcess_Param.MotorSpeedSetBigin < (WalkMode_MotorSpeedSet << 5) - 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin += 1;
		}
		else if(MC_WalkProcess_Param.MotorSpeedSetBigin > (WalkMode_MotorSpeedSet << 5) + 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin -= 1;
		}
		else
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin = WalkMode_MotorSpeedSet << 5;
		}
		//根据SOC计算限流
	  SpdMotorByIdc = PID_Regulator((MC_ConfigParam1.CurrentLimit * 500 * MC_LimitCurrent_Cal_K_BySOC(MC_RunInfo.SOC)) >> 17, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower); // 母线电流闭环
	}
	
	//速度环
	TorQueBySpd = PID_Regulator((MC_WalkProcess_Param.MotorSpeedSetBigin >> 5), MC_RunInfo.MotorSpeed, &PID_MotorSpd); // 电机速度闭环输出
	TorQueBySpd += SpdMotorByIdc;
	
	//限制车速低于设置值
	if(p_MC_WorkMode != MC_WorkMode_Config) //运行模式,推行限速
	{
		uint8_t WalkMode_SpeedLimit = 0;
		
		//新增配置项，兼容旧电机	
		WalkMode_SpeedLimit = (MC_ConfigParam1.WalkMode_SpeedLimit == 0) ? 60 : MC_ConfigParam1.WalkMode_SpeedLimit;
		
	  TorQueBySpd = (uint16_t)((uint32_t)(TorQueBySpd * Function_Linear_3Stage((WalkMode_SpeedLimit - 5), 0, (WalkMode_SpeedLimit - 5), 128, MC_RunInfo.BikeSpeed)) >> 10);
		if(MC_RunInfo.BikeSpeed > WalkMode_SpeedLimit)
		{
			MC_WalkProcess_Param.MotorSpeedSetBigin = 0;
			MC_MotorStop(&MC_StarFlag);
		}
		else
		{
			//电机启动
			MC_MotorStar(&MC_StarFlag);
		}
	}
	else //配置模式不限速
	{
	  //电机启动
	  MC_MotorStar(&MC_StarFlag);
	}
	
	#if 1
	static uint16_t K_ByVoltage_Set_Old = 1024;
	uint16_t K_ByVoltage_Set;
	static uint16_t K_ByVoltage_Result;
	uint16_t K_ByTemperature_Set;
	static uint16_t K_ByTemperature_Result;

	//根据电压调节输出
  K_ByVoltage_Set = MC_Cal_K_ByVoltage(MC_RunInfo.BusVoltage, MC_MotorParam.Rate_Voltage, K_ByVoltage_Set_Old);//根据母线电压计算衰减比例，递减
	K_ByVoltage_Set_Old = K_ByVoltage_Set;
  K_ByVoltage_Result = MC_DataSet_Linear_Process(K_ByVoltage_Set, K_ByVoltage_Result, 1, 1); //设定值与给定值线性处理
	
	//根据温度调节输出
	K_ByTemperature_Set = MC_Cal_K_ByTemperature(MC_RunInfo.T_Coil, MC_ConfigParam1.TempTH_Alarm); //根据温度计算衰减比例
	K_ByTemperature_Result = MC_DataSet_Linear_Process(K_ByTemperature_Set, K_ByTemperature_Result, 1, 1); //设定值与给定值线性处理
	
	#else
	
	uint16_t K_ByVoltage_Result = 1024;
	uint16_t K_ByTemperature_Result = 1024;
	#endif
	
	#if 0
	//限制最大输出功率为250W
	static uint16_t IqsMax;
	if(MC_RunInfo.MotorSpeed < 10)
	{
	  IqsMax = 1050;
	}
	else
	{
	  IqsMax = 235000 / MC_RunInfo.MotorSpeed;
	}
	IqsMax = (IqsMax > 1050) ? 1050 : IqsMax;
	if(TorQueBySpd > IqsMax)
	{
	  TorQueBySpd = IqsMax;
	}
	#elif 0
	if(TorQueBySpd > 450)
	{
	  TorQueBySpd = 450;
	}
	#endif
	
	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.Foc_Flag = SET;
	p_MC_CalParam.AssistRunMode = MC_AssistRunMode_WALK;
	
	return (p_MC_CalParam);
}

//踏频模式处理
MC_CalParam_Struct_t MC_AssistRunMode_Cadence_Process(MC_GearSt_Struct_t GearSt)
{
  MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};
	
  //...插入踏频处理
	
	//电机启动
	MC_MotorStar(&MC_StarFlag);
	
	p_MC_CalParam.Foc_Flag = SET;
	p_MC_CalParam.AssistRunMode = MC_AssistRunMode_CADENCE;
	
	return (p_MC_CalParam);
}

//力矩模式处理
MC_CalParam_Struct_t MC_AssistRunMode_Torque_Process(uint16_t SenorData, MC_GearSt_Struct_t GearSt, uint16_t SenorDataByCadence)
{
  MC_CalParam_Struct_t p_MC_CalParam = {MC_AssistRunMode_INVALID, 0,	0, RESET};
  uint8_t TorqueAccStep = 0;//力矩上升斜率
	uint8_t TorqueDecStep = 0;//力矩下降斜率
	uint16_t TorqueStartData, TorqueStopData;//力矩启动值，力矩停机值
	int16_t Torque_Temp;
	int32_t Torque_Ref_Temp;
	static uint32_t TorqueStopDelayTimeCnt = 0; //低力矩停机计时
	uint16_t TorqueStopDelayTime;
	static int16_t IqRefByInPower;           //限流计算结果
  static uint16_t CurrentLimitPresent;     //限流实际值，做升降速处理
	uint16_t CurrentLimitSet;                //限流设置值，不同助力档位更新
	
	static uint8_t TorqueRefEndUpdateCount = 0;
	
	#define SOFT_SATRT 1
	
	#if SOFT_SATRT
	static FlagStatus SoftStartFlag = SET;
	static uint16_t SoftStartDelayTimeCount = 0;
	uint16_t SoftStartDelayTime = 0;
	uint16_t SoftStartAcc = 0;
	#endif

	#if NormalWork
	
	//踩踏力矩输入
  MC_TorqueProcess_Param.TorqueApp = SenorData;
	
	#elif 1
	
	//输入阶跃
	MC_TorqueProcess_Param.TorqueApp  = 1000;
	
	//踏频设为启动
	MC_CadenceResult.Cadence_Dir = MC_Cadence_Forward;
	MC_CadenceResult.IsStopFlag = FALSE;
	
	#elif 1
	
	//输入斜坡
	static uint32_t WaveTime_Zero = 0;
	static uint32_t Time_Enter = 0;
	if((HAL_GetTick() - Time_Enter) > 10) // 超时10ms未进入，波形发生初始时刻清零
	{
	  WaveTime_Zero = HAL_GetTick();
	}
	Time_Enter = HAL_GetTick();
	MC_TorqueProcess_Param.TorqueApp = RampWaveGenerate(WaveTime_Zero, 6000, 2100);
	
	//踏频设为启动
	MC_CadenceResult.Cadence_Dir = MC_Cadence_Forward;
	MC_CadenceResult.IsStopFlag = FALSE;
	
	#elif 1
	
	//输入三角波，测试输出响应
	static uint32_t WaveTime_Zero = 0;
	static uint32_t Time_Enter = 0;
	if((HAL_GetTick() - Time_Enter) > 10) // 超时10ms未进入，波形发生初始时刻清零
	{
	  WaveTime_Zero = HAL_GetTick();
	}
	Time_Enter = HAL_GetTick();
	MC_TorqueProcess_Param.TorqueApp  = TriangleWaveGenerate(WaveTime_Zero, 500, 1000 ,1500);
	
	//踏频设为启动
	MC_CadenceResult.Cadence_Dir = MC_Cadence_Forward;
	MC_CadenceResult.IsStopFlag = FALSE;
	
	#elif 1
	
	//输入方波，测试输出响应
	static uint32_t WaveTime_Zero = 0;
	static uint32_t Time_Enter = 0;
	if((HAL_GetTick() - Time_Enter) > 10) // 超时10ms未进入，波形发生初始时刻清零
	{
	  WaveTime_Zero = HAL_GetTick();
	}
	Time_Enter = HAL_GetTick();
	MC_TorqueProcess_Param.TorqueApp  = SquareWaveGenerate(WaveTime_Zero, 5000, 8000, 1500);
	
	//踏频设为启动
	MC_CadenceResult.Cadence_Dir = MC_Cadence_Forward;
	MC_CadenceResult.IsStopFlag = FALSE;
	
	#endif
		
	//低力矩停机
	TorqueStopData = (TorqueSensorStartData < 200) ? 100 : (TorqueSensorStartData >> 1);
	if(MC_TorqueProcess_Param.TorqueApp >= (TorqueStopData))
	{
	  TorqueStopDelayTimeCnt = HAL_GetTick();
	}
	else
	{
		if(MC_RunInfo.MotorSpeed > 200)
		{
		  TorqueStopDelayTime = 218400 / MC_RunInfo.MotorSpeed; //60s / (电机转速 / 4.55 / 2.4) / 3，曲柄1/3圈
		}
		else
		{
		  TorqueStopDelayTime = 1200;
		}
		TorqueStopDelayTime= (TorqueStopDelayTime < 500) ? 500 : TorqueStopDelayTime;
		if((HAL_GetTick() - TorqueStopDelayTimeCnt) > TorqueStopDelayTime)//超时1200ms
		{
			MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
		}
	}
	
	//启动值判断
	if(MC_RunInfo.BikeSpeed > 60) 
	{
		TorqueStartData =  (TorqueSensorStartData < 200 ? 150 : (TorqueSensorStartData > 700 ? 525 : ((TorqueSensorStartData * 3) >> 2)));
	}
	else
	{
		TorqueStartData =  (TorqueSensorStartData < 200 ? 150 : (TorqueSensorStartData > 700 ? 525 : TorqueSensorStartData));
	}
	if(MC_TorqueProcess_Param.TorqueApp >= TorqueStartData)
	{
		 MC_TorqueProcess_Param.MotorStopLock_Flag = RESET;
	}
	
	//踏频反向或踏频停止停机
	if((MC_CadenceResult.Cadence_Dir == MC_Cadence_Backward) ||
		 (MC_CadenceResult.IsStopFlag == TRUE)		
		)
	{
		MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
	}
	
	//停机状态，延时处理
	if(MC_TorqueProcess_Param.MotorStopLock_Flag == SET)
	{
	  if(MC_TorqueProcess_Param.TorqueRefEnd <= 7)
		{
		  MC_TorqueProcess_Param.TorqueRefEnd = 0;
			//停机处理
			MC_MotorStop(&MC_StarFlag);
			
			#if SOFT_SATRT
			//缓启动标志置位
			SoftStartFlag = SET;
			SoftStartDelayTimeCount = 0;
			#endif
		}
		else
		{
		  MC_TorqueProcess_Param.TorqueRefEnd -= 7;  //这里影响到停止踩踏后的断电时间
			MC_MotorStar(&MC_StarFlag);
		}
	}
	//力矩给定升降速处理
	else
	{
		static int32_t SpeedRatio_NoFlt=1092;
		static int32_t speedRatio_temp=1092<<12;
		
		/*调节助力比，使用经过踏频信号滤波处理的力矩值*/
		#if NormalWork
		MC_TorqueProcess_Param.TorqueApp = SenorDataByCadence;
		#endif
		if( (MC_RunInfo.MotorSpeed > 500)&&(MC_CadenceResult.Cadence_Data >= 15)&&(MC_TorqueProcess_Param.TorqueApp > 200) )
		{
			SpeedRatio_NoFlt= (100*MC_RunInfo.MotorSpeed)/MC_CadenceResult.Cadence_Data;
			speedRatio_temp += ((SpeedRatio_NoFlt << 12) - speedRatio_temp) >> 10;
			SpeedRatio = speedRatio_temp >> 12;
		}
		//按照助力档位调节力矩输入值
		switch(GearSt)
		{
			case MC_GearSt_Torque_ECO:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_ECO.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_ECO.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_ECO.DecCnt;
				//随力矩输入调节助力比
				Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_ECO.Gain_K >> 1, MC_AssisParam.Gear_ECO.Gain_K, MC_AssisParam.Gear_ECO.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_ECO.Lower_Iq) ? MC_AssisParam.Gear_ECO.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_ECO.Upper_Iq) ? MC_AssisParam.Gear_ECO.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_ECO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_ECO.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_ECO.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_NORM:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_NORM.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_NORM.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_NORM.DecCnt;
				//随力矩输入调节助力比
				Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_NORM.Gain_K >> 1, MC_AssisParam.Gear_NORM.Gain_K, MC_AssisParam.Gear_NORM.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_NORM.Lower_Iq) ? MC_AssisParam.Gear_NORM.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_NORM.Upper_Iq) ? MC_AssisParam.Gear_NORM.Upper_Iq : Torque_Temp;
				
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_NORM.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * 100;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_NORM.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_NORM.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_SPORT:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_SPORT.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_SPORT.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_SPORT.DecCnt;
				//随力矩输入调节助力比
				Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_SPORT.Gain_K >> 1, MC_AssisParam.Gear_SPORT.Gain_K, MC_AssisParam.Gear_SPORT.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_SPORT.Lower_Iq) ? MC_AssisParam.Gear_SPORT.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_SPORT.Upper_Iq) ? MC_AssisParam.Gear_SPORT.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_SPORT.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_SPORT.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_SPORT.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_Torque_TURBO:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_TURBO.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_TURBO.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_TURBO.DecCnt;
				//随力矩输入调节助力比
				Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_TURBO.Gain_K >> 1, MC_AssisParam.Gear_TURBO.Gain_K, MC_AssisParam.Gear_TURBO.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_TURBO.Lower_Iq) ? MC_AssisParam.Gear_TURBO.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_TURBO.Upper_Iq) ? MC_AssisParam.Gear_TURBO.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_TURBO.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_TURBO.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_TURBO.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			case MC_GearSt_SMART:
			{
				//控制输入给定加速斜率
				TorqueAccStep = (MC_AssisParam.Gear_SMART.AccCnt <= 0) ? 1 : MC_AssisParam.Gear_SMART.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_SMART.DecCnt;
				//助力比控制系数
				if(MC_TorqueProcess_Param.TorqueApp < 510)
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(MC_AssisParam.Gear_NORM.Gain_K >> 1, MC_AssisParam.Gear_NORM.Gain_K, MC_AssisParam.Gear_NORM.TorqueApp_TH, MC_TorqueProcess_Param.TorqueApp)) >> 10);
				}
				else
				{
					Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_TorqueProcess_Param.TorqueApp) / (MC_AssisParam.Gear_SMART.TorqueApp_TH);
				}				
				//给定下限
				Torque_Temp = (Torque_Temp < MC_AssisParam.Gear_SMART.Lower_Iq) ? MC_AssisParam.Gear_SMART.Lower_Iq : Torque_Temp;
				//给定上限
				Torque_Temp = (Torque_Temp > MC_AssisParam.Gear_SMART.Upper_Iq) ? MC_AssisParam.Gear_SMART.Upper_Iq : Torque_Temp;
				//限流参数设置
				CurrentLimitSet = (uint32_t)(MC_AssisParam.Gear_SMART.CurrentMax_K * MC_ConfigParam1.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_DataSet_Linear_Process(CurrentLimitSet, CurrentLimitPresent, 5 ,1);
				PID_IMax.hLower_Limit_Output = -(MC_AssisParam.Gear_SMART.Upper_Iq);   //Lower Limit for Output limitation
				PID_IMax.hUpper_Limit_Output = 0; //Upper Limit for Output limitation
				PID_IMax.wLower_Limit_Integral = -(MC_AssisParam.Gear_SMART.Upper_Iq << 10); // 放大1024
				PID_IMax.wUpper_Limit_Integral = 0; // 放大1024
				break;
			}
			default: 
			{
				TorqueAccStep = 0;
				TorqueDecStep = 0;
				Torque_Temp = 0;
				break;
			}
		}
		
		//限速参数调整
		static uint16_t speedLimitStart = 250, speedLimitEnd = 272, slewRate = 52;
		if((MC_ConfigParam2.SpeedLimitStartAdj > 50) || (MC_ConfigParam2.SpeedLimitStartAdj < -50)) MC_ConfigParam2.SpeedLimitStartAdj = 0;
		if((MC_ConfigParam2.SpeedLimitEndAdj > 50) || (MC_ConfigParam2.SpeedLimitEndAdj < -50)) MC_ConfigParam2.SpeedLimitEndAdj = 0;
		
		speedLimitStart = (((MC_ConfigParam1.SpeedLimit + (int8_t)MC_ConfigParam2.SpeedLimitAdj) * 10 + MC_ConfigParam2.SpeedLimitStartAdj) * MC_LimitSpeed_Cal_K_BySOC(MC_RunInfo.SOC)) >> 10;
		speedLimitEnd   = (((MC_ConfigParam1.SpeedLimit + (int8_t)MC_ConfigParam2.SpeedLimitAdj) * 10 + MC_ConfigParam2.SpeedLimitEndAdj + 22) * MC_LimitSpeed_Cal_K_BySOC(MC_RunInfo.SOC)) >> 10;
		slewRate        = 1024 / (speedLimitEnd - speedLimitStart - 3);		
		//随车速调节助力比
		Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Function_Linear_3Stage(speedLimitStart, 0, speedLimitStart, slewRate, MC_SpeedSensorData.Speed_Data)) >> 10);
			
    //助力输出		
		MC_TorqueProcess_Param.TorqueRef = Torque_Temp;
		if(MC_TorqueProcess_Param.TorqueRef <= 0)
		{
			MC_TorqueProcess_Param.TorqueRef = 0;
		}
				
		//升降速曲线计算
		if( MC_SpeedSensorData.Speed_Data > speedLimitStart ) //限速处理
		{
			if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
			{
				MC_TorqueProcess_Param.TorqueRefEnd += 1;
			}
			else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
			{
				MC_TorqueProcess_Param.TorqueRefEnd -= 1; 
			}
		}
		else if((0)&&(MC_SpeedSensorData.Speed_Data < 100))  //上坡处理			  
		{
			if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
			{
				MC_TorqueProcess_Param.TorqueRefEnd += TorqueAccStep;
			}
			else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
			{
				TorqueRefEndUpdateCount++;			
				if(TorqueRefEndUpdateCount >=3)
				{
					TorqueRefEndUpdateCount = 0;
					MC_TorqueProcess_Param.TorqueRefEnd -= TorqueDecStep;
				}
			}		
		}	
		
    #if SOFT_SATRT		
    else if(SoftStartFlag == SET)  //启动处理
		{
			if(MC_ConfigParam1.StarModel == MC_StarMode_DYNAMIC)  //强劲模式，无延迟
			{
			  SoftStartDelayTimeCount = 0;
				SoftStartFlag = RESET;
			}
			else
			{
				if(MC_ConfigParam1.StarModel == MC_StarMode_SOFT)   //柔和模式，延迟300ms
				{
					SoftStartDelayTime = 300;      //启动处理延时300ms
					SoftStartAcc = 30;             //30ms递增0.1倍
				}
				else                                                //正常模式，延迟100ms
				{
					SoftStartDelayTime = 100;      //启动处理延时100ms
					SoftStartAcc = 10;             //10ms递增0.1倍
				}
				SoftStartDelayTimeCount++;
				if(SoftStartDelayTimeCount <= SoftStartDelayTime) // 缓启动过程,按照0.1倍率逐步增加加减速斜率
				{  
					if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
					{
						if((SoftStartDelayTimeCount % (10 - SoftStartDelayTimeCount / SoftStartAcc))  == 0)
						{
							MC_TorqueProcess_Param.TorqueRefEnd += TorqueAccStep;
						}
					}
					else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
					{
						MC_TorqueProcess_Param.TorqueRefEnd -= TorqueDecStep;
					}
				}
				else
				{
					SoftStartDelayTimeCount = 0;
					SoftStartFlag = RESET;
				}
			}
		}	
    #endif
		
		else //正常骑行
		{
			if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) > 2)
			{
				MC_TorqueProcess_Param.TorqueRefEnd += TorqueAccStep;
			}
			else if((MC_TorqueProcess_Param.TorqueRef - MC_TorqueProcess_Param.TorqueRefEnd) < (- 1))
			{
				MC_TorqueProcess_Param.TorqueRefEnd -= TorqueDecStep;
			}
		}
		MC_TorqueProcess_Param.TorqueRefEnd = (MC_TorqueProcess_Param.TorqueRefEnd < 6) ? 6 : MC_TorqueProcess_Param.TorqueRefEnd;
		
		//限速点处理
		if( MC_SpeedSensorData.Speed_Data > speedLimitEnd ) //限速值 + 2.2
		{
			MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
			MC_TorqueProcess_Param.TorqueRefEnd = 0;
			//停机处理
			MC_MotorStop(&MC_StarFlag);
		}
		else
		{
		  MC_MotorStar(&MC_StarFlag);
		}
	}
	
	#if 1
	static uint16_t K_ByVoltage_Set_Old = 1024;
	uint16_t K_ByVoltage_Set;
	static uint16_t K_ByVoltage_Result;
	uint32_t K_ByTemperature_Set, K_ByTemperature_Set1, K_ByTemperature_Set2;
	static uint16_t K_ByTemperature_Result;

	//根据电压调节输出
  K_ByVoltage_Set = MC_Cal_K_ByVoltage(MC_RunInfo.BusVoltage, MC_MotorParam.Rate_Voltage, K_ByVoltage_Set_Old);//根据母线电压计算衰减比例，递减
	K_ByVoltage_Set_Old = K_ByVoltage_Set;
  K_ByVoltage_Result = MC_DataSet_Linear_Process(K_ByVoltage_Set, K_ByVoltage_Result, 1, 1); //设定值与给定值线性处理
	
	//根据温度调节输出
	K_ByTemperature_Set1 = MC_Cal_K_ByTemperature(MC_RunInfo.T_Coil, MC_ConfigParam1.TempTH_Alarm); //根据温度计算衰减比例
	K_ByTemperature_Set2 = MC_Cal_K_ByTemperature(MC_RunInfo.T_PCB, (MC_ConfigParam1.TempTH_Alarm-15)); //根据温度计算衰减比例
	K_ByTemperature_Set  = (K_ByTemperature_Set1 * K_ByTemperature_Set2)>>10;	
	K_ByTemperature_Result = MC_DataSet_Linear_Process(K_ByTemperature_Set, K_ByTemperature_Result, 1, 1); //设定值与给定值线性处理
	
	#else
	
	uint16_t K_ByVoltage_Result = 1024;
	uint16_t K_ByTemperature_Result = 1024;
	#endif
	
  //根据SOC计算限流
	IqRefByInPower = PID_Regulator((CurrentLimitPresent * MC_LimitCurrent_Cal_K_BySOC(MC_RunInfo.SOC) / 100) >> 10, (MC_RunInfo.BusCurrent >> 7), &PID_IMax);
	
	Torque_Ref_Temp = ((int32_t)MC_TorqueProcess_Param.TorqueRefEnd * K_ByVoltage_Result) >> 10;
	Torque_Ref_Temp = (Torque_Ref_Temp * K_ByTemperature_Result) >> 10;
	Torque_Ref_Temp = (Torque_Ref_Temp + IqRefByInPower) >> 1;
	
	//Iq输出
	p_MC_CalParam.Ref_Torque = (int16_t)Torque_Ref_Temp;
	p_MC_CalParam.Foc_Flag = SET;
	p_MC_CalParam.AssistRunMode = MC_AssistRunMode_TORQUE;
	
	return (p_MC_CalParam);
}


/******************************全局函数定义*****************************/
//传感器初始化
void MC_SensorInit(void)
{
  //霍尔传感器IO设置
	HallSensor_GPIO_Init();
	
	//霍尔电角度初始化
	HallSensorAngle_Init();
	
	//踏频传感器IO设置
	CadenceSensor_GPIO_Init();
	
	//速度传感器IO设置
	SpeedSensor_GPIO_Init();
	
	//刹车信号和Gear信号检测IO设置
	KeyInitial();
	
	//力矩传感器参数还原
	TorqueSensor_ParamSetDefaultData_Init(&TorqueSensor_1_Param, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_1]);
	TorqueSensor_ParamSetDefaultData_Init(&TorqueSensor_2_Param, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_2]);
	TorqueSensor_ParamSetDefaultData_Init(&TorqueSensor_3_Param, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_3]);
	
	//指拨零点初值
	GasSensorOffSet_Init(&GasSensor_OffSet, ADC1_Result[ADC1_RANK_GAS]);
	
}

//MC控制初始化
void MC_Init(void)
{	
	//PID参数初始化
	PID_Init(MC_ConfigParam1.SerialNum);	
	
	//助力参数初始化
	UpdateGearParam(MC_ConfigParam1.SerialNum);
	
	//三相电流零点校准
	SVPWM_3ShuntCurrentReadingCalibration(&MC_ErrorCode);
	
	//母线电流零点校准
	CurrentReadingCalibration(&MC_ErrorCode);
	
	//力矩传感器零点值处理
	TorqueOffSetData_Process(&TorqueSensor_1_Param.Torque_OffSetData, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_1]);//112ms
	TorqueOffSetData_Process(&TorqueSensor_2_Param.Torque_OffSetData, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_2]);//112ms
	TorqueOffSetData_Process(&TorqueSensor_3_Param.Torque_OffSetData, ADC1_Result[ADC1_RANK_TORQUE_SENSOR_3]);//112ms
	
	
	//12V驱动电源初始化
	Power12V_Driver_Init();
	
	//打开12V驱动电源
  Power12V_Driver_Process(SET);
}

//MC控制参数初始化
void MC_ControlParam_Init(void)
{
  //清除推行模式初始变量
	MC_WalkProcess_Param.IsEnterFlag = FALSE;
	MC_WalkProcess_Param.MotorSpeedSetBigin = 0;
	
	//清除力矩模式初始变量
	MC_TorqueProcess_Param.MotorStopLock_Flag  = SET;
	MC_TorqueProcess_Param.TorqueApp = 0;
	MC_TorqueProcess_Param.TorqueRef = 0;
	MC_TorqueProcess_Param.TorqueRefEnd = 0;
	
	//全局运算变量归零
	IqFdbFlt =0;
	IdFdbFlt = 0;
	VoltSquareFlt = 0;
	UqVoltFlt = 0;
	UdVoltFlt = 0;
		
	//PDI积分清零
	PID_Flux_InitStructure.wIntegral = 0;
 	PID_Torque_InitStructure.wIntegral = 0;
 	PID_Weak_InitStructure.wIntegral = 0;
 	PID_IMax.wIntegral = 0;
  PID_MotorSpd.wIntegral = 0;
 	PID_ConstantPower.wIntegral = 0;
}

//控制参数输入值计算
void MC_CalParam_Cal(MC_WorkMode_Struct_t p_MC_WorkMode, \
	                   ADC_SensorData_Struct_t p_ADC_SensorData, \
                     MC_GearSt_Struct_t GearSt, \
                     TrueOrFalse_Flag_Struct_t Break_Flag, \
                     TrueOrFalse_Flag_Struct_t GearSensor_Flag, \
                     MC_CalParam_Struct_t* p_MC_CalParam)
{
	MC_AssistRunMode_Struct_t MC_AssistRunMode_Temp;
	static FlagStatus MC_AssistRunMode_ShiftFlag = RESET;  //电机助力模式切换标志
	
	//根据指拨信号、助力档位指令、刹车信号判断助力模式
	MC_AssistRunMode_Temp = MC_JudgeAsistRunMode_Process(MC_GasMode_Param, p_ADC_SensorData.GasSensor, GearSt, (TrueOrFalse_Flag_Struct_t)(Break_Flag & GearSensor_Flag & FALSE));//TRUE 0, FALSE 1
	
	//发生助力模式切换时，清空变量
	if(MC_AssistRunMode_Temp != p_MC_CalParam->AssistRunMode)
	{
		if(MC_AssistRunMode_ShiftFlag == RESET)
		{
	  	MC_AssistRunMode_Temp = MC_AssistRunMode_INVALID;
			MC_AssistRunMode_ShiftFlag = SET;
			
			
			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;
				}
			}
		}
	}
	//助力模式处理
	switch(MC_AssistRunMode_Temp)
	{
	  //指拨模式
		case MC_AssistRunMode_GAS:
		{
			//计算FOC控制输入
			if(MC_GasMode_Param.Mode_bit.CrontrolMode == 0) //速度模式
			{
			  *p_MC_CalParam = MC_AssistRunMode_GasSpeed_Process(MC_GasMode_Param, p_ADC_SensorData.GasSensor, (MC_GearSt_Struct_t)(GearSt & 0x0F));
			}
			else if(MC_GasMode_Param.Mode_bit.CrontrolMode == 1) //力矩模式
			{
			  *p_MC_CalParam = MC_AssistRunMode_GasTorque_Process(MC_GasMode_Param, p_ADC_SensorData.GasSensor, p_ADC_SensorData.TorqueSensor, (MC_GearSt_Struct_t)(GearSt & 0x0F));
			}
			//助力模式切换标志复位
			MC_AssistRunMode_ShiftFlag = RESET;
			break;
		}
		//推行模式
		case MC_AssistRunMode_WALK:
		{
			//计算FOC控制输入
			if(MC_WalkProcess_Param.IsEnterFlag == FALSE)
			{
				MC_WalkProcess_Param.MotorSpeedSetBigin = (uint32_t)MC_RunInfo.MotorSpeed << 5;
				MC_WalkProcess_Param.IsEnterFlag = TRUE;
			}
			*p_MC_CalParam = MC_AssistRunMode_Walk_Process(p_MC_WorkMode);
			//助力模式切换标志复位
			MC_AssistRunMode_ShiftFlag = RESET;
			break;
		}
		//踏频模式
		case MC_AssistRunMode_CADENCE:
		{
			//计算FOC控制输入
			*p_MC_CalParam = MC_AssistRunMode_Cadence_Process(GearSt);
			//助力模式切换标志复位
			MC_AssistRunMode_ShiftFlag = RESET;
			break;
		}
		//力矩模式
		case MC_AssistRunMode_TORQUE:
		{
			//计算FOC控制输入
			*p_MC_CalParam = MC_AssistRunMode_Torque_Process(p_ADC_SensorData.TorqueSensor, GearSt, MC_CadenceResult.torqueByCadence);
			//助力模式切换标志复位
			MC_AssistRunMode_ShiftFlag = RESET;
			break;
		}
		//空闲模式或存在故障
		case MC_AssistRunMode_INVALID: default:
		{
			//停机处理
			MC_MotorStop(&MC_StarFlag);
			//更新母线电流零点值
			CurrentReadingCalibration(&MC_ErrorCode);
			//控制计算值初始化为默认值
			p_MC_CalParam->AssistRunMode = MC_AssistRunMode_INVALID;
			p_MC_CalParam->Foc_Flag = RESET;
			p_MC_CalParam->Ref_Torque = 0;
			p_MC_CalParam->Ref_Speed = 0;
			break;
		}
	}	
}

void MC_MotorStop(FlagStatus* StarFlag)
{			
	//关闭PWM输出
	Pwm_Timer_Stop();
	
	//FOC运算停止
  FOC_Disable();
	
	//控制参数归零
	MC_ControlParam_Init();
	
	//电机启动标志复位
	*StarFlag = RESET;
}

void MC_MotorStar(FlagStatus* StarFlag)
{	
	if(*StarFlag == RESET)
	{
		//开启PWM输出
		Enable_Pwm_Output();
		
		//霍尔电角度初始化
	  HallSensorAngle_Init();
		
		//FOC运算启动
    FOC_Enable();
		
		//电机启动标志置位
		*StarFlag = SET;
	}
}	

/*
指拨模式计算速比，计算费时，在主循环调用
*/
void SpdProportion_calculate(void)
{
	if(SpdProportion_CAL_flag==1)
	{
		SpdProportion_StandardDeviation = Standard_deviation_aver(SpdProportion_buff, 50, &test_SpdProportionAver);
		test_StandardDeviation = (int32_t)(SpdProportion_StandardDeviation );
		SpdProportion_CAL_flag = 0;
		/*更新速比*/
		if(test_StandardDeviation < 30)
		{
			SpdProportion = test_SpdProportionAver;
		}
	}
}