#include "Motor.h"
#include "zf_driver_gpio.h"
#include "zf_driver_pwm.h"
#include "PID_Controller.h"
#include "MotorEncoder.h"

void motor_init(void)
{
    gpio_init(DIR_L, GPO, GPIO_HIGH, GPO_PUSH_PULL);                           
    pwm_init(PWM_L, 17000, 0);                                                

    gpio_init(DIR_R, GPO, GPIO_HIGH, GPO_PUSH_PULL);                            
    pwm_init(PWM_R, 17000, 0);
}
// 右电机速度设置
void motor_set_pwm_right(int32 pwm)
{
    uint8_t dir = (pwm >= 0) ? 1 : 0;//根据电机接线方式调整方向
    uint32 abs_pwm = abs(pwm);
    if(pwm > MOTOR_MAX) pwm = MOTOR_MAX;
	gpio_set_level(DIR_R, dir);//设置方向
    pwm_set_duty(PWM_R, abs_pwm);//设置PWM占空比
}

// 左电机速度设置 - 需要根据实际硬件修改
void motor_set_pwm_left(int32 pwm)
{
    uint8_t dir = (pwm >= 0) ? 0 : 1;//根据电机接线方式调整方向
    uint32 abs_pwm = abs(pwm);
    if(pwm > MOTOR_MAX) pwm = MOTOR_MAX;
    gpio_set_level(DIR_L, dir);//设置方向
    pwm_set_duty(PWM_L, abs_pwm);//设置PWM占空比
}