35 lines
941 B
C
35 lines
941 B
C
#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) {
|
|
// fonction lineaire [0 - 90] -> [0.05 - 0.1]
|
|
return (float)angle / 1800.0 + 0.05;
|
|
}
|
|
|
|
int convertDutyCycleToAngle(float dutyCycle) {
|
|
// fonction lineaire [0.049 - 0.1] -> [0 - 90]
|
|
return (dutyCycle - 0.049) * 1800;
|
|
}
|
|
|
|
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 convertDutyCycleToAngle(dutyCycle);
|
|
}
|