/*********************************************************************
	* 1.Description					: Drive by back electromotive force 
	* 2.Hardware solution		: 9ut3-v2 GD32F303VET6 & EG3112 & TSM045NB06CR
	* 3.Motor 3phase 				: 71BLDC040/24VDC 0.4N.m/2100+-10%rpm 5A/SN:00002/Date:2024.01.29
	* 4.Author/Engineer			: Nieyansheng
	*
	*	
	******************************************************************************/

#define MOT_LOCAL
#include "bemf.h"
#include <stdio.h>


typedef unsigned          char uint8_t;
typedef unsigned short     int uint16_t;
typedef unsigned           int uint32_t;

#define BI1_U		gpio_input_bit_get(GPIOE, GPIO_PIN_14)==SET
#define BI0_U		gpio_input_bit_get(GPIOE, GPIO_PIN_14)==RESET
#define BI1_V		gpio_input_bit_get(GPIOE, GPIO_PIN_7)==SET
#define BI0_V		gpio_input_bit_get(GPIOE, GPIO_PIN_7)==RESET
#define BI1_W		gpio_input_bit_get(GPIOE, GPIO_PIN_15)==SET
#define BI0_W		gpio_input_bit_get(GPIOE, GPIO_PIN_15)==RESET


#define U_1     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_0,mci.pulse);\
								gpio_bit_reset(GPIOE, GPIO_PIN_8);							
#define U_0     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_0,0);\
								gpio_bit_set(GPIOE, GPIO_PIN_8);							
#define U_F     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_0,0);\
								gpio_bit_reset(GPIOE, GPIO_PIN_8);
								
#define V_1     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_1,mci.pulse);\
								gpio_bit_reset(GPIOE, GPIO_PIN_10);								
#define V_0     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_1,0);\
								gpio_bit_set(GPIOE, GPIO_PIN_10);								
#define V_F     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_1,0);\
								gpio_bit_reset(GPIOE, GPIO_PIN_10);

#define W_1     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_2,mci.pulse);\
								gpio_bit_reset(GPIOE, GPIO_PIN_12);								
#define W_0     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_2,0);\
								gpio_bit_set(GPIOE, GPIO_PIN_12);								
#define W_F     timer_channel_output_pulse_value_config(TIMER0,TIMER_CH_2,0);\
								gpio_bit_reset(GPIOE, GPIO_PIN_12);
								
uint8_t  match = 0;								
#define UW      U_1;W_0;V_F;match=2;
#define UV      U_1;V_0;W_F;match=3;
#define WV      W_1;V_0;U_F;match=1;
#define WU      W_1;U_0;V_F;match=2;
#define VU      V_1;U_0;W_F;match=3;
#define VW      V_1;W_0;U_F;match=1;
#define BRAKE   V_0;W_0;U_0;
#define RELEASE V_F;W_F;U_F;

#define  STARTSTEP   6
#define  ALLSTEP    15
uint32_t start_trig[ALLSTEP];
uint32_t startbuffer[ALLSTEP]={
400000,50000,30000,20000,20000,20000,20000,16000,15000,15000,15000};//@29Vduty_speedup35
extern  unsigned short adc_value[10];
uint8_t intFlag;//中断标志
uint32_t timer2_cnt;
uint16_t speed_invert_cnt=0;
uint32_t Get_Counter(void);
void Clr_Counter(void);
void COMP_TriggerCallback(void);
void BEMF_Pid(void);
void BEMF_Sensor_NoIT(void);
void BEMF_StartInverter(unsigned char step);
void BEMF_Inverter(void);
void BEMF_Init(uint16_t dir,uint16_t speed,uint16_t dutyDefault);
void BEMF_Task(void);
/** 
  * @function	: clr counter
	* @parameter: no
	* @return		: no
  */
void Clr_Counter()
{
	timer2_cnt = 0;					//TIMER_CNT(TIMER2)=0;//TIM2->CNT = 0;//TIM_SetCounter(TIM2,0);
}

/** 
  * @function	: get counter
	* @parameter: no
	* @return		: no
  */
uint32_t Get_Counter()
{
	return (timer2_cnt);//TIMER_CNT(TIMER2);//timer_counter_read(TIMER2);//TIM2->CNT;//TIM_GetCounter(TIM2);
}
/** 
  * @function	: BEMF_Init
	* @parameter: dir 0/1/x, speed 2000 @3pole, dutyDefault 10%
	* @return		: no
  */
