// motor_driver.c
#include "motor_driver.h"
#include "delay.h"
// 全局变量
static uint8_t L_pwm_duty = 0;      // PWM占空比 0-100
static uint8_t M_pwm_duty = 0;      // PWM占空比 0-100
static uint8_t R_pwm_duty = 0;      // PWM占空比 0-100
static MotorState L_motor_dir = MOTOR_STOP;
static MotorState M_motor_dir = MOTOR_STOP;
static MotorState R_motor_dir = MOTOR_STOP;
// GPIO初始化
void DRV8832_GPIO_Init(void) {
    GPIO_InitTypeDef GPIO_InitStructure;
    
	  //左电机 引脚初始化
    // 使能GPIOE时钟
    RCC_AHB1PeriphClockCmd(L_MOTOR_IN1_GPIO_CLK, ENABLE);
    
    // 配置IN1 (PE9) 和 IN2 (PE13) 为输出
    GPIO_InitStructure.GPIO_Pin = L_MOTOR_IN1_GPIO_PIN | L_MOTOR_IN2_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;  // 推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(L_MOTOR_IN1_GPIO_PORT, &GPIO_InitStructure);
    
    // 配置FAULTN (PE4) 为输入
    GPIO_InitStructure.GPIO_Pin = L_MOTOR_FAULT_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;  // 上拉
    GPIO_Init(L_MOTOR_FAULT_GPIO_PORT, &GPIO_InitStructure);
    
    // 初始状态：停止电机
    GPIO_ResetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
    GPIO_ResetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
		
		//===========================================================================
		//中电机 引脚初始化
		// 使能GPIOE时钟
    RCC_AHB1PeriphClockCmd(M_MOTOR_IN1_GPIO_CLK, ENABLE);
		// 配置IN1 (PB3)
    GPIO_InitStructure.GPIO_Pin = M_MOTOR_IN1_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;  // 推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(M_MOTOR_IN1_GPIO_PORT, &GPIO_InitStructure);
		
    // 配置 IN2 (PE14) 为输出
    GPIO_InitStructure.GPIO_Pin = M_MOTOR_IN2_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;  // 推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(M_MOTOR_IN2_GPIO_PORT, &GPIO_InitStructure);
    
    // 配置FAULTN (PE5) 为输入
    GPIO_InitStructure.GPIO_Pin = M_MOTOR_FAULT_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;  // 上拉
    GPIO_Init(M_MOTOR_FAULT_GPIO_PORT, &GPIO_InitStructure);
    
    // 初始状态：停止电机
    GPIO_ResetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
    GPIO_ResetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
		//===========================================================================
		//右电机 引脚初始化
		// 使能GPIOE时钟
    RCC_AHB1PeriphClockCmd(R_MOTOR_IN1_GPIO_CLK, ENABLE);
		// 配置IN1 (PD15)
    GPIO_InitStructure.GPIO_Pin = R_MOTOR_IN1_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;  // 推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(R_MOTOR_IN1_GPIO_PORT, &GPIO_InitStructure);
		
    // 配置 IN2 (PE15) 为输出
    GPIO_InitStructure.GPIO_Pin = R_MOTOR_IN2_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;  // 推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(R_MOTOR_IN2_GPIO_PORT, &GPIO_InitStructure);
    
    // 配置FAULTN (PE6) 为输入
    GPIO_InitStructure.GPIO_Pin = R_MOTOR_FAULT_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;  // 上拉
    GPIO_Init(R_MOTOR_FAULT_GPIO_PORT, &GPIO_InitStructure);
    
    // 初始状态：停止电机
    GPIO_ResetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
    GPIO_ResetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
		
		
		
}

// 初始化DRV8832
void DRV8832_Init(void) {
    DRV8832_GPIO_Init();
}




// 使用TIM2实现软件PWM
#define PWM_FREQUENCY    20000   // 20kHz PWM频率
#define PWM_RESOLUTION   100     // 100级调速分辨率

static uint8_t L_pwm_counter = 0;  // PWM计数器
static uint8_t M_pwm_counter = 0;  // PWM计数器
static uint8_t R_pwm_counter = 0;  // PWM计数器
// 定时器初始化
void PWM_Timer_Init(void) {
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    
    // 使能TIM2时钟
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
    
    // 定时器基础配置
    TIM_TimeBaseStructure.TIM_Period = (SystemCoreClock / (2 * PWM_FREQUENCY)) - 1;  // 168MHz/2/20kHz
    TIM_TimeBaseStructure.TIM_Prescaler = 0;
    TIM_TimeBaseStructure.TIM_ClockDivision = 0;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
    
    // 使能定时器更新中断
    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
    
    // 配置NVIC
    NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
    
    // 启动定时器
    TIM_Cmd(TIM2, ENABLE);
}

