#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);
}