#include "ServoMotor.h"

#define SERVO_MOTO_FREQ 50

void ServoMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin)
{
	Timer_pwmo_conf(timer, channel, SERVO_MOTO_FREQ, 0);
	GPIO_conf(gpio, pin, LL_GPIO_MODE_ALTERNATE, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP);
}

void ServoMotor_start(TIM_TypeDef * timer)
{
	Timer_start(timer);
}

float convertAngleToDutyCycle(int angle) {
	return ((float) angle) / 90.0 / 20 + 0.05;
}

void ServoMotor_setAngle(TIM_TypeDef * timer, int channel, int angle)
{
	Timer_pwmo_setDutyCycle(timer, channel, convertAngleToDutyCycle(angle));
}

int ServoMotor_getAngle(TIM_TypeDef * timer, int channel)
{
	const float dutyCycle = Timer_pwmo_getDutyCycle(timer, channel);
	return 359 * dutyCycle;
}