/*************************** (C) COPYRIGHT 2017 CXHY ****************************
 * 文件名  ：motor.c
 * 描  述  ：电机控制程序
 * 版  本  ：V1.0
 * 作  者  ：XuZhongPing
 * 日  期  ：2017/03/06 
*********************************************************************************/
#include "motor.h"
#include "delay.h"
#include "timer.h"
#include "adc.h"
#include "my_math.h"
#include "device.h"

//电机使用的IO口定义
#define MOTOR1_PORT_PeriphClock_A 	RCC_APB2Periph_GPIOB	
#define MOTOR1_PORT_A								GPIOB
#define MOTOR1_CTRL_PIN_A						GPIO_Pin_15
#define MOTOR1_PORT_PeriphClock_B 	RCC_APB2Periph_GPIOC	
#define MOTOR1_PORT_B								GPIOC
#define MOTOR1_CTRL_PIN_B						GPIO_Pin_6

#define MOTOR2_PORT_PeriphClock_A 	RCC_APB2Periph_GPIOC	
#define MOTOR2_PORT_A								GPIOC
#define MOTOR2_CTRL_PIN_A						GPIO_Pin_7
#define MOTOR2_PORT_PeriphClock_B 	RCC_APB2Periph_GPIOC	
#define MOTOR2_PORT_B								GPIOC
#define MOTOR2_CTRL_PIN_B						GPIO_Pin_8

#define MOTOR3_PORT_PeriphClock_A 	RCC_APB2Periph_GPIOC	
#define MOTOR3_PORT_A								GPIOC
#define MOTOR3_CTRL_PIN_A						GPIO_Pin_9
#define MOTOR3_PORT_PeriphClock_B 	RCC_APB2Periph_GPIOA	
#define MOTOR3_PORT_B								GPIOA
#define MOTOR3_CTRL_PIN_B						GPIO_Pin_8	

#define MOTOR4_PORT_PeriphClock_A 	RCC_APB2Periph_GPIOA	
#define MOTOR4_PORT_A								GPIOA
#define MOTOR4_CTRL_PIN_A						GPIO_Pin_11
#define MOTOR4_PORT_PeriphClock_B 	RCC_APB2Periph_GPIOA	
#define MOTOR4_PORT_B								GPIOA
#define MOTOR4_CTRL_PIN_B						GPIO_Pin_12


#define CurrentValSize	50
uint16_t motorCurrentValBuf_1[CurrentValSize];
uint16_t motorCurrentVal_1 = 0;
uint8_t motorRunningStatus_1 = 0;
uint8_t motorPreStatus_1 = 0;	

uint16_t motorCurrentValBuf_2[CurrentValSize];
uint16_t motorCurrentVal_2 = 0;
uint8_t motorRunningStatus_2 = 0;
uint8_t motorPreStatus_2 = 0;	

uint8_t motorRunningStatus_3 = 0;
uint8_t motorRunningStatus_4 = 0;
	
