#include "Orientation.h"
#include "math.h"

#define THRESHOLD 30

TIM_TypeDef * RECEIVER_TIMER = TIM4;
const int RECEIVER_CHANNEL = LL_TIM_CHANNEL_CH1;

TIM_TypeDef * DCMOTOR_TIMER = TIM2;
const int DCMOTOR_CHANNEL = LL_TIM_CHANNEL_CH2;
GPIO_TypeDef * DCMOTOR_GPIO = GPIOA;
const int DCMOTOR_PIN = LL_GPIO_PIN_2;

void Orientation_conf() 
{
	DCMotor_conf(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN);
	RFReceiver_conf(RECEIVER_TIMER, LL_TIM_CHANNEL_CH1);
}

void Orientation_background()
{
	const float speed = RFReceiver_getData(RECEIVER_TIMER);
	
	//Si la vitesse (en valeur absolue) ne dépasse pas un certain seuil, on ne démarre pas le moteur
	if (THRESHOLD < fabs(speed)) {
		DCMotor_setSpeed(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN, 0);
	}
	else { 
		DCMotor_setSpeed(DCMOTOR_TIMER, DCMOTOR_CHANNEL, DCMOTOR_GPIO, DCMOTOR_PIN, speed);
	}
}