#include "FocControl.h"
#include "FocConfig.h"
#include "FocObserver.h"
#include "FocComponentsSvpwm.h"
#include "arm_math.h" 
#include "PIController.h"
#include <string.h>
#include "main.h"
#include "FocSample.h"
#include "FocStartUp.h"
#include "focDebug.h"
#include "bsp.h"
#include "stdio.h"
PIController_t gIDLoop;   //d轴电流环
PIController_t gIQLoop;   //q轴电流环
PIController_t gSpeedLoop;//速度环
ComponentsSvpwm_t gSvpwm; //svpwm结构体
FocSample_t gAdcSample;   //电流采样结构体
ifStartup_t gIfStartup;   //if启动结构体
ObserverSmc_t gObSmc;        //观测器结构体
ObserverLuenberger_t gObLgb;//龙伯格观测器
control_t gFocMachine;    //foc状态机 


void CurrentLoopTest(void)
{
		static float time;
		time += FOC_PWM_PERIOD;
		if(time < 5.0f)
		{
			 gIfStartup.iq = 0.05f;
		}
		else if(time >= 5.0f && time < 10.0f)
		{
			gIfStartup.iq = 0.5f;
		}
		else
		{
			time = 0;
		}
}
/*******************************************************************************
 * @brief   foc主函数
 * @param   
 * @return  
 ******************************************************************************/
void FocMain(void)
{
	 BspInit();
	EN_GATE_RESET;
	HAL_Delay(50);
	EN_GATE_SET;
	HAL_Delay(50);
	gFocMachine.sw = 1;
	gFocMachine.target = 0.5f;
	 for(;;)
		{
			DoControl(&gFocMachine);
			VofaGraphSend(2);
		}
}
void USART2_IRQHandler(void)
{
  /* USER CODE BEGIN USART2_IRQn 0 */
	extern uint8_t aRxBuffer[];
	extern float comm[6];
	extern CRC_HandleTypeDef hcrc;
	extern DMA_HandleTypeDef hdma_usart2_rx;
  extern UART_HandleTypeDef huart2;
	uint32_t crc_calc, crc_comm;
	int speed;
  /* USER CODE END USART2_IRQn 0 */
  HAL_UART_IRQHandler(&huart2);
  /* USER CODE BEGIN USART2_IRQn 1 */
	if( __HAL_UART_GET_FLAG( &huart2, UART_FLAG_IDLE) != RESET)
	{
		int len = 100 - hdma_usart2_rx.Instance->CNDTR;
		//获取转速
		char *p = strstr((char*)aRxBuffer,"speed");
		if(p != NULL)
		{
			 if(sscanf(&p[5],"%d",&speed) == 1)
			 {
				  gSpeedLoop.expTarget = SpeedToRads(speed);
			 }
		}
		HAL_UART_DMAStop(&huart2);
		memset(aRxBuffer,0,100);
		HAL_UART_Receive_DMA( &huart2, aRxBuffer, 100);
		__HAL_UART_CLEAR_FLAG( &huart2, UART_CLEAR_IDLEF);
	}

  /* USER CODE END USART2_IRQn 1 */
}
/*******************************************************************************
 * @brief   外设初始化
 * @param   
 * @return  
 ******************************************************************************/
void BspInit(void)
{
}
/*******************************************************************************
 * @brief   对各个模块进行初始化
 * @param   
 * @return  
 ******************************************************************************/
void ControlInit(void)
{
	memset(&gIDLoop,0,sizeof(gIDLoop));
	memset(&gIQLoop,0,sizeof(gIQLoop));
	memset(&gSvpwm ,0,sizeof(gSvpwm));
	memset(&gIfStartup ,0,sizeof(gIfStartup));
	memset(&gObSmc ,0,sizeof(gObSmc));
	memset(&gObLgb ,0,sizeof(gObLgb));
	//D轴电流初始化
	gIDLoop.kp = CURRENT_ID_KP;
	gIDLoop.ki = CURRENT_ID_KI;
	gIDLoop.maxValue = CURRENT_ID_OUT_MAX;
	gIDLoop.minValue = CURRENT_ID_OUT_MIN;
	gIDLoop.integSat = CURRENT_ID_INTEG_SAT;
	//Q轴电流初始化
	gIQLoop.kp = CURRENT_IQ_KP;
	gIQLoop.ki = CURRENT_IQ_KI;
	gIQLoop.maxValue = CURRENT_IQ_OUT_MAX;
	gIQLoop.integSat = CURRENT_IQ_INTEG_SAT;
	gIQLoop.minValue = CURRENT_IQ_OUT_MIN;
	//速度环初始化
	gSpeedLoop.kp = SPEED_LOOP_KP;
	gSpeedLoop.ki = SPEED_LOOP_KI;
	gSpeedLoop.integSat = SPEED_INTEG_SAT;
	gSpeedLoop.maxValue = SPEED_LOOP_OUT_SAT;
	gSpeedLoop.minValue = -SPEED_LOOP_OUT_SAT;
	gSpeedLoop.expTarget = SpeedToRads(1400.0f);
	gSpeedLoop.out = 0;
	//观测器初始化
	ObserverSmcInit(&gObSmc);
	ObserverLuenbergerInit(&gObLgb);
	//svpwm初始化
	gSvpwm.vBus = 24.0f;
	
	#if MOTOR_CONTORL_MODE == MOTOR_CONTORL_VOLTAGE_OPEN_LOOP //电压开环控制 id控制
	gIDLoop.maxValue = 24 * 0.5;
	gIDLoop.ki = 14 * 0.0005f;
	gIDLoop.kp = 0.010f * 0.0005f;
	gIDLoop.kc = 0;
	gSvpwm.vBus = 24.0;
  #endif 
}
/*******************************************************************************
 * @brief   ADC中断，foc执行周期
 * @param   
 * @return  
 ******************************************************************************/