void BEMF_Init(uint16_t var1,uint16_t var2,uint16_t var3)
{
	if(var1 == DIR0 || (var1 == DIR1))
	{
		if((mci.alarm & (ALARM_OVERVOL|ALARM_LOWVOL)) == 0)
		{
			
			Mcp.start_gap = STARTGAP;
			Mcp.end_gap		= var2;
			Mcp.start_pulse = DUTY_BASE;
			Mcp.end_pulse  = Mcp.start_pulse+3;//(var3/(mci.voltage/100));// p600->load310 800->360
//			if(Mcp.end_pulse<Mcp.start_pulse+3)
//			{
//				Mcp.end_pulse = Mcp.start_pulse+3;
//			}
			
			mci.dir   = var1;
			mci.alarm = 0x00;
			mci.state = START;
		}
	}
	else
	{
		mci.state = STOP;
	}
}

/** 
  * @function	: api
	* @parameter: no
	* @return		: no
  */
void BEMF_Task()
{	
	static	uint8_t step;
	unsigned char i;
	
	timer2_cnt+=100;;
	switch(mci.state)
	{
		case IDEL:
		{
		}
		break;

		case START://it's ok. 
		{	
				step = 0;	// Mcp step			
				mci.invert_cnt = 0;
				mci.gap 	= Mcp.start_gap;
				mci.pulse = Mcp.start_pulse;			// Mcp duty = 5%		
				BEMF_StartInverter(step%6);				// Mcp set pwm
				mci.state = POSITION;		
				mci.spd = SPEED0;
				for(i=0;i<ALLSTEP;i++)
					start_trig[i] = 0;			
		}
		break;
		
		
		case POSITION://it's ok. /* motor position state *//*max current is 6A,holding 0.5sec.*/
		{
			if(mci.pulse < Mcp.end_pulse)					// Limit maximum current and hold time. 
			{
				if(Get_Counter() > 50000)						// Incremental interval 50ms
				{
					mci.pulse++;
					BEMF_StartInverter(step%6);
				}
			}
			else
			{
				if(Get_Counter() > startbuffer[0])						
				{
					mci.state = OPENLOOP;							// go to OPENLOOP SPEEDUP  
				}
			}
		}
		break;
		
		
		case OPENLOOP://it's ok.											/* motor openloop state */		
		{ 
			if(step < STARTSTEP)									// step 1~0 error: no back elect 
			{		
				step++;
				mci.pulse++;
				BEMF_StartInverter(step%6);
				mci.state = SWITCH;							// goto check.
			}
			else																	// step 1~0 error: no back elect 
			{				
				if(Mcp.end_pulse < DUTY_POSITIONEND && mci.invert_cnt<INVERT_CNT)//during the start state
				{
						Mcp.end_pulse+=3;
						mci.state = START;
				}
				else
				{
					mci.alarm |= ALARM_OVEROPENLOOP;
					mci.state = STOP;	
				}				
			}
		}
		break;
		
		case SWITCH://step=1,2,3...  
	  {
			if(Get_Counter() < startbuffer[step])// cycle N time, & delay y(us)
			{
				//if(Get_Counter()>500)// filter waves
				{
					BEMF_Sensor_NoIT();
					if(intFlag == 1)
					{
						intFlag = 0;
						start_trig[step] = Get_Counter();
						
						if(step >= STARTSTEP)				//last time 
						{
							BEMF_Inverter();
							mci.state = RUN;					
							mci.spd = SPEEDUP;
							break;
						}	
					}
				}	
			}
			else
			{
				mci.state = OPENLOOP;
			}			
		}
		break;
		
		case RUN:/* motor run state */
		{
			BEMF_Sensor_NoIT();
			if(intFlag == 1)
			{		
				intFlag = 0;

				if((uint8_t)mci.spd == SPEEDUP)
				{	
					if(mci.gap > Mcp.end_gap)
					{
						mci.gap = mci.gap-5;
					}
					else
					{
						mci.spd = SPEEDHOLD;
					}
					BEMF_Pid();	
				}
				else if((uint8_t)mci.spd == SPEEDHOLD)
				{
					BEMF_Pid();	
				}
				else if((uint8_t)mci.spd == SPEEDDOWN)
				{	
					if(mci.gap < (Mcp.end_gap<<1))
					{
						mci.gap = mci.gap+5;
						BEMF_Pid();	
					}					
					else
					{		
						mci.state = STOP;
						mci.spd = SPEED0;
					}
				}
			}
			
			if(Get_Counter() >= 30000)				// !!! error1: timeout
			{	
				if(Mcp.end_pulse<DUTY_POSITIONEND && mci.invert_cnt < INVERT_CNT)//during the start state
				{
						Mcp.end_pulse+=3;
						mci.state = START;
				}
				else
				{
					mci.alarm |= ALARM_TIMEOUT;
					mci.state = STOP;
					mci.spd   = SPEED0;
					RELEASE;
				}
			}
		}
		break;
		
		case STOP:/* motor stop state */
		{
			if(mci.spd == SPEEDHOLD)//run
			{
				mci.spd = SPEEDDOWN;
				mci.state = RUN;
			}
			else
			{
				RELEASE;
				if(Get_Counter() >= 60000)				// holding 1second
				{
					BRAKE;
					mci.state = IDEL;
				}
			}
		}
		break;
		
		default:
		{
			BRAKE;
			mci.state = IDEL;
		}
		break;
	}
}
/** 
  * @function	: 预驱N步
	* @parameter: 步数
	* @return		: no
  */
