#include "Motor.h"

extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim4;

/*******
PWMA --- TIM4_CHANNEL4

PMWB --- TIM3_CHANNEL4

STBY --- 5V 默认5V 所以无需初始化
*******/
void PWM_GPIO_TIM_Init(void)
{
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_4);
	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
}


void MotorA_Dir(uint8_t dir)
{
	if(0 == dir) // 前进
	{
		HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
	}
	else  // 反转
	{
		HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET);
	}
	
}


void MotorB_Dir(uint8_t dir)
{
	if(0 == dir) // 前进
	{
		HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET);
		HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_RESET);
	}
	else  // 反转
	{
		HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET);
		HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_SET);
	}
	
}

// 控制 电机A的速度
void MotorA_Speed(uint8_t speed)
{
	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,speed);
}

// 控制 电机B的速度
void MotorB_Speed(uint8_t speed)
{
	__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,speed);
}


// 示例 同时控制电机A/B
// 提示 对于 Dir 0  ---- 前进 go forward
//      对于 Dir 1  ---- 后退 go forward 
void DriveBOTH(int16_t speedA,int16_t speedB)
{
	// 电机 A
	if (speedA >= 0) MotorA_Dir(0);
	else
	{
		MotorA_Dir(1);
		speedA = -speedA;
	}
	MotorA_Speed(speedA);
	
	// 电机 B
	if (speedB >= 0) MotorA_Dir(0);
	else
	{
		MotorB_Dir(1);
		speedB = -speedB;
	}
	MotorB_Speed(speedB);
}