// 定时器中断处理函数
void TIM2_IRQHandler(void) 
{
    if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) 
	{
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
			
			
			
     {   //左电机运行
        L_pwm_counter++;
        if (L_pwm_counter >= PWM_RESOLUTION) 
			 {
            L_pwm_counter = 0;
        }
        
        // 根据占空比和方向控制电机
        if (L_motor_dir == MOTOR_FORWARD) {
            if (L_pwm_counter < L_pwm_duty) {
                // 正转：IN1=1, IN2=0
                GPIO_SetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
            } else {
                // 停止（滑行）
                GPIO_ResetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
            }
        } 
        else if (L_motor_dir == MOTOR_REVERSE) {
            if (L_pwm_counter < L_pwm_duty) {
                // 反转：IN1=0, IN2=1
                GPIO_ResetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
                GPIO_SetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
            } else {
                // 停止（滑行）
                GPIO_ResetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
            }
        }
        else {
            // 停止状态
            GPIO_ResetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
        }
		  }
		 //--------------------------------------------------------------------------------
			{   //中电机运行
        M_pwm_counter++;
        if (M_pwm_counter >= PWM_RESOLUTION) 
				{
            M_pwm_counter = 0;
        }
        
        // 根据占空比和方向控制电机
        if (M_motor_dir == MOTOR_FORWARD) {
            if (M_pwm_counter < M_pwm_duty) {
                // 正转：IN1=1, IN2=0
                GPIO_SetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
            } else {
                // 停止（滑行）
                GPIO_ResetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
            }
        } 
        else if (M_motor_dir == MOTOR_REVERSE) {
            if (M_pwm_counter < M_pwm_duty) {
                // 反转：IN1=0, IN2=1
                GPIO_ResetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
                GPIO_SetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
            } else {
                // 停止（滑行）
                GPIO_ResetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
            }
        }
        else {
            // 停止状态
            GPIO_ResetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
        }
		  }
			
		 //--------------------------------------------------------------------------------
      {   //右电机运行
        R_pwm_counter++;
        if (R_pwm_counter >= PWM_RESOLUTION) 
				{
            R_pwm_counter = 0;
        }
        
        // 根据占空比和方向控制电机
        if (R_motor_dir == MOTOR_FORWARD) 
				{
            if (R_pwm_counter < R_pwm_duty) {
                // 正转：IN1=1, IN2=0
                GPIO_SetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
            } else {
                // 停止（滑行）
                GPIO_ResetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
            }
        } 
        else if (R_motor_dir == MOTOR_REVERSE) 
					{
            if (R_pwm_counter < R_pwm_duty) {
                // 反转：IN1=0, IN2=1
                GPIO_ResetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
                GPIO_SetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
            } else {
                // 停止（滑行）
                GPIO_ResetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
                GPIO_ResetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
            }
        }
        else {
            // 停止状态
            GPIO_ResetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
        }
		  }			
		//---------------------------------------------------------------------------------	
			
    }
}

//=========电机速度和方向设置================================================
// 设置左电机速度和方向
void L_Motor_SetSpeed(uint8_t speed, MotorState direction) 
{
    // 限制速度范围
    if (speed > 100) speed = 100;
    
    // 如果速度为0或100%，直接使用基础控制
    if (speed == 0) {
        L_motor_dir = MOTOR_STOP;
        L_pwm_duty = 0;
        L_Motor_Control(MOTOR_STOP);
    } 
    else if (speed == 100) {
        L_motor_dir = direction;
        L_pwm_duty = 100;
        L_Motor_Control(direction);
    }
    else {
        // 设置PWM参数
        L_motor_dir = direction;
        L_pwm_duty = speed;  // 直接使用百分比
    }
}

// 设置中电机速度和方向
void M_Motor_SetSpeed(uint8_t speed, MotorState direction) 
{
    // 限制速度范围
    if (speed > 100) speed = 100;
    
    // 如果速度为0或100%，直接使用基础控制
    if (speed == 0) {
        M_motor_dir = MOTOR_STOP;
        M_pwm_duty = 0;
        M_Motor_Control(MOTOR_STOP);
    } 
    else if (speed == 100) {
        M_motor_dir = direction;
        M_pwm_duty = 100;
        M_Motor_Control(direction);
    }
    else {
        // 设置PWM参数
        M_motor_dir = direction;
        M_pwm_duty = speed;  // 直接使用百分比
    }
}

