40 lines
1.2 KiB
C
40 lines
1.2 KiB
C
#include "Orientation.h"
|
|
#include "math.h"
|
|
|
|
const float THRESHOLD = 0.05;
|
|
|
|
const float MIN_DUTY_CYCLE = 0.05;
|
|
const float ZERO_DUTY_CYCLE = 0.075;
|
|
const float MAX_DUTY_CYCLE = 0.1;
|
|
|
|
// Recepteur
|
|
TIM_TypeDef * RECEIVER_TIMER = TIM4;
|
|
const int RECEIVER_CHANNEL = LL_TIM_CHANNEL_CH1;
|
|
|
|
// Moteur
|
|
TIM_TypeDef * DCMOTOR_TIMER = TIM2;
|
|
const int DCMOTOR_CHANNEL = LL_TIM_CHANNEL_CH2;
|
|
GPIO_TypeDef * DCMOTOR_GPIO = GPIOA;
|
|
const int DCMOTOR_PIN_PWM = LL_GPIO_PIN_2;
|
|
const int DCMOTOR_PIN_DIRECTION = LL_GPIO_PIN_2;
|
|
|
|
|
|
void Orientation_conf()
|
|
{
|
|
DCMotor_conf(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN_PWM, DCMOTOR_GPIO, DCMOTOR_PIN_DIRECTION);
|
|
RFReceiver_conf(RECEIVER_TIMER, LL_TIM_CHANNEL_CH1);
|
|
}
|
|
|
|
void Orientation_background()
|
|
{
|
|
const float duty_cycle = RFReceiver_getData(RECEIVER_TIMER);
|
|
|
|
// Calcul de la vitesse du moteur (entre 0 et 1)
|
|
const float speed = (duty_cycle - ZERO_DUTY_CYCLE) / (MAX_DUTY_CYCLE - ZERO_DUTY_CYCLE);
|
|
|
|
// On ne demarre le moteur que si la vitesse depasse un certain seuil
|
|
if (fabs(speed) > THRESHOLD)
|
|
DCMotor_setSpeed(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN, speed);
|
|
else
|
|
DCMotor_setSpeed(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN, 0);
|
|
}
|