long current[3],time[3];
int _delayCnt;

void HAL_ADCEx_InjectedConvCpltCallback( ADC_HandleTypeDef *hadc)
{

  static char speedLoopCnt;
	float realOmega,obTheta;
	if( hadc->Instance == hadc1.Instance)
	{
			if(_delayCnt < 20000)
			{
				_delayCnt++;
				return;
			}
	    current[1] = 2048 - HAL_ADCEx_InjectedGetValue( &hadc1, ADC_INJECTED_RANK_1);//pHandle->bCurrentOffset);
  	  current[2] = 2048 - HAL_ADCEx_InjectedGetValue( &hadc2, ADC_INJECTED_RANK_1);	
			current[0] = -(current[1] + current[2]);
			gAdcSample.aCurrent = current[0];
			gAdcSample.bCurrent = current[1];
			gAdcSample.cCurrent = current[2];
			CurrentSampleStep(&gAdcSample,gSvpwm.sin_cos);

		 
//速度闭环模式
#if  MOTOR_CONTORL_SPEED_LOOP == MOTOR_CONTORL_MODE

#endif
		#if OB_MODE == OB_MODE_SMC
		//估算角度
		ObserverSmcStep(&gObSmc,gSvpwm.vAlpha,gSvpwm.vBeta,gAdcSample.iAlpha,gAdcSample.iBeta);
		obTheta = gObSmc.theta;
		realOmega = gObSmc.outOmega;
		#elif OB_MODE == OB_MODE_LGB
			
		ObserverLuenbergerStep(&gObLgb,gSvpwm.vAlpha,gSvpwm.vBeta,gAdcSample.iAlpha,gAdcSample.iBeta);
		obTheta = gObLgb.theta;
		realOmega = gObLgb.outOmega;			
			
		#endif
		//if启动
		IfStartupStep(&gIfStartup,obTheta,&gSvpwm.sin_cos[0]);

		if(speedLoopCnt++ > 30 && gIfStartup.state == 3)
		{
				//小于转速时就增加目标速度
				if(gSpeedLoop.target < gSpeedLoop.expTarget)
				{
					gSpeedLoop.target += SpeedToRads(SPEED_RAMP_INC) * 60.0f;
					if(gSpeedLoop.target > gSpeedLoop.expTarget)
					{
						gSpeedLoop.target = gSpeedLoop.expTarget;
					}
				}

				//低于目标速度时就减少速度
				if(gSpeedLoop.target > gSpeedLoop.expTarget)
				{
					gSpeedLoop.target -= SpeedToRads(SPEED_RAMP_DEC) * 60.0f;
					if(gSpeedLoop.target < gSpeedLoop.expTarget)
					{
						gSpeedLoop.target = gSpeedLoop.expTarget;
					}
				}				
				//对于目标速度进行限幅
				if(SpeedToRads(SPEED_MAX_TARGET) <= gSpeedLoop.target)
				{
					 gSpeedLoop.target = SpeedToRads(SPEED_MAX_TARGET);
				}	
			  if(SpeedToRads(SPEED_MIN_TARGET) >= gSpeedLoop.target)
				{
					 gSpeedLoop.target = SpeedToRads(SPEED_MIN_TARGET);
				}
				gSpeedLoop.target = gSpeedLoop.target;
				gSpeedLoop.real = realOmega;
				SpeedLoopStep(&gSpeedLoop);
				if(gFocMachine.isEnterSpeedLoop == 0)
				{
					gSpeedLoop.lastout = gIfStartup.iq;
					gFocMachine.isEnterSpeedLoop = 1;				
				}
		  	speedLoopCnt = 0;
		}
		//计算电流环		
		if(gFocMachine.isEnterSpeedLoop)
		{
			 gIfStartup.iq = gSpeedLoop.out;
		}		
		gIDLoop.target = 0;
		gIDLoop.real = gAdcSample.iD;
		gIQLoop.target = gIfStartup.iq;
		gIQLoop.real = gAdcSample.iQ;
		
		PIControllerStep(&gIQLoop);
		PIControllerStep(&gIDLoop);
		gSvpwm.vBus = 24.0f;
		gSvpwm.vd = gIDLoop.out;
		gSvpwm.vq = gIQLoop.out;
		SvpwmStep(&gSvpwm);
		//svpwm时间赋值给定时器
		TIM1->CCR1 = 4199 * gSvpwm.ta;
		TIM1->CCR2 = 4199 * gSvpwm.tb;
		TIM1->CCR3 = 4199 * gSvpwm.tc;	
		float graph[3];
		graph[0] = gAdcSample.aCurrent;
		graph[1] = gIQLoop.real;
		graph[2] = gObLgb.outOmega * (60.0f / 2.0 / 6.28);
		GraphDataCopy(graph,3);
	}
} 
	
