#include "zf_common_headfile.h"
#include "cw_servo.h"

void cw_servo_init(void)
{
    pwm_init(SERVO_L_PWM_CHANNEL, 50, 1000);
    pwm_init(SERVO_R_PWM_CHANNEL, 50, 1000);
}

void cw_servo_set_angle(float servo_l_angle, float servo_r_angle)
{
    uint32_t servo_l_duty_s = (uint32_t)(servo_l_angle * SERVO_L_DUTY_PER_ANGLE);
    uint32_t servo_r_duty_s = (uint32_t)(servo_r_angle * SERVO_R_DUTY_PER_ANGLE);

    pwm_set_duty(SERVO_L_PWM_CHANNEL, servo_l_duty_s);
    pwm_set_duty(SERVO_R_PWM_CHANNEL, servo_r_duty_s);
}