/*
***********************************************************************
*	函 数 名: MotorInit
*	功能说明: 电机控制IO口初始化
*	形    参：无
*	返 回 值: 无
***********************************************************************
*/
void MotorInit(void)
{ 
 	GPIO_InitTypeDef GPIO_InitStructure;
	
 	RCC_APB2PeriphClockCmd(MOTOR1_PORT_PeriphClock_A, ENABLE);
	RCC_APB2PeriphClockCmd(MOTOR1_PORT_PeriphClock_B, ENABLE);
	RCC_APB2PeriphClockCmd(MOTOR2_PORT_PeriphClock_A, ENABLE);
	RCC_APB2PeriphClockCmd(MOTOR2_PORT_PeriphClock_B, ENABLE);
	RCC_APB2PeriphClockCmd(MOTOR3_PORT_PeriphClock_A, ENABLE);
	RCC_APB2PeriphClockCmd(MOTOR3_PORT_PeriphClock_B, ENABLE);
	RCC_APB2PeriphClockCmd(MOTOR4_PORT_PeriphClock_A, ENABLE);
	RCC_APB2PeriphClockCmd(MOTOR4_PORT_PeriphClock_B, ENABLE);

	//电机1动作控制IO口初始化
	GPIO_InitStructure.GPIO_Pin  = MOTOR1_CTRL_PIN_A;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MOTOR1_PORT_A, &GPIO_InitStructure);
	GPIO_ResetBits(MOTOR1_PORT_A, MOTOR1_CTRL_PIN_A);
	
	GPIO_InitStructure.GPIO_Pin  = MOTOR1_CTRL_PIN_B;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MOTOR1_PORT_B, &GPIO_InitStructure);
	GPIO_ResetBits(MOTOR1_PORT_B, MOTOR1_CTRL_PIN_B);
	
	//电机2动作控制IO口初始化
	GPIO_InitStructure.GPIO_Pin  = MOTOR2_CTRL_PIN_A;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MOTOR2_PORT_A, &GPIO_InitStructure);
	GPIO_ResetBits(MOTOR2_PORT_A, MOTOR2_CTRL_PIN_A);
	
	GPIO_InitStructure.GPIO_Pin  = MOTOR2_CTRL_PIN_B;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MOTOR2_PORT_B, &GPIO_InitStructure);
	GPIO_ResetBits(MOTOR2_PORT_B, MOTOR2_CTRL_PIN_B);
	
	//电机3动作控制IO口初始化
	GPIO_InitStructure.GPIO_Pin  = MOTOR3_CTRL_PIN_A;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MOTOR3_PORT_A, &GPIO_InitStructure);
	GPIO_ResetBits(MOTOR3_PORT_A, MOTOR3_CTRL_PIN_A);
	
	GPIO_InitStructure.GPIO_Pin  = MOTOR3_CTRL_PIN_B;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MOTOR3_PORT_B, &GPIO_InitStructure);
	GPIO_ResetBits(MOTOR3_PORT_B, MOTOR3_CTRL_PIN_B);
	
	//电机4动作控制IO口初始化
	GPIO_InitStructure.GPIO_Pin  = MOTOR4_CTRL_PIN_A;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MOTOR4_PORT_A, &GPIO_InitStructure);
	GPIO_ResetBits(MOTOR4_PORT_A, MOTOR4_CTRL_PIN_A);
	
	GPIO_InitStructure.GPIO_Pin  = MOTOR4_CTRL_PIN_B;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MOTOR4_PORT_B, &GPIO_InitStructure);
	GPIO_ResetBits(MOTOR4_PORT_B, MOTOR4_CTRL_PIN_B);
}

/*
***********************************************************************
*	函 数 名: DoorOpen
*	功能说明: 开门
*	形    参：num -> 哪一个门
*	返 回 值: 无
***********************************************************************
*/
void DoorOpen(uint8_t num)
{
	printf("\r\n电机（%d）正转！\r\n", num);
	switch(num)
	{
		case 0x01:
			GPIO_ResetBits(MOTOR1_PORT_A, MOTOR1_CTRL_PIN_A);
			GPIO_SetBits(MOTOR1_PORT_B, MOTOR1_CTRL_PIN_B);
			motorRunningStatus_1 = 1;
			break;
		
		case 0x02:
			GPIO_ResetBits(MOTOR2_PORT_A, MOTOR2_CTRL_PIN_A);
			GPIO_SetBits(MOTOR2_PORT_B, MOTOR2_CTRL_PIN_B);
			motorRunningStatus_2 = 1;
			break;
			
		case 0x03:
			GPIO_ResetBits(MOTOR3_PORT_A, MOTOR3_CTRL_PIN_A);
			GPIO_SetBits(MOTOR3_PORT_B, MOTOR3_CTRL_PIN_B);
			motorRunningStatus_3 = 1;
			break;
			
		case 0x04:
			GPIO_ResetBits(MOTOR4_PORT_A, MOTOR4_CTRL_PIN_A);
			GPIO_SetBits(MOTOR4_PORT_B, MOTOR4_CTRL_PIN_B);
			motorRunningStatus_4 = 1;
			break;

		default:
			break;
	}
}

