/*****
 * Copyright (c) 2023 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include <stdio.h>
#include "hardware/adc.h"
#define LED_VERTE 25
#define PIN_VITESSE_M1 2
#define PIN_SENS_A_M1 0                                                                                                                     
#define PIN_SENS_B_M1 1

uint16_t result1 = 0;
uint16_t result0 = 0;
int M1_INITIALISE()
{
	gpio_init(PIN_VITESSE_M1);
	gpio_init(PIN_SENS_A_M1);
	gpio_init(PIN_SENS_B_M1);

    gpio_set_dir(PIN_VITESSE_M1, GPIO_OUT);
	gpio_set_dir(PIN_SENS_A_M1, GPIO_OUT);
	gpio_set_dir(PIN_SENS_B_M1, GPIO_OUT);

	gpio_set_function(PIN_VITESSE_M1, GPIO_FUNC_PWM);
	pwm_set_wrap(PIN_VITESSE_M1, 100);
	pwm_set_chan_level(PIN_VITESSE_M1, PWM_CHAN_B, 50);
    
}

int M1_AVANCE()
{

	gpio_put(PIN_VITESSE_M1, 1);
	gpio_put(PIN_SENS_A_M1, 1);
	gpio_put(PIN_SENS_B_M1, 0);



}

int M1_RECULE()
{

	gpio_put(PIN_VITESSE_M1, 1);
	gpio_put(PIN_SENS_A_M1, 0);
	gpio_put(PIN_SENS_B_M1, 1);



}
int Adc_Init()
{
	adc_init();
	adc_gpio_init(26);
	adc_gpio_init(27);
}
int AdcRead0()
{
	adc_select_input(0);
	uint16_t resultadc = adc_read();
	return resultadc;
}





int AdcRead1()
{
	adc_select_input(1);
	uint16_t resultadc= adc_read();
	return resultadc;
}






void main()
{
	stdio_init_all();

	// CLignottement LED
	gpio_set_function(LED_VERTE, GPIO_FUNC_PWM);
	pwm_set_wrap(4, 100);
	pwm_set_chan_level(4, PWM_CHAN_B, 25);
	pwm_set_enabled(4, true);

	// ADC
	adc_init();
	adc_gpio_init(26);
	adc_gpio_init(27);
	
	while (1) 
	{
		// Voie Y
		result0 = AdcRead0();
		printf(">result:%d\n",result0-2048);

		// Voie X
		result1 = AdcRead1();
	 	printf(">result1:%d\n",result1-2048);
		printf("test\n",result0);

		sleep_ms(100);

	}
	M1_INITIALISE();

	while(1)
	{
		M1_AVANCE();
		sleep_ms(1000);
		M1_RECULE();
		sleep_ms(1000);
	
	}

}