#include "motor.h"


MOTORSTR MotorStr;
MOTOR_STATE_STR LockCarState;
motor_state motor_cmd = motor_stop;
void motor_init(void)
{
	GPIO_Init(MOTOR_GPIO,MOTOR_FALL_PIN,GPIO_Mode_Out_PP_High_Fast);
	GPIO_Init(MOTOR_GPIO,MOTOR_RISE_PIN,GPIO_Mode_Out_PP_High_Fast);
	GPIO_Init(LIMIT_GPIO,LIMIT_PIN,GPIO_Mode_In_FL_No_IT);
	CLK_PeripheralClockConfig(CLK_Peripheral_ADC1, ENABLE);
	ADC_Init(ADC1,ADC_ConversionMode_Single,ADC_Resolution_12Bit,ADC_Prescaler_1);
	ADC_SamplingTimeConfig(ADC1, ADC_Group_SlowChannels, ADC_SamplingTime_384Cycles);
	ADC_Cmd(ADC1,ENABLE);
}

void motor_deinit(void)
{
    ADC_Cmd(ADC1,DISABLE);
    GPIO_Init(MOTOR_GPIO,MOTOR_FALL_PIN,GPIO_Mode_In_FL_No_IT);
    GPIO_Init(MOTOR_GPIO,MOTOR_RISE_PIN,GPIO_Mode_In_FL_No_IT);
}
uint16_t MotorADGet(void)
{
        ADC_Cmd(ADC1,ENABLE);
        ADC_ChannelCmd(ADC1,ADC_Channel_22,DISABLE);
	ADC_ChannelCmd(ADC1,ADC_Channel_2,ENABLE);
	ADC_SoftwareStartConv(ADC1);
	while(ADC_GetFlagStatus(ADC1,ADC_FLAG_EOC)==RESET);
	return ADC_GetConversionValue(ADC1);
	//LockCarState.
}
static void motor_act(motor_state cmd)
{
	switch(cmd)
	{
	case motor_stop:
	{
		GPIO_WriteBit(MOTOR_GPIO,MOTOR_FALL_PIN,SET);
		GPIO_WriteBit(MOTOR_GPIO,MOTOR_RISE_PIN,SET);
		break;
	}
	case motor_rise:
	{
		GPIO_WriteBit(MOTOR_GPIO,MOTOR_FALL_PIN,RESET);
		GPIO_WriteBit(MOTOR_GPIO,MOTOR_RISE_PIN,SET);
		break;
	}
	case motor_fall:
	{
		GPIO_WriteBit(MOTOR_GPIO,MOTOR_FALL_PIN,SET);
		GPIO_WriteBit(MOTOR_GPIO,MOTOR_RISE_PIN,RESET);
		break;
	}
	default:
	{
		break;
	}
	}
}
void motor_contro_act(motor_state *cmd)
{
	float df;
	static uint8_t OverCurrentCount = 0;
	static uint8_t MotorTurnCount = 0;
	static bool MotorActionFlag = FALSE;
	static motor_state motor_cur_state = motor_stop;
	static motor_state motor_next_state = motor_stop;
	if(!MOTOR_TEST)return;
        
	if(MotorActionFlag==FALSE)
	{
		MotorActionFlag = TRUE;
		motor_next_state = *cmd;
	}
	motor_cur_state = motor_next_state;
	switch(motor_cur_state)
	{
	case motor_idle:
	{
                
		break;
	}
	case motor_rise:
	{
                motor_act(motor_stop);
                if(MotorStr.delay_ms500)break;
		motor_act(motor_cur_state);
		motor_next_state = motor_delay_ms200;
		break;
	}
	case motor_fall:
	{
                motor_act(motor_stop);
                if(MotorStr.delay_ms500)break;
		motor_act(motor_cur_state);
		motor_next_state = motor_delay_ms200;
		break;
	}
	case motor_delay_ms200:
	{
		MotorStr.motor_delay = MOTOR_DELAY;
		MotorStr.delay_ms200 = MOTOR_DELAY_MS200;
		motor_next_state = motor_busy;
		break;
	}
	case motor_busy:
	{
		if(MotorStr.delay_ms200)break;
		df = MotorADGet()/4096.0*3.3/0.1;
		ADC_ClearFlag(ADC1,ADC_FLAG_EOC);
		ADC_ChannelCmd(ADC1,ADC_Channel_2,DISABLE);
		if(df>1.2)
			OverCurrentCount++;
		else
			OverCurrentCount = 0;
		if((OverCurrentCount>=10)||MotorStr.motor_delay==0)
		{
			OverCurrentCount = 0;
			motor_next_state = motor_turn;
		}
		break;
	}
	case motor_turn:
	{
		if(*cmd == motor_rise)
		{
			if(LIMIT_READ)
			{
				motor_next_state = motor_stop;
				MotorStr.motor_delay = 0;
				MotorTurnCount = 0;
				bsp_beep_two();
				//beep_start(1,3,2);
				LockCarState.MotorActResults = RISE_OK;
			}
			else
			{
				//if(!MOTOR_TEST)printf("cnt= %d\n",MotorTurnCount);
				MotorTurnCount++;
				if(MotorTurnCount%2)
				{
					motor_next_state = motor_fall;
                                        MotorStr.delay_ms500 = MOTOR_DELAY_MS500;
				}
				else
				{
					motor_next_state = motor_rise;
                                        MotorStr.delay_ms500 = MOTOR_DELAY_MS500;
				}
				if(MotorTurnCount>=6)
				{
					MotorTurnCount = 0;
					motor_next_state = motor_stop;
					LockCarState.MotorActResults = RISE_ERROR;
				}
			}
		}
		else if(*cmd == motor_fall)
		{
			LockCarState.MotorActResults = FALL_ERROR;
			if(LIMIT_READ==0)
			{
				bsp_beep_two();//beep_start(1,3,2);
				LockCarState.MotorActResults = FALL_OK;
                                QMC5883LStr.DataOkFlag=0;
			}
			motor_next_state = motor_stop;
		}
		break;
	}
	case motor_stop:
	{               
		OverCurrentCount = 0;
		MotorTurnCount = 0;
		MotorActionFlag = FALSE;
		*cmd = motor_stop;
		motor_act(motor_cur_state);
		motor_next_state = motor_idle;
                motor_deinit();
		break;
	}
	default:
		break;
	}
}

void CarAction(bool ReportFlag,uint8_t RepotCnt,enum CMD_SOURCE CmdSource,enum MOTOR_CMD MotorCmd)
{
        motor_init();
        LockCarState.HostReportDelay = 2;
	LockCarState.HostReportFlag = ReportFlag;
	LockCarState.HostReportCnt = RepotCnt;
	LockCarState.MotorCmdSource = CmdSource;
	LockCarState.MotorCmd = MotorCmd;        
}

