From 96a831ae9ef0cc823b28326326856524b008afe4 Mon Sep 17 00:00:00 2001 From: Arnaud Vergnet Date: Fri, 6 Nov 2020 09:42:38 +0100 Subject: [PATCH] add servo motor functions --- MyDrivers/Timer.c | 32 +++++++++++++++++++++++++------- MyDrivers/Timer.h | 24 +++++++++++++++++++++--- Services/ServoMotor.c | 24 ++++++++++++++++++++++++ Services/ServoMotor.h | 38 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 108 insertions(+), 10 deletions(-) diff --git a/MyDrivers/Timer.c b/MyDrivers/Timer.c index ee67375..61eb197 100644 --- a/MyDrivers/Timer.c +++ b/MyDrivers/Timer.c @@ -171,15 +171,15 @@ void Timer_conf(TIM_TypeDef * timer, int arr, int psc) * PWM OUTPUT ***************************************************************************/ -int getArrFromFreq(float freq_khz) +int getArrFromFreq(float freq) { - return (72000 / freq_khz) - 1; + return (72000000 / freq) - 1; } -void Timer_pwmo_conf(TIM_TypeDef * timer, int channel, float freq_khz, float dutyCycle) +void Timer_pwmo_conf(TIM_TypeDef * timer, int channel, float freq, float dutyCycle) { - const int arr = getArrFromFreq(freq_khz); - Timer_conf(timer, arr, 0); + const int arr = getArrFromFreq(freq); + Timer_conf(timer, arr, 1000); LL_TIM_OC_InitTypeDef init_struct; LL_TIM_OC_StructInit(&init_struct); @@ -190,15 +190,33 @@ void Timer_pwmo_conf(TIM_TypeDef * timer, int channel, float freq_khz, float dut LL_TIM_OC_Init(timer, channel, &init_struct); } -void Timer_pwmo_setDutyCycle(TIM_TypeDef * timer, int channel, float freq_khz, float dutyCycle) +void Timer_pwmo_setFreq(TIM_TypeDef * timer, float freq) { - int compare = dutyCycle * getArrFromFreq(freq_khz); + const int arr = getArrFromFreq(freq); + LL_TIM_SetAutoReload(timer, arr); +} + +void Timer_pwmo_setDutyCycle(TIM_TypeDef * timer, int channel, float dutyCycle) +{ + const int arr = LL_TIM_GetAutoReload(timer); + int compare = dutyCycle * arr; if (channel == LL_TIM_CHANNEL_CH1) LL_TIM_OC_SetCompareCH1(timer, compare); else if (channel == LL_TIM_CHANNEL_CH2) LL_TIM_OC_SetCompareCH2(timer, compare); else if (channel == LL_TIM_CHANNEL_CH3) LL_TIM_OC_SetCompareCH3(timer, compare); else LL_TIM_OC_SetCompareCH4(timer, compare); } +float Timer_pwmo_getDutyCycle(TIM_TypeDef * timer, int channel) +{ + int compare = 0; + const int arr = LL_TIM_GetAutoReload(timer); + if (channel == LL_TIM_CHANNEL_CH1) compare = LL_TIM_OC_GetCompareCH1(timer); + else if (channel == LL_TIM_CHANNEL_CH2) compare = LL_TIM_OC_GetCompareCH2(timer); + else if (channel == LL_TIM_CHANNEL_CH3) compare = LL_TIM_OC_GetCompareCH3(timer); + else compare = LL_TIM_OC_GetCompareCH4(timer); + return ((float) compare) / ((float) arr); +} + /**************************************************************************** * ENCODER ***************************************************************************/ diff --git a/MyDrivers/Timer.h b/MyDrivers/Timer.h index b92cfd7..a28ac9c 100644 --- a/MyDrivers/Timer.h +++ b/MyDrivers/Timer.h @@ -65,11 +65,20 @@ int PWMi_getDutyCycle(TIM_TypeDef * timer, int channel); * @note * @param TIM_TypeDef Timer : indique le timer à utiliser : TIM1, TIM2, TIM3 ou TIM4 * int channel : Le channel utilisé par la PWM - * float freq_khz : Fréquence en KHz + * float freq : Fréquence en Hz (entre 2Hz et pas trop grand svp) * float dutyCycle : Valeur entre 0 et 1 * @retval None */ -void Timer_pwmo_conf(TIM_TypeDef * timer, int channel, float freq_khz, float dutyCycle); +void Timer_pwmo_conf(TIM_TypeDef * timer, int channel, float freq, float dutyCycle); + +/** + * @brief Modifie la fréquence des PWMs associées au timer donné + * @note + * @param TIM_TypeDef Timer : indique le timer à utiliser : TIM1, TIM2, TIM3 ou TIM4 + * float freq : Fréquence en Hz (entre 2Hz et pas trop grand svp) + * @retval None + */ +void Timer_pwmo_setFreq(TIM_TypeDef * timer, float freq); /** * @brief Modifie le duty cycle de la PWM @@ -79,7 +88,16 @@ void Timer_pwmo_conf(TIM_TypeDef * timer, int channel, float freq_khz, float dut * float dutyCycle : Valeur entre 0 et 1 * @retval None */ -void Timer_pwmo_setDutyCycle(TIM_TypeDef * timer, int channel, float freq_khz, float dutyCycle); +void Timer_pwmo_setDutyCycle(TIM_TypeDef * timer, int channel, float dutyCycle); + +/** + * @brief Récupère le duty cycle de la PWM + * @note + * @param TIM_TypeDef Timer : indique le timer à utiliser : TIM1, TIM2, TIM3 ou TIM4 + * int channel : Le channel utilisé par la PWM + * @retval float dutyCycle + */ +float Timer_pwmo_getDutyCycle(TIM_TypeDef * timer, int channel); /**************************************************************************** * ENCODER diff --git a/Services/ServoMotor.c b/Services/ServoMotor.c index c3523b2..d006b26 100644 --- a/Services/ServoMotor.c +++ b/Services/ServoMotor.c @@ -1 +1,25 @@ #include "ServoMotor.h" +#include "Timer.h" + +#define SERVO_MOTO_FREQ 50 + +void ServoMotor_conf(TIM_TypeDef * timer, int channel) +{ + Timer_pwmo_conf(timer, channel, SERVO_MOTO_FREQ, 0); +} + +void ServoMotor_start(TIM_TypeDef * timer) +{ + Timer_start(timer); +} + +void ServoMotor_setAngle(TIM_TypeDef * timer, int channel, int angle) +{ + Timer_pwmo_setDutyCycle(timer, channel, ((float) angle) / 359.0); +} + +int ServoMotor_getAngle(TIM_TypeDef * timer, int channel) +{ + const float dutyCycle = Timer_pwmo_getDutyCycle(timer, channel); + return 359 * dutyCycle; +} diff --git a/Services/ServoMotor.h b/Services/ServoMotor.h index c28a0fc..65561b8 100644 --- a/Services/ServoMotor.h +++ b/Services/ServoMotor.h @@ -1,4 +1,42 @@ #ifndef SERVO_MOTOR_H #define SERVO_MOTOR_H +#include "stm32f103xb.h" + +/** + * @brief Configure le servo moteur associé au timer donné + * @note + * @param TIM_TypeDef Timer : indique le timer à utiliser : TIM1, TIM2, TIM3 ou TIM4 + * int channel : Le channel utilisé par le servo moteur + * @retval None + */ +void ServoMotor_conf(TIM_TypeDef * timer, int channel); + +/** + * @brief Démarre les servo moteurs associés au timer donné + * @note + * @param TIM_TypeDef Timer : indique le timer à utiliser : TIM1, TIM2, TIM3 ou TIM4 + * @retval None + */ +void ServoMotor_start(TIM_TypeDef * timer); + +/** + * @brief Modifie l'angle du servo moteur + * @note + * @param TIM_TypeDef Timer : indique le timer à utiliser : TIM1, TIM2, TIM3 ou TIM4 + * int channel : Le channel utilisé par le servo moteur + * float angle : .En degrès (Valeur entre 0 et 359) + * @retval None + */ +void ServoMotor_setAngle(TIM_TypeDef * timer, int channel, int angle); + +/** + * @brief Récupère l'angle du servo moteur + * @note + * @param TIM_TypeDef Timer : indique le timer à utiliser : TIM1, TIM2, TIM3 ou TIM4 + * int channel : Le channel utilisé par le servo moteur + * @retval int angle + */ +int ServoMotor_getAngle(TIM_TypeDef * timer, int channel); + #endif