/*******************************************************************************
 * @brief   PWM开始
 * @param   
 * @return  
 ******************************************************************************/
void PwmStart(void)
{
	HAL_TIM_PWM_Start( &htim1, TIM_CHANNEL_1);
	HAL_TIM_PWM_Start( &htim1, TIM_CHANNEL_2);
	HAL_TIM_PWM_Start( &htim1, TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Start( &htim1, TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Start( &htim1, TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Start( &htim1, TIM_CHANNEL_3);
}
/*******************************************************************************
 * @brief   PWM停止
 * @param   
 * @return  
 ******************************************************************************/
void PwmStop(void)
{
	extern TIM_HandleTypeDef htim1;
	HAL_TIM_PWM_Stop( &htim1, TIM_CHANNEL_1);
	HAL_TIM_PWM_Stop( &htim1, TIM_CHANNEL_2);
	HAL_TIM_PWM_Stop( &htim1, TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Stop( &htim1, TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Stop( &htim1, TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Stop( &htim1, TIM_CHANNEL_3);;	
}
/*******************************************************************************
 * @brief   电机控制状态机
 * @param   pHandle控制实例
 * @return  
 ******************************************************************************/
void DoControl(control_t *pHandle)
{

	switch(pHandle->machineState)
	{
#if MOTOR_CONTORL_MODE == MOTOR_CONTORL_VOLTAGE_OPEN_LOOP //电压开环控制 id控制
#define OLTAGE_OPEN_LOOP_OMEGA             (10.0f * 6.28f)   //开环时速度 
#define OLTAGE_OPEN_LOOP_ID                (0.3A)   //开环时D轴电流
		case FOC_STATE_IDLE:
			if(pHandle->sw)
			{
				ControlInit();
				pHandle->machineState = FOC_STATE_ALIGN;
				pHandle->openVoltageTheta = 0;
				PwmStart();
			}
			break;
		
		case FOC_STATE_ALIGN:	//对其角度0
		for(float i = 0.001f;i < 0.3f;i += 0.001f)
		{
			 gIDLoop.target = i;
		}
		pHandle->machineState = FOC_STATE_RUN;
			break;
		
		case FOC_STATE_RUN://对其位置后开环拖动电机运行
			gSvpwm.vd = 2.0f;
		  gSvpwm.vq = 0.0f;
		  pHandle->openVoltageTheta += OLTAGE_OPEN_LOOP_OMEGA * FOC_PWM_PERIOD;
			if(pHandle->openVoltageTheta > 6.28f)
			{
				 pHandle->openVoltageTheta -= 6.28f;
			}
			gSvpwm.sin_cos[0] = arm_sin_f32(pHandle->openVoltageTheta);
			gSvpwm.sin_cos[1] = arm_sin_f32(pHandle->openVoltageTheta);
			break;
		case FOC_STATE_STOP:
			break;
#elif MOTOR_CONTORL_MODE == MOTOR_CONTORL_CURRENT_LOOP //电流环模式
		case FOC_STATE_IDLE:
			if(pHandle->sw)
			{
				ControlInit();
				pHandle->machineState = FOC_STATE_RUN;
				pHandle->openVoltageTheta = 0;
				pHandle->isEnterSpeedLoop = 0;
				PwmStart();
			}
			break;
		
		case FOC_STATE_ALIGN:	//对其角度0
//		for(float i = 0.001f;i < 0.3f;i += 0.001f)
//		{
//			 gIQLoop.target = i;
//			
//		}
		pHandle->machineState = FOC_STATE_RUN;
		break;	
		case FOC_STATE_RUN://直接进入闭环
			gIQLoop.target = gIfStartup.iq;
			break;
		case FOC_STATE_STOP:
			ControlInit();
			break;
#endif
	}
}