#include "servo.h"
uint8_t channel;
float now_angle,servo_offset_angle_up = 0,servo_offset_angle_down = 0;

void servo_init(uint8_t servo_pin,uint8_t servo_channel)
{
    int servo_freq = 50;      // 频率(20ms周期)
    int servo_resolution = 12; // 分辨率(8 ~ 16)
    channel = servo_channel;
    ledcSetup(servo_channel, servo_freq, servo_resolution);
    ledcAttachPin(servo_pin, servo_channel);   
    servo_set_angle(0);
}

int  calculatePWM(float degree)
{
  if (degree < 0)
    degree = 0;
  if (degree > 360)
    degree = 360;
   return (degree / 360.0) * 410 + 103.0;
  
}

void servo_off()
{
  ledcWrite(channel,0);
}

void servo_set_angle(float angle)
{
  now_angle = angle;
  ledcWrite(channel, calculatePWM(angle));
}
//获取舵机的角度
float servo_get_angle()
{
  return now_angle;
}

void servo_up()
{
  float a = 180 + servo_offset_angle_up;
  if(a < 0 || a > 270) a = 180;
  // servo_set_angle(200);
  //  servo_set_angle(213);
  servo_set_angle(a);
}

void servo_down()
{
  float a = 0 + servo_offset_angle_down;
  if(a < 0 || a > 270) a = 0;
  // servo_set_angle(12);
  //  servo_set_angle(28);
  servo_set_angle(a);
}
/// @brief 
/// @param dir   0 down  1 up
/// @param angle
void servo_offset(float angle,uint8_t dir)
{

  switch (dir)
  {
    case 0:
      servo_offset_angle_down = angle;
      servo_down();
      break;
    case 1:
      servo_offset_angle_up = angle;
      servo_up();
      break;  
    default:
      break;
  }
  
}


