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"
#include "ICM20600.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;
//电机转速和车速传动比限流系数
uint8_t MC_MotorSpeedLimit_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);
}

//随温度计算助力衰减系数
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_SupportFlag_Struct_t GasCtrlMode, 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))
		{
		  //进入推行模式
			if(GearSt == MC_GearSt_WALK)
			{
				MC_AssistRunMode_Result = MC_AssistRunMode_WALK;
			}
			//进入踏频模式
			else if((GearSt & 0xF0) == 0x10)
			{
			  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;
}

//指拨模式处理
MC_CalParam_Struct_t MC_AssistRunMode_Gas_Process(uint16_t SensorData, 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_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.MotorSpeedSetBegin < (MC_MotorParam.Rate_Speed << 5) * MC_WalkMode_Persent / 100 - 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBegin += StepData;
		}
		else if(MC_WalkProcess_Param.MotorSpeedSetBegin > (MC_MotorParam.Rate_Speed << 5) * MC_WalkMode_Persent / 100 + 10)
		{
		  if(MC_WalkProcess_Param.MotorSpeedSetBegin > StepData)
			{
			  MC_WalkProcess_Param.MotorSpeedSetBegin -= StepData;
			}
			else
			{
			  MC_WalkProcess_Param.MotorSpeedSetBegin = 0;
			}
		}
		else
		{
		  MC_WalkProcess_Param.MotorSpeedSetBegin = (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.MotorSpeedSetBegin < (WalkMode_MotorSpeedSet << 5) - 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBegin += 1;
		}
		else if(MC_WalkProcess_Param.MotorSpeedSetBegin > (WalkMode_MotorSpeedSet << 5) + 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBegin -= 1;
		}
		else
		{
		  MC_WalkProcess_Param.MotorSpeedSetBegin = WalkMode_MotorSpeedSet << 5;
		}
		SpdMotorByIdc = PID_Regulator((MC_ConfigParam1.CurrentLimit * 500) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower); // 母线电流闭环
	}
	
	//速度环
	TorQueBySpd = PID_Regulator((MC_WalkProcess_Param.MotorSpeedSetBegin >> 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.MotorSpeedSetBegin = 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(MC_CadenceResult_Struct_t CadenceData, ADC_SensorData_Struct_t AdcSenorData, MC_GearSt_Struct_t GearSt, MC_SupportFlag_Struct_t GasCtrlMode)
{
  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 FlagStatus IsEnterGasMode = RESET;
	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((GasCtrlMode == MC_SUPPORT_ENABLE) && (AdcSenorData.GasSensor > 50))
	{
	  MC_TorqueProcess_Param.MotorStopLock_Flag = RESET;
		//停止判断
		if(AdcSenorData.GasSensor < 30)
		{
		  MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
		}
		//设定马达输出力矩
		MC_TorqueProcess_Param.TorqueApp = (AdcSenorData.GasSensor * MC_AssisParam.Gear_TURBO.Upper_Iq) >> 11;
		if(MC_TorqueProcess_Param.TorqueApp < CadenceData.torqueByCadence)
		{
		  MC_TorqueProcess_Param.TorqueApp = CadenceData.torqueByCadence;
		}
		//进入指拨模式标志
		IsEnterGasMode = SET;
	}
	
	//根据踩踏力矩控制
	else
	{
		#if 1
		
		//踩踏力矩输入
		MC_TorqueProcess_Param.TorqueApp = AdcSenorData.TorqueSensor;
		
		#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;
		}
		
		//设定输入力矩
		MC_TorqueProcess_Param.TorqueApp = CadenceData.torqueByCadence;
		
		//进入指拨模式标志
		IsEnterGasMode = RESET;
	}
	
	//停机状态，延时处理
	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
	{
		//指拨模式按照限流值
		if(IsEnterGasMode == SET)
		{
		  //设定力矩给定值
			Torque_Temp = MC_TorqueProcess_Param.TorqueApp;
			//设定加减速斜率
			TorqueAccStep = 3;
			TorqueDecStep = 7;
			//根据档位给定力矩最大值和限流值
			switch(GearSt)
			{
				case MC_GearSt_Torque_ECO:
				{
					//给定上限
					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:
				{
					//给定上限
					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:
				{
					//给定上限
					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_MotorSpeedLimit_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:
				{
					//给定上限
					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_MotorSpeedLimit_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:
				{
					//给定上限
					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_MotorSpeedLimit_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;
				}
			}	
		}
		else
		{
			//按照助力档位调节力矩输入值
			switch(GearSt)
			{
				case MC_GearSt_Torque_ECO:
				{
					//控制输入给定加速斜率
					if(MC_ConfigParam1.UserAdjParam_ECO.StarModel_GAIN <= 80)
					{
						TorqueAccStep = MC_AssisParam.Gear_ECO.AccCnt - 1;
					}
					else if(MC_ConfigParam1.UserAdjParam_ECO.StarModel_GAIN >= 120)
					{
						TorqueAccStep = MC_AssisParam.Gear_ECO.AccCnt + 1;
					}
					else
					{
						TorqueAccStep = MC_AssisParam.Gear_ECO.AccCnt;
					}
					TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
					//控制输入给定减速斜率
					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_ConfigParam1.UserAdjParam_ECO.Assist_K_GAIN / 100;
					//给定下限
					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:
				{
					//控制输入给定加速斜率
					if(MC_ConfigParam1.UserAdjParam_NORM.StarModel_GAIN <= 80)
					{
						TorqueAccStep = MC_AssisParam.Gear_NORM.AccCnt - 1;
					}
					else if(MC_ConfigParam1.UserAdjParam_NORM.StarModel_GAIN >= 120)
					{
						TorqueAccStep = MC_AssisParam.Gear_NORM.AccCnt + 1;
					}
					else
					{
						TorqueAccStep = MC_AssisParam.Gear_NORM.AccCnt;
					}
					TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
					//控制输入给定减速斜率
					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_ConfigParam1.UserAdjParam_NORM.Assist_K_GAIN / 100;
					//给定下限
					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:
				{
					//控制输入给定加速斜率
					if(MC_ConfigParam1.UserAdjParam_SPORT.StarModel_GAIN <= 80)
					{
						TorqueAccStep = MC_AssisParam.Gear_SPORT.AccCnt - 1;
					}
					else if(MC_ConfigParam1.UserAdjParam_SPORT.StarModel_GAIN >= 120)
					{
						TorqueAccStep = MC_AssisParam.Gear_SPORT.AccCnt + 1;
					}
					else
					{
						TorqueAccStep = MC_AssisParam.Gear_SPORT.AccCnt;
					}
					TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
					//控制输入给定减速斜率
					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_ConfigParam1.UserAdjParam_SPORT.Assist_K_GAIN / 100;
					//给定下限
					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:
				{
					//控制输入给定加速斜率
					if(MC_ConfigParam1.UserAdjParam_TURBO.StarModel_GAIN <= 80)
					{
						TorqueAccStep = MC_AssisParam.Gear_TURBO.AccCnt - 1;
					}
					else if(MC_ConfigParam1.UserAdjParam_TURBO.StarModel_GAIN >= 120)
					{
						TorqueAccStep = MC_AssisParam.Gear_TURBO.AccCnt + 1;
					}
					else
					{
						TorqueAccStep = MC_AssisParam.Gear_TURBO.AccCnt;
					}
					TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
					//控制输入给定减速斜率
					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_ConfigParam1.UserAdjParam_TURBO.Assist_K_GAIN / 100;
					//给定下限
					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:
				{
					//控制输入给定加速斜率
					if(MC_ConfigParam1.UserAdjParam_SMART.StarModel_GAIN <= 80)
					{
						TorqueAccStep = MC_AssisParam.Gear_SMART.AccCnt - 1;
					}
					else if(MC_ConfigParam1.UserAdjParam_SMART.StarModel_GAIN >= 120)
					{
						TorqueAccStep = MC_AssisParam.Gear_SMART.AccCnt + 1;
					}
					else
					{
						TorqueAccStep = MC_AssisParam.Gear_SMART.AccCnt;
					}
					TorqueAccStep = (TorqueAccStep <= 0) ? 1 : TorqueAccStep;
					//控制输入给定减速斜率
					TorqueDecStep = MC_AssisParam.Gear_SMART.DecCnt;
					//助力比控制系数
					Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_TorqueProcess_Param.TorqueApp) / (MC_AssisParam.Gear_SMART.TorqueApp_TH);
					//根据助力增益调节助力比
					Torque_Temp = Torque_Temp * MC_ConfigParam1.UserAdjParam_SMART.Assist_K_GAIN / 100;
					//给定下限
					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;
				}
			}
		}
		
		//随车速调节助力比
		Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Function_Linear_3Stage(0, 0, MC_ConfigParam1.SpeedLimit * 10, 52, 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 > (MC_ConfigParam1.SpeedLimit * 10) ) //限速处理
		{
			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 -= 10; 
			}
		}
		else if( (Bike_Attitude.UpWardSlope_flag == TRUE)&&(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 > (MC_ConfigParam1.SpeedLimit * 10 + 22) ) //限速值+2.2
		{
			MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
			MC_TorqueProcess_Param.TorqueRefEnd = 0;
			//停机处理
			MC_MotorStop(&MC_StarFlag);
		}
		#if 0  //低于限速点启动电机
		else if(MC_RunInfo.BikeSpeed < ((MC_ConfigParam1.SpeedLimit) * 10))
		{
		  MC_MotorStar(&MC_StarFlag);
		}
		#elif 1  //低于断电点即启动电机
		else
		{
		  MC_MotorStar(&MC_StarFlag);
		}
		#endif
	}
	
	#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_PCB, 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
	
  //限流计算
  IqRefByInPower =  PID_Regulator((uint32_t)(CurrentLimitPresent / 100 * K_ByVoltage_Result * K_ByTemperature_Result) >> 20, (MC_RunInfo.BusCurrent >> 7), &PID_IMax);
	
	Torque_Ref_Temp = MC_TorqueProcess_Param.TorqueRefEnd;
	Torque_Ref_Temp = (Torque_Ref_Temp + IqRefByInPower) >> 1;
	
	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]);
	
	//ICM20600初始化	
	ICM20600_initialize();
	if(ICM20600_OK_Flag == TRUE)
	{
		ICM20600_coefficientinitialize(RANGE_250_DPS, RANGE_2G, &ICM20600Sensor);
	}
}

//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
	
	//智能档位初始化处理
	if(MC_ConfigParam1.NoPBU_Flag == MC_SUPPORT_ENABLE)
	{
	  MC_ControlCode.GearSt = MC_GearSt_SMART;
		Update_MC_ControlCode_Back();
	}
	
	//12V驱动电源初始化
	Power12V_Driver_Init();
	
	//打开12V驱动电源
  Power12V_Driver_Process(SET);
}

//MC控制参数初始化
void MC_ControlParam_Init(void)
{
  //清除推行模式初始变量
	MC_WalkProcess_Param.IsEnterFlag = FALSE;
	MC_WalkProcess_Param.MotorSpeedSetBegin = 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_ConfigParam1.GasCtrlMode_Flag, p_ADC_SensorData.GasSensor, GearSt, (TrueOrFalse_Flag_Struct_t)(Break_Flag & GearSensor_Flag & Bike_Attitude.FellDown_flag));//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;
			
		}
	}
	//助力模式处理
	switch(MC_AssistRunMode_Temp)
	{
	  //指拨模式
		case MC_AssistRunMode_GAS:
		{
			//计算FOC控制输入
			*p_MC_CalParam = MC_AssistRunMode_Gas_Process(p_ADC_SensorData.GasSensor, (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.MotorSpeedSetBegin = (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(MC_CadenceResult, p_ADC_SensorData, GearSt, MC_ConfigParam1.GasCtrlMode_Flag);
			//助力模式切换标志复位
			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;
	}
}