#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) MotorB_Dir(0);
//	else
//	{
//		MotorB_Dir(1);
//		speedB = -speedB;
//	}
//	MotorB_Speed(speedB);
//}


// Motor.c   早上进行处理
void DriveBOTH(int16_t speedA, int16_t speedB)
{
    // 处理电机A
    if(speedA == 0) {
        // 制动：两个方向都设为低电平
        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
    } 
    else if(speedA > 0) {
        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
        speedA = abs(speedA); // 取绝对值
    }
    else {
        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
        speedA = abs(speedA); // 取绝对值
    }
    
	
	if(speedB == 0) {
        // 制动：两个方向都设为低电平
        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
    } 
    else if(speedB > 0) {
        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
        speedB = abs(speedB); // 取绝对值
    }
    else {
        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
        speedB = abs(speedB); // 取绝对值
    }
    // 同样处理电机B...
    
	
    // 设置PWM
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, speedA);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, speedB);
}