// 设置右电机速度和方向
void R_Motor_SetSpeed(uint8_t speed, MotorState direction) 
{
    // 限制速度范围
    if (speed > 100) speed = 100;
    
    // 如果速度为0或100%，直接使用基础控制
    if (speed == 0) {
        R_motor_dir = MOTOR_STOP;
        R_pwm_duty = 0;
        R_Motor_Control(MOTOR_STOP);
    } 
    else if (speed == 100) {
        R_motor_dir = direction;
        R_pwm_duty = 100;
        R_Motor_Control(direction);
    }
    else {
        // 设置PWM参数
        R_motor_dir = direction;
        R_pwm_duty = speed;  // 直接使用百分比
    }
}
//=====================电机直接控制================================================================
// 左电机基础控制函数
void L_Motor_Control(MotorState state) 
	{
    switch(state) {
        case MOTOR_STOP:    // IN1=0, IN2=0
            GPIO_ResetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_FORWARD: // IN1=1, IN2=0
            GPIO_SetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_REVERSE: // IN1=0, IN2=1
            GPIO_ResetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
            GPIO_SetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_BRAKE:   // IN1=1, IN2=1
            GPIO_SetBits(L_MOTOR_IN1_GPIO_PORT, L_MOTOR_IN1_GPIO_PIN);
            GPIO_SetBits(L_MOTOR_IN2_GPIO_PORT, L_MOTOR_IN2_GPIO_PIN);
            break;
    }
}
	
// 中电机基础控制函数
void M_Motor_Control(MotorState state) 
	{
    switch(state) {
        case MOTOR_STOP:    // IN1=0, IN2=0
            GPIO_ResetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_FORWARD: // IN1=1, IN2=0
            GPIO_SetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_REVERSE: // IN1=0, IN2=1
            GPIO_ResetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
            GPIO_SetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_BRAKE:   // IN1=1, IN2=1
            GPIO_SetBits(M_MOTOR_IN1_GPIO_PORT, M_MOTOR_IN1_GPIO_PIN);
            GPIO_SetBits(M_MOTOR_IN2_GPIO_PORT, M_MOTOR_IN2_GPIO_PIN);
            break;
    }
}
	
// 右电机基础控制函数
void R_Motor_Control(MotorState state) 
	{
    switch(state) {
        case MOTOR_STOP:    // IN1=0, IN2=0
            GPIO_ResetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_FORWARD: // IN1=1, IN2=0
            GPIO_SetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
            GPIO_ResetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_REVERSE: // IN1=0, IN2=1
            GPIO_ResetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
            GPIO_SetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
            break;
            
        case MOTOR_BRAKE:   // IN1=1, IN2=1
            GPIO_SetBits(R_MOTOR_IN1_GPIO_PORT, R_MOTOR_IN1_GPIO_PIN);
            GPIO_SetBits(R_MOTOR_IN2_GPIO_PORT, R_MOTOR_IN2_GPIO_PIN);
            break;
    }
}
//--------------------------------------------------------------------------------------------
// 获取故障状态
uint8_t L_Motor_GetFaultStatus(void) {
    // FAULTN引脚为低电平时表示故障
    if (GPIO_ReadInputDataBit(L_MOTOR_FAULT_GPIO_PORT, L_MOTOR_FAULT_GPIO_PIN) == Bit_RESET) {
        return 1;  // 故障
    }
    return 0;      // 正常
}

// 安全的方向切换（避免电流冲击）
void L_Motor_SafeDirectionChange(MotorState new_direction) {
    // 先刹车
    L_Motor_Control(MOTOR_BRAKE);
    delay_ms(5);  // 5ms延迟
    
    // 再切换到新方向
    L_Motor_Control(new_direction);
}


// 获取故障状态
uint8_t M_Motor_GetFaultStatus(void) {
    // FAULTN引脚为低电平时表示故障
    if (GPIO_ReadInputDataBit(M_MOTOR_FAULT_GPIO_PORT, M_MOTOR_FAULT_GPIO_PIN) == Bit_RESET) {
        return 1;  // 故障
    }
    return 0;      // 正常
}

// 安全的方向切换（避免电流冲击）
void M_Motor_SafeDirectionChange(MotorState new_direction) {
    // 先刹车
    M_Motor_Control(MOTOR_BRAKE);
    delay_ms(5);  // 5ms延迟
    
    // 再切换到新方向
    M_Motor_Control(new_direction);
}

// 获取故障状态
uint8_t R_Motor_GetFaultStatus(void) {
    // FAULTN引脚为低电平时表示故障
    if (GPIO_ReadInputDataBit(R_MOTOR_FAULT_GPIO_PORT, R_MOTOR_FAULT_GPIO_PIN) == Bit_RESET) {
        return 1;  // 故障
    }
    return 0;      // 正常
}

// 安全的方向切换（避免电流冲击）
void R_Motor_SafeDirectionChange(MotorState new_direction) {
    // 先刹车
    R_Motor_Control(MOTOR_BRAKE);
    delay_ms(5);  // 5ms延迟
    
    // 再切换到新方向
    R_Motor_Control(new_direction);
}