/*****
 * Copyright (c) 2024 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/stdlib.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Teleplot.h"
#include "Temps.h"
#include <stdio.h>
#include <math.h>

void setup(void);
void loop(void);

int valeur = 1;
uint32_t m_temps_ms;

void main(void)
{
    setup();
	while(1){
		loop();
	}
}

void setup(void){
	stdio_init_all();
	Teleplot_init();
	Temps_init();
	Moteur_init();
	QEI_init();
}

void loop(void){
	static float cA_distance = 0, cB_distance = 0;
	float t = (float) Temps_get_temps_ms() / 1000. ;
	Moteur_set_commande(MOTEUR_A, sin(t * 3.14) * MOTEUR_COMMANDE_MAX) ;
	Moteur_set_commande(MOTEUR_B, sin(t * 3.14) * MOTEUR_COMMANDE_MAX) ;
	if(m_temps_ms != Temps_get_temps_ms()){
		m_temps_ms = Temps_get_temps_ms();

		if(m_temps_ms % 1 == 0){ // Fréquence d'actualisation des codeurs (1 ms)
			QEI_update();
			cA_distance += QEI_get_mm(QEI_A_NAME);
			cB_distance += QEI_get_mm(QEI_B_NAME);
		}

		if(m_temps_ms % 20 == 0){ // Fréquence d'affichage (20 ms)
			Teleplot_fige_temps();
			Teleplot_add_variable_float_2decimal("codeur_A_vitesse", QEI_get_mm(QEI_A_NAME));
			Teleplot_add_variable_float_2decimal("codeur_B_vitesse", QEI_get_mm(QEI_B_NAME));
			Teleplot_add_variable_float_2decimal("codeur_A_distance", cA_distance);
			Teleplot_add_variable_float_2decimal("codeur_B_distance", cA_distance);
			Teleplot_envoie_tampon();
		}
	}

}