void BEMF_StartInverter(unsigned char stp)
{
 if(mci.dir == DIR0)	// 正向
 {
	 switch(stp)
		{	
			case 0:UW; 
			break;
			case 1:UV; 
			break;
			case 2:WV; 
			break;
			case 3:WU; 
			break;
			case 4:VU; 
			break;
			case 5:VW; 
			break;
			default:BRAKE;
			break;
				
		}
	}
  else if(mci.dir == DIR1)
  {
  	switch(stp)
		{		
			case 0:WU;
			break;
			case 1:WV;
			break;
			case 2:UV;
			break;
			case 3:UW;  
			break;
			case 4:VW;  
			break;
			case 5:VU;  
			break;
			default:BRAKE;
			break;			
		}
	}
	Clr_Counter();	
}
/** 
  * @function	: change step
	* @parameter: no
	* @return		: no
  */
void BEMF_Inverter()
{
  if(mci.dir == DIR0)					// 正向
  {
    if(BI1_U && BI1_V && BI0_W)			// U_0-1  011
    {
      UW; 
    }
    else if(BI1_U && BI0_V && BI0_W)// V_1-0 001
    {
      UV; 
    }
    else if(BI1_U && BI0_V && BI1_W)// W_0-1 101
    {
      WV;
    }
    else if(BI0_U && BI0_V && BI1_W)// U_1-0 100
    {
      WU; 
    }
    else if(BI0_U && BI1_V && BI1_W)// V_0-1 110 
    {
      VU;
    }
    else if(BI0_U && BI1_V && BI0_W)// W_1-0 010
    {
      VW; 
    }  
    else 
    {
     // BRAKE;
    }

  }
  else if(mci.dir == DIR1)
  {
    if(BI1_U && BI1_V && BI0_W)//011
    {
      VW;
    }
    else if(BI0_U && BI1_V && BI0_W)//010
    {
      VU;
    }
    else if(BI0_U && BI1_V && BI1_W)//110
    {
      WU; 
    }
    else if(BI0_U && BI0_V && BI1_W)//100
    {
      WV;
    }
    else if(BI1_U && BI0_V && BI1_W)//101
    {
      UV;
    }
    else if(BI1_U && BI0_V && BI0_W)//001
    {
       UW;
    }  
    else {
      //BRAKE;
    }
  }
		Clr_Counter();
}
	

/** 
  * @function	: pid
	* @parameter: no
	* @return		: no
  */
void BEMF_Pid()
{
	if(Get_Counter()>200)//!!!
	{
		if(Get_Counter() < (mci.gap-100))  
		{
			if(mci.pulse > 1)
			{
				mci.pulse -= 1;
			} 
		}
		else if(Get_Counter() < (mci.gap+200)) 
		{    
		}
		else if(Get_Counter() < (mci.gap+500)) 
		{
			if(mci.pulse < DUTYMAX)
			{
				mci.pulse += 1; 
			}   
		}
		else 
		{
			if(mci.pulse < DUTYMAX)
			{	
				mci.pulse += 2;   
			} 
		}
		
		BEMF_Inverter();
		speed_invert_cnt++;
		
		if(mci.invert_cnt<1000)
			mci.invert_cnt++;
	}
}

