#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_CurrentLimit_Linear_Process(uint16_t SetData, uint16_t PresentData)
{
  int16_t ErrorData;
	uint16_t Result;
	
	ErrorData = SetData - PresentData;
	if(ErrorData > 0)  //升速
	{
	  Result = PresentData + 5;
	}
	else if(ErrorData < 0)  //降速
	{
	  Result = PresentData - 1;
	}
	else
	{
	  Result = SetData;
	}
	
	return Result;
}

//助力模式判断处理
MC_AssistRunMode_Struct_t MC_JudgeAsistRunMode_Process(uint16_t GasSensorData, MC_GearSt_Struct_t GearSt, TrueOrFalse_Flag_Struct_t Break_Flag)
{
  MC_AssistRunMode_Struct_t MC_AssistRunMode_Result;
	
	if(MC_ErrorCode.Code == 0)  // 无故障
	{
	  if((GearSt != MC_GearSt_OFF) && (Break_Flag == FALSE))
		{
		  //进入指拨模式
			if(GasSensorData > 100)  
			{
				MC_AssistRunMode_Result = MC_AssistRunMode_GAS;
			}
			//退出指拨模式
			else if(GasSensorData < 50)  
			{
				//进入推行模式
				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;
		}
	}
	else  //存在故障
	{
	  MC_AssistRunMode_Result = MC_AssistRunMode_INVALID;
		MC_ControlCode.GearSt = MC_GearSt_OFF;
		MC_ControlCode_Back.GearSt = (MC_GearSt_Struct_t)~MC_ControlCode.GearSt;
	}
	
	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;
	int16_t SpdMotorByIdc = 0;
	
	
	//配置模式，设定转速 = 最高转速
	if(p_MC_WorkMode == MC_WorkMode_Config)
	{
	  if(MC_WalkProcess_Param.MotorSpeedSetBigin < (MC_MotorParam.Rate_Speed << 5) * MC_WalkMode_Persent / 100 - 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin += 5;
		}
		else if(MC_WalkProcess_Param.MotorSpeedSetBigin > (MC_MotorParam.Rate_Speed << 5) * MC_WalkMode_Persent / 100 + 10)
		{
		  if(MC_WalkProcess_Param.MotorSpeedSetBigin > 5)
			{
			  MC_WalkProcess_Param.MotorSpeedSetBigin -= 5;
			}
			else
			{
			  MC_WalkProcess_Param.MotorSpeedSetBigin = 0;
			}
		}
		else
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin = (MC_MotorParam.Rate_Speed << 5 ) * MC_WalkMode_Persent / 100;
		}
		SpdMotorByIdc = PID_Regulator((MC_ConfigParam.CurrentLimit * 1000) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower); // 母线电流闭环
	}
	
	//运行模式，设定转速 = 150rpm
	else
	{
	  if(MC_WalkProcess_Param.MotorSpeedSetBigin < (135 << 5) - 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin += 4;
		}
		else if(MC_WalkProcess_Param.MotorSpeedSetBigin > (135 << 5) + 10)
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin -= 4;
		}
		else
		{
		  MC_WalkProcess_Param.MotorSpeedSetBigin = 135 << 5;
		}
		SpdMotorByIdc = PID_Regulator((MC_ConfigParam.CurrentLimit * 500) >> 7, MC_RunInfo.BusCurrent >> 7, &PID_ConstantPower); // 母线电流闭环
	}
	
	//速度环
	TorQueBySpd = PID_Regulator((MC_WalkProcess_Param.MotorSpeedSetBigin >> 5), MC_RunInfo.MotorSpeed, &PID_MotorSpd); // 电机速度闭环输出
	TorQueBySpd += SpdMotorByIdc;
	
	//限制车速低于6km/h
	if(p_MC_WorkMode != MC_WorkMode_Config) //运行模式,推行限速6km/h
	{
	  TorQueBySpd = (uint16_t)((uint32_t)(TorQueBySpd * Function_Linear_3Stage(55, 0, 55, 128, MC_RunInfo.BikeSpeed)) >> 10);
		if(MC_RunInfo.BikeSpeed > 60)
		{
			MC_WalkProcess_Param.MotorSpeedSetBigin = 0;
			MC_MotorStop(&MC_StarFlag);
		}
		else
		{
			//电机启动
			MC_MotorStar(&MC_StarFlag);
		}
	}
	else //配置模式不限速
	{
	  //电机启动
	  MC_MotorStar(&MC_StarFlag);
	}
	
	#if 1
	//根据电压调节输出
	TorQueBySpd = (int16_t)((int32_t)TorQueBySpd * Function_Linear_3Stage(258, 21, 328, 0, MC_RunInfo.BusVoltage >> 7) >> 10);//电压单位按照(mV / 128)计算
	
	//根据温度调节输出
	TorQueBySpd = (int16_t)((int32_t)TorQueBySpd * Function_Linear_3Stage(MC_ConfigParam.TempTH_Alarm, 0, MC_ConfigParam.TempTH_Alarm, 26, MC_RunInfo.T_Roil) >> 10);
	#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

	p_MC_CalParam.Ref_Speed = TorQueBySpd;
	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)
{
  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;
	static uint32_t TorqueStopDelayTimeCnt = 0; //低力矩停机计时
	uint16_t TorqueStopDelayTime;
	static int16_t IqRefByInPower;           //限流计算结果
  static uint16_t CurrentLimitPresent;     //限流实际值，做升降速处理
	uint16_t CurrentLimitSet;                //限流设置值，不同助力档位更新
	
	#if 1
	
	//踩踏力矩输入
  MC_TorqueProcess_Param.TorqueApp = SenorData;
	
	#elif 0
	
	//输入阶跃
	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 = (MC_TorqueCorrectParam.StarData < 200) ? 100 : (MC_TorqueCorrectParam.StarData >> 1);
	if(MC_TorqueProcess_Param.TorqueApp >= (TorqueStopData))
	{
	  TorqueStopDelayTimeCnt = HAL_GetTick();
	}
	else
	{
		if(MC_RunInfo.MotorSpeed > 300)
		{
		  TorqueStopDelayTime = 327600 / MC_RunInfo.MotorSpeed + 200;
		}
		else
		{
		  TorqueStopDelayTime = 1200;
		}
		TorqueStopDelayTime = (TorqueStopDelayTime > 1200) ? 1200 : TorqueStopDelayTime;
		TorqueStopDelayTime = (TorqueStopDelayTime < 300) ? 300 : TorqueStopDelayTime;
		
		if((HAL_GetTick() - TorqueStopDelayTimeCnt) > TorqueStopDelayTime)//超时1200ms
		{
			MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
		}
	}
	
	//启动值判断
	if(MC_RunInfo.BikeSpeed > 60)
	{
		TorqueStartData =  (MC_TorqueCorrectParam.StarData < 200 ? 150 \
																														 : (MC_TorqueCorrectParam.StarData > 700 ? 525 \
																																																		 : (MC_TorqueCorrectParam.StarData * 3 >> 2)));
	}
	else
	{
		TorqueStartData =  (MC_TorqueCorrectParam.StarData < 200 ? 150 \
																														 : (MC_TorqueCorrectParam.StarData > 700 ? 525 \
																																																		 : MC_TorqueCorrectParam.StarData));
	}
	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 < 10)
		{
		  MC_TorqueProcess_Param.TorqueRefEnd = 0;
			//停机处理
			MC_MotorStop(&MC_StarFlag);
		}
		else
		{
		  MC_TorqueProcess_Param.TorqueRefEnd -= 14;  //这里影响到停止踩踏后的断电时间
			MC_MotorStar(&MC_StarFlag);
		}
	}
	//力矩给定升降速处理
	else
	{
		//按照助力档位调节力矩输入值
		switch(GearSt)
		{
			case MC_GearSt_Torque_ECO:
			{
				//控制输入给定加速斜率
				TorqueAccStep = MC_AssisParam.Gear_ECO.AccCnt;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_ECO.DecCnt;
				//随力矩输入调节助力比
				if(MC_ConfigParam.StarModel == MC_StarMode_DYNAMIC)//强劲模式
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(307, MC_AssisParam.Gear_ECO.Gain_K, MC_AssisParam.Gear_ECO.Upper_Iq, MC_TorqueProcess_Param.TorqueApp)) >> 10);
					TorqueAccStep <<= 1;
					TorqueDecStep <<= 1;
				}
				else
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(102, MC_AssisParam.Gear_ECO.Gain_K, MC_AssisParam.Gear_ECO.Upper_Iq, 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_ConfigParam.CurrentLimit * 1000 >> 17);
				CurrentLimitPresent = CurrentLimitSet * 100;
				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;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_NORM.DecCnt;
				//随力矩输入调节助力比
				if(MC_ConfigParam.StarModel == MC_StarMode_DYNAMIC)//强劲模式
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(512, MC_AssisParam.Gear_NORM.Gain_K, MC_AssisParam.Gear_NORM.Upper_Iq, MC_TorqueProcess_Param.TorqueApp)) >> 10);
					TorqueAccStep <<= 1;
					TorqueDecStep <<= 1;
				}
				else
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(204, MC_AssisParam.Gear_NORM.Gain_K, MC_AssisParam.Gear_NORM.Upper_Iq, 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_ConfigParam.CurrentLimit * 1000 >> 17);
				CurrentLimitPresent = CurrentLimitSet * 100;
				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;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_SPORT.DecCnt;
				//随力矩输入调节助力比
				if(MC_ConfigParam.StarModel == MC_StarMode_DYNAMIC)//强劲模式
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(718, MC_AssisParam.Gear_SPORT.Gain_K, MC_AssisParam.Gear_SPORT.Upper_Iq, MC_TorqueProcess_Param.TorqueApp)) >> 10);
					TorqueAccStep <<= 1;
					TorqueDecStep <<= 1;
				}
				else
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(307, MC_AssisParam.Gear_SPORT.Gain_K, MC_AssisParam.Gear_SPORT.Upper_Iq, 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_ConfigParam.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_CurrentLimit_Linear_Process(CurrentLimitSet, CurrentLimitPresent);
				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;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_TURBO.DecCnt;
				//随力矩输入调节助力比
				if(MC_ConfigParam.StarModel == MC_StarMode_DYNAMIC)//强劲模式
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(819, MC_AssisParam.Gear_TURBO.Gain_K, MC_AssisParam.Gear_TURBO.Upper_Iq, MC_TorqueProcess_Param.TorqueApp)) >> 10);
					TorqueAccStep <<= 1;
					TorqueDecStep <<= 1;
				}
				else
				{
					Torque_Temp = (uint16_t)((uint32_t)(MC_TorqueProcess_Param.TorqueApp * Coefficient_GainCal(409, MC_AssisParam.Gear_TURBO.Gain_K, MC_AssisParam.Gear_TURBO.Upper_Iq, 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_ConfigParam.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_CurrentLimit_Linear_Process(CurrentLimitSet, CurrentLimitPresent);
				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;
				//控制输入给定减速斜率
				TorqueDecStep = MC_AssisParam.Gear_SMART.DecCnt;
				//助力比控制系数
				Torque_Temp = (uint32_t)(MC_TorqueProcess_Param.TorqueApp * MC_TorqueProcess_Param.TorqueApp) / (MC_AssisParam.Gear_SMART.i_Sport_TH);
				if(MC_ConfigParam.StarModel == MC_StarMode_DYNAMIC)//强劲模式
				{
					Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Coefficient_GainCal(1024, 1382, MC_AssisParam.Gear_SMART.i_Sport_TH, Torque_Temp)) >> 10);
					TorqueAccStep <<= 1;
					TorqueDecStep <<= 1;
				}
				//给定下限
				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_ConfigParam.CurrentLimit * 1000 >> 17) * MC_CadenceLimit_K;
				CurrentLimitPresent = MC_CurrentLimit_Linear_Process(CurrentLimitSet, CurrentLimitPresent);
				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;
			}
		}
		
		//随车速调节助力比
		if(MC_ConfigParam.StarModel == MC_StarMode_DYNAMIC)
		{
			Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Function_Linear_3Stage(90, -2, MC_ConfigParam.SpeedLimit * 10, 82, MC_RunInfo.BikeSpeed)) >> 10);
		}
		else if(MC_ConfigParam.StarModel == MC_StarMode_SOFT)
		{
			Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Function_Linear_3Stage(120, -3, MC_ConfigParam.SpeedLimit * 10, 82, MC_RunInfo.BikeSpeed)) >> 10);
		}
		else
		{
			Torque_Temp = (uint16_t)((uint32_t)(Torque_Temp * Function_Linear_3Stage(MC_ConfigParam.SpeedLimit * 10, 0, MC_ConfigParam.SpeedLimit * 10, 82, MC_RunInfo.BikeSpeed)) >> 10);
		}
		
		//限流计算
		IqRefByInPower =  PID_Regulator(CurrentLimitPresent / 100, (MC_RunInfo.BusCurrent >> 7), &PID_IMax); 
		MC_TorqueProcess_Param.TorqueRef = Torque_Temp + IqRefByInPower;
		if(MC_TorqueProcess_Param.TorqueRef <= 0)
		{
			MC_TorqueProcess_Param.TorqueRef = 0;
		}
		
		//升降速曲线计算
		if(IqRefByInPower < 0)
		{
			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 * 5;
			}
		}
		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_RunInfo.BikeSpeed > ((MC_ConfigParam.SpeedLimit + 1) * 10))
		{
			MC_TorqueProcess_Param.MotorStopLock_Flag = SET;
			MC_TorqueProcess_Param.TorqueRefEnd = 0;
			//停机处理
			MC_MotorStop(&MC_StarFlag);
		}
		else if(MC_RunInfo.BikeSpeed < ((MC_ConfigParam.SpeedLimit - 1) * 10))
		{
		  MC_MotorStar(&MC_StarFlag);
		}
	}
	
	#if 1
	//根据电压调节输出
	MC_TorqueProcess_Param.TorqueRefEnd = (int16_t)((int32_t)MC_TorqueProcess_Param.TorqueRefEnd * Function_Linear_3Stage(258, 21, 328, 0, MC_RunInfo.BusVoltage >> 7) >> 10);//电压单位按照(mV / 128)计算
	
	//根据温度调节输出
	MC_TorqueProcess_Param.TorqueRefEnd = (int16_t)((int32_t)MC_TorqueProcess_Param.TorqueRefEnd * Function_Linear_3Stage(MC_ConfigParam.TempTH_Alarm, 0, MC_ConfigParam.TempTH_Alarm, 26, MC_RunInfo.T_Roil) >> 10);
	#endif
	
	p_MC_CalParam.Ref_Torque = (int16_t)(MC_TorqueProcess_Param.TorqueRefEnd >> 1);
	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();
	//力矩传感器零点初值
	TorqueOffSetDefaultData_Init(&TorqueOffSetData, ADC1_Result[ADC1_RANK_TORQUE_SENSOR]);
	//指拨零点初值
	GasSensorOffSet_Init(&GasSensor_OffSet, ADC1_Result[ADC1_RANK_GAS]);
}

//MC控制初始化
void MC_Init(void)
{	
	//PID参数初始化
	PID_Init(&PID_Torque_InitStructure,\
					 &PID_Flux_InitStructure,\
					 &PID_Weak_InitStructure,\
					 &PID_IMax,\
					 &PID_MotorSpd,\
					 &PID_ConstantPower\
					);	
	
	//助力参数初始化
	UpdateGearParam(MC_ConfigParam.SerialNum);
	
	//三相电流零点校准
	SVPWM_3ShuntCurrentReadingCalibration(&MC_ErrorCode);
	
	//母线电流零点校准
	CurrentReadingCalibration(&MC_ErrorCode);
	
	//力矩传感器零点值处理
	TorqueOffSetData_Process(&TorqueOffSetData, ADC1_Result[ADC1_RANK_TORQUE_SENSOR]);
	
	//智能档位初始化处理
	if(MC_ConfigParam.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.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, 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(p_ADC_SensorData.GasSensor, GearSt, Break_Flag);
	
	//发生助力模式切换时，清空变量
	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.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_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;
	}
}