/*
***********************************************************************
*	函 数 名: DoorClose
*	功能说明: 关门
*	形    参：无
*	返 回 值: 无
***********************************************************************
*/
void DoorClose(uint8_t num)
{
	printf("\r\n电机（%d）反转！\r\n", num);
	switch(num)
	{
		case 0x01:
			GPIO_SetBits(MOTOR1_PORT_A, MOTOR1_CTRL_PIN_A);
			GPIO_ResetBits(MOTOR1_PORT_B, MOTOR1_CTRL_PIN_B);
			motorRunningStatus_1 = 2;
			break;
		
		case 0x02:
			GPIO_SetBits(MOTOR2_PORT_A, MOTOR2_CTRL_PIN_A);
			GPIO_ResetBits(MOTOR2_PORT_B, MOTOR2_CTRL_PIN_B);
			motorRunningStatus_2 = 2;
			break;
			
		case 0x03:
			GPIO_SetBits(MOTOR3_PORT_A, MOTOR3_CTRL_PIN_A);
			GPIO_ResetBits(MOTOR3_PORT_B, MOTOR3_CTRL_PIN_B);
			motorRunningStatus_3 = 2;
			break;
			
		case 0x04:
			GPIO_SetBits(MOTOR4_PORT_A, MOTOR4_CTRL_PIN_A);
			GPIO_ResetBits(MOTOR4_PORT_B, MOTOR4_CTRL_PIN_B);
			motorRunningStatus_4 = 2;
			break;

		default:
			break;
	}
}

/*
***********************************************************************
*	函 数 名: DoorStop
*	功能说明: 停止
*	形    参：无
*	返 回 值: 无
***********************************************************************
*/
void DoorStop(uint8_t num)
{
	printf("\r\n电机（%d）停止！\r\n", num);
	switch(num)
	{
		case 0x01:
			GPIO_ResetBits(MOTOR1_PORT_A, MOTOR1_CTRL_PIN_A);
			GPIO_ResetBits(MOTOR1_PORT_B, MOTOR1_CTRL_PIN_B);
			motorRunningStatus_1 = 0;
			break;
		
		case 0x02:
			GPIO_ResetBits(MOTOR2_PORT_A, MOTOR2_CTRL_PIN_A);
			GPIO_ResetBits(MOTOR2_PORT_B, MOTOR2_CTRL_PIN_B);
			motorRunningStatus_2 = 0;
			break;
			
		case 0x03:
			GPIO_ResetBits(MOTOR3_PORT_A, MOTOR3_CTRL_PIN_A);
			GPIO_ResetBits(MOTOR3_PORT_B, MOTOR3_CTRL_PIN_B);
			motorRunningStatus_3 = 0;
			break;
			
		case 0x04:
			GPIO_ResetBits(MOTOR4_PORT_A, MOTOR4_CTRL_PIN_A);
			GPIO_ResetBits(MOTOR4_PORT_B, MOTOR4_CTRL_PIN_B);
			motorRunningStatus_4 = 0;
			break;

		default:
			break;
	}
}

/*
***********************************************************************
*	函 数 名: Get_MotorCurrentVal
*	功能说明: 获取电机1电流采样数据
*	形    参：无	         
*	返 回 值: 无
***********************************************************************
*/
void Get_MotorCurrentVal_1(void)
{
	Data_collection(motorCurrentValBuf_1, CurrentValSize, ADC_Channel_8);
	sort_16(motorCurrentValBuf_1, CurrentValSize);           							//对存在缓存中的采集数据进行排序
	motorCurrentVal_1 = average_16(motorCurrentValBuf_1, CurrentValSize, 20);				//去掉头和尾后，求平均数
}

/*
***********************************************************************
*	函 数 名: Get_MotorCurrentVal
*	功能说明: 获取电机2电流采样数据
*	形    参：无	         
*	返 回 值: 无
***********************************************************************
*/
void Get_MotorCurrentVal_2(void)
{
	Data_collection(motorCurrentValBuf_2, CurrentValSize, ADC_Channel_9);
	sort_16(motorCurrentValBuf_2, CurrentValSize);           							//对存在缓存中的采集数据进行排序
	motorCurrentVal_2 = average_16(motorCurrentValBuf_2, CurrentValSize, 20);				//去掉头和尾后，求平均数
}


