/*****
 * Copyright (c) 2023 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/pwm.h"
#include "Asser_Moteurs.h"
#include "Moteurs.h"
#include "Temps.h"
#include <stdio.h>
#include "QEI.h"

#define LED1PIN 20
#define M1_SENS1 7
#define M1_SENS2 13
#define M1_VITESSE 27 //5B
#define M2_SENS1 10
#define M2_SENS2 5
#define M2_VITESSE 9 //4B

void affichage(void);

void main(void)
{
	int ledpower = 500;


    stdio_init_all();
	AsserMoteur_Init();
	Temps_init();
	uint32_t temps_ms = Temps_get_temps_ms();
	uint32_t step_ms=1;

	gpio_init(LED1PIN);
	gpio_set_dir(LED1PIN, GPIO_OUT );		
	gpio_put(LED1PIN, 1);
	AsserMoteur_setConsigne_mm_s(MOTEUR_A, 6000);
	AsserMoteur_setConsigne_mm_s(MOTEUR_B, 6000);

	multicore_launch_core1(affichage);
	Moteur_SetVitesse(MOTEUR_A, 16000);
	Moteur_SetVitesse(MOTEUR_B, 16000);

	while(1){
		
		if(temps_ms != Temps_get_temps_ms()){
			temps_ms = Temps_get_temps_ms();
			QEI_update();
			AsserMoteur_Gestion(step_ms);
			if(temps_ms % 100 == 0){
				//printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );	
			}
		}

		
		
	}
}

void affichage(void){
	while(1){
		
		printf(">c1:%d\n>c1_mm:%f\n", QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME) );
		//printf(">c2:%d\n>c2_mm:%f\n", QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME) );
		printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1), AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1) );
		printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );
		sleep_ms(100);
	}
}