/** 
  * @function	: trigger flag
	* @parameter: no
	* @return		: no
  */
void COMP_TriggerCallback()
{
	intFlag = 1;
}
/** 
  * @function	: trigger up & down
	* @parameter: no
	* @return		: no
  */
void BEMF_Sensor_NoIT()
{
	static unsigned char hallA;
	static unsigned char hallB;
	static unsigned char hallC;
	{
		if(BI0_U)
		{ 
			if(hallB == 1 && (match == 1))
			{
				COMP_TriggerCallback();
			}
			hallB = 0;  
		}
		else//if(BI1_U)
		{ 
			if(hallB == 0 && (match == 1))
			{
				COMP_TriggerCallback();
			}
			hallB = 1;
		}
	}
	{
		if(BI0_V)//[HALL]
		{
			if(hallA == 1 && (match == 2))
			{
				COMP_TriggerCallback();
			}
			hallA = 0;  
		}
		else//if(BI1_V)
		{ 
			if(hallA == 0 && (match == 2))
			{
				COMP_TriggerCallback();
			}
			hallA = 1;
		}
	}

	{
		if(BI0_W)
		{ 
			if(hallC == 1 && (match == 3))
			{
				COMP_TriggerCallback();
			}
			hallC = 0;  
		}
		else//if(BI1_W)
		{ 
			if(hallC == 0 && (match == 3))
			{
				COMP_TriggerCallback();
			}
			hallC = 1;
		}
	}
}

/** 
  * @function	: Calculate average
	* @parameter: *buf,*point,value,count
	* @return		: average
  */
uint16_t average_buf(uint16_t *buf,uint16_t *index,uint16_t dat,uint16_t cnt)
{
	uint32_t temp=0,i;
	
	buf[*index] = dat;
	*index +=1;
	if(*index >= cnt)
	{
		*index = 0;
	}
	for(i=0;i<cnt;i++)
	{
		temp+=buf[i];
	}
	return temp/cnt;
}

/** 
  * @function	: Calculate speed
	* @parameter: 
	* @return		: 
  */
void get_speed(void)
{
	static uint16_t cnt;
	cnt++;
	if(cnt>9999)
	{
		cnt = 0;
		mci.speed = speed_invert_cnt*10/POLE; 
		speed_invert_cnt = 0;
	}
}
/** 
  * @function	: Calculate voltage
	* @parameter: 
	* @return		: 
  */
void get_voltage(void)
{
		static uint16_t vmbuf[20];
		static uint16_t vmindex,vmvalue;
		vmvalue = ((uint32_t)adc_value[0]*3300*12)>>12;
		mci.voltage  = average_buf(vmbuf,&vmindex,vmvalue,10);
}

/** 
  * @function	: Calculate current
	* @parameter: 
	* @return		: 
  */
void get_current(void)
{
		static uint16_t imbuf[200];
		static uint16_t imindex,imvalue;
	
		if(adc_value[1] > adc_zero && adc_zero > 2048)
		{
			imvalue = ((uint32_t)(adc_value[1]-adc_zero)*3300*1000/48)>>12;
		}
		else
		{
			imvalue = 0;
		}
		
		mci.current = average_buf(imbuf,&imindex,imvalue,100);
}
/*!
    \brief      this function handles TIMER2 interrupt request.
    \param[in]  none
    \param[out] none
    \retval     none
*/
void TIMER2_IRQHandler(void)
{
	
    if(SET == timer_interrupt_flag_get(TIMER2, TIMER_INT_UP)){
			/* clear channel 0 interrupt bit */
			timer_interrupt_flag_clear(TIMER2, TIMER_INT_UP);

			get_voltage();
			get_current();
			if(mci.voltage < 10000)
			{
				mci.alarm |= ALARM_LOWVOL;
//				mci.state = BRAKESTOP;
			}
			else if(mci.voltage <= 40000)
			{
				mci.alarm &= ~(ALARM_OVERVOL|ALARM_LOWVOL);
			}
			else
			{
				mci.alarm |= ALARM_OVERVOL;
//				mci.state = BRAKESTOP;
			}//end check power


			BEMF_Task();	
	
			get_speed();	
			
			timer2++;//for main
    }
}