#include "servo.h"
#include "tim.h"
#include "lcd.h"

int16_t Servo_Speed = 1, Servo_goal = 0, Servo_position = 0;

void Servo_SetAngle(int goal)
{
    Servo_position = goal;
    __HAL_TIM_SET_COMPARE(&htim15, TIM_CHANNEL_2, ANGLETOCCR(goal));
}

// 往哪个方向转
int Servo_toward(void)
{
    if (Servo_goal > Servo_position)
    {
        return 1;
    }
    return -1;
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    static uint8_t times = 0, tt = 0;
    if (htim == &htim7)
    {
        if (++times == 1000)
        {
            times = 0;
        }
        if (Servo_position == Servo_goal)
        {
            return;
        }
        if (Servo_Speed == 1)
        {
            Servo_position = Servo_goal;
        }
        if (Servo_Speed == 2 && times % 5 == 0)
        {
            Servo_position += Servo_toward();
        }
        if (Servo_Speed == 3 && times % 50 == 0)
        {
            Servo_position += Servo_toward();
        }
        Servo_SetAngle(Servo_position);
    }
}