/*****
 * Copyright (c) 2023 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/stdlib.h"
#include "hardware/pwm.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 Moteur_init(void);
void M1_forward(int speed);
void M1_backward(int speed);
void M1_speed(int speed);
void M2_forward(int speed);
void M2_backward(int speed);
void M2_speed(int speed);

void main(void)
{
	int ledpower = 500;

    stdio_init_all();
	QEI_init();
	Moteur_init();

	gpio_init(LED1PIN);
	gpio_set_dir(LED1PIN, GPIO_OUT );		
	gpio_put(LED1PIN, 1);

	while(1){
		QEI_update();

		printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );	
		
		M1_speed(1000);
		M2_speed(1000);
		
		sleep_ms(100);
	}
}

void Moteur_init(void){

	gpio_init(M1_SENS1);
	gpio_init(M1_SENS2);
	gpio_init(M2_SENS1);
	gpio_init(M2_SENS2);
	gpio_set_dir(M1_SENS1, GPIO_OUT);
	gpio_set_dir(M1_SENS2, GPIO_OUT);
	gpio_set_dir(M2_SENS1, GPIO_OUT);
	gpio_set_dir(M2_SENS2, GPIO_OUT);
	gpio_put(M1_SENS1, 0);
	gpio_put(M1_SENS2, 0);
	gpio_put(M2_SENS1, 0);
	gpio_put(M2_SENS2, 0);

	gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM);
	gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM);
	pwm_set_wrap(5, 1000);
	pwm_set_wrap(4, 1000);
	pwm_set_chan_level(5, PWM_CHAN_B, 0);
	pwm_set_chan_level(4, PWM_CHAN_B, 0);
	pwm_set_enabled(5, true);
	pwm_set_enabled(4, true);

}

void M1_forward(int speed)
{
	pwm_set_chan_level(5, PWM_CHAN_B, speed);
	gpio_put(M1_SENS1, 1);
	gpio_put(M1_SENS2, 0);
}

// Set motor 1 speed backward
void M1_backward(int speed)
{
	pwm_set_chan_level(5, PWM_CHAN_B, speed);
	gpio_put(M1_SENS1, 0);
	gpio_put(M1_SENS2, 1);
}

// Set motor 1 speed and direction (negative value : backward / positive value : forward)
void M1_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		M1_backward(speed);
	}
	else
	{
		M1_forward(speed);
	}
}

// Set motor 2 speed forward
void M2_forward(int speed)
{
	pwm_set_chan_level(4, PWM_CHAN_B, speed);
	gpio_put(M2_SENS1, 0);
	gpio_put(M2_SENS2, 1);
}

// Set motor 2 speed backward
void M2_backward(int speed)
{
	pwm_set_chan_level(4, PWM_CHAN_B, speed);
	gpio_put(M2_SENS1, 1);
	gpio_put(M2_SENS2, 0);
}

// Set motor 2 speed and direction (negative value : backward / positive value : forward)
void M2_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		M2_backward(speed);
	}
	else
	{
		M2_forward(speed);
	}
}