/*
***********************************************************************
*	函 数 名: Get_MotorCurrentVal
*	功能说明: 获取电机电流采样数据
*	形    参：无	         
*	返 回 值: 无
***********************************************************************
*/
void Get_MotorCurrentVal(void)
{
	Get_MotorCurrentVal_1();
	Get_MotorCurrentVal_2();
// 	printf("%4d, %4d\r\n", motorCurrentVal_1, motorCurrentVal_2);
}

/*
***********************************************************************
*	函 数 名: MotorTest
*	功能说明: 电机控制测试程序
*	形    参：无	         
*	返 回 值: 无
***********************************************************************
*/
void MotorTest(void)
{
	static uint8_t motorNum = 1;
	static uint8_t motorStatus = 0;
	
	static timer timer_01, timer_02;
	static uint8_t timer_ok = 0;
	
	if(timer_ok == 0)
	{
		timer_ok = 1;
		timer_set(&timer_01, 20);
		timer_set(&timer_02, CLOCK_SECOND*2);
	}
	switch(motorStatus)
	{
		case 0x00:
			if(timer_expired(&timer_01))
			{
				DoorOpen(motorNum);
				timer_restart(&timer_01);
				motorStatus = 0x01;
				break;
			}
			break;
			
		case 0x01:
			if(timer_expired(&timer_01))
			{
				DoorStop(motorNum);
				timer_restart(&timer_02);
				motorStatus = 0x02;
				break;
			}
			break;
			
		case 0x02:
			if(timer_expired(&timer_02))
			{
				DoorClose(motorNum);
				timer_restart(&timer_01);
				motorStatus = 0x03;
				break;
			}
			break;
			
		case 0x03:
			if(timer_expired(&timer_01))
			{
				DoorStop(motorNum);
				timer_restart(&timer_02);
				motorStatus = 0x04;
				break;
			}
			break;
			
		case 0x04:
			if(timer_expired(&timer_02))
			{
				motorNum++;
				if(motorNum >= 5)
				{
					motorNum = 1;
				}
				motorStatus = 0x00;
				break;
			}
			break;
	}
}

/*
***********************************************************************
*	函 数 名: Get_MotorCurrentVal
*	功能说明: 获取电机电流采样数据
*	形    参：无	         
*	返 回 值: 无
***********************************************************************
*/
void MotorCtrlTask2(void)
{
	static timer timer_01, timer_02;
	static uint8_t timer_ok = 0;
	static uint8_t motorStatus = 0;
	
	if(timer_ok == 0)
	{
		timer_ok = 1;
		timer_set(&timer_01, 1);
		timer_set(&timer_02, CLOCK_SECOND*2);
	}
	switch(motorStatus)
	{
		case 0x00:
			if(motorRunningStatus_2)
			{
				motorStatus = 0x01;
				timer_restart(&timer_01);
			}
			break;
			
		case 0x01:
			if(timer_expired(&timer_01))
			{
				motorStatus = 0x02;
			}
			break;
			
		case 0x02:
			if(motorCurrentVal_2 > 600)
			{
				motorPreStatus_2 = motorRunningStatus_2;
				motorRunningStatus_2 = 0;
				motorStatus = 0x03;
				timer_restart(&timer_02);
			}
			break;
			
		case 0x03:
			if(timer_expired(&timer_02))
			{
				if(motorPreStatus_2 == 1)
				{
					motorRunningStatus_2 = 2;
				}else{
					motorRunningStatus_2 = 1;
				}
				motorStatus = 0x00;
			}	
			break;
	}
	
	if(motorRunningStatus_2 == 0)
	{
		DoorStop(2);
	}else if(motorRunningStatus_2 == 1){
		DoorClose(2);
	}else{
		DoorOpen(2);
	}
}

/******************** (C) COPYRIGHT 2017 ping *****END OF FILE*******************/
