#include "moteur.h"

// Define pins servo
#define SERVO1_PIN 20
#define SERVO2_PIN 21

// S0 : 26 - PWM 5 - Chan A
// S1 : 22 - PWM 3 - Chan A
// S2 : 21 - PWM 2 - Chan B
// S3 : 20 - PWM 2 - Chan A
// S4 : 27 - PWM 5 - Chan B

void Servomoteur_init(){
    // Set wrap of pwm slice
	pwm_set_wrap(2, 25000);
    pwm_set_wrap(3, 25000);
    pwm_set_wrap(5, 25000);

	// Set clock div for this pwm slice
	pwm_set_clkdiv(2, 100);
    pwm_set_clkdiv(3, 100);
    pwm_set_clkdiv(5, 100);

	// Enable pwm slice
	pwm_set_enabled(2, true);
    pwm_set_enabled(3, true);
    pwm_set_enabled(5, true);

	// Set channel level
	pwm_set_chan_level(2, PWM_CHAN_A, 2800); // S3 - Ouvert : 1700 - Fermé : 2800
	pwm_set_chan_level(2, PWM_CHAN_B, 2650); // S2 - Ouvert : 1500 - Fermé : 2650
    pwm_set_chan_level(3, PWM_CHAN_A, 2300); // S1 - 2300 Pousse, 1500 Neutre
	pwm_set_chan_level(5, PWM_CHAN_A, 900); // S0 - Dépose 900 - 1750
    pwm_set_chan_level(5, PWM_CHAN_B, 1500); // S4 - Ouvert 2700 - Fermé : 1500 


    // Init pin for the servos
	gpio_init(SERVO1_PIN);
	gpio_init(SERVO2_PIN);
    gpio_init(22);
    gpio_init(26);
	gpio_init(27);
	gpio_set_function(SERVO1_PIN, GPIO_FUNC_PWM);
	gpio_set_function(SERVO2_PIN, GPIO_FUNC_PWM);
	gpio_set_function(22, GPIO_FUNC_PWM);
	gpio_set_function(26, GPIO_FUNC_PWM);
	gpio_set_function(27, GPIO_FUNC_PWM);

	// Edit 17/01/2024
	// Réglage servomoteurs en 25000 * 100
	// Pince
	// Pot : 1700
	// Plante : 1970
	// Ouvert : 1000
	// Ascenseur
	// Haut : 2550
	// Bas : 1550
	// Lâche plante : 2050
	// Lâche pot jardinière : 1950
}

void Servo_pince_tient(){
    pwm_set_chan_level(2, PWM_CHAN_A, 2800);
    pwm_set_chan_level(5, PWM_CHAN_B, 1350);
    pwm_set_chan_level(2, PWM_CHAN_B, 2650);
}

void Servo_pince_lache(){
    pwm_set_chan_level(2, PWM_CHAN_A, 1700);
    pwm_set_chan_level(5, PWM_CHAN_B, 2550);
    pwm_set_chan_level(2, PWM_CHAN_B, 1500);
}

void Servo_came_pousse(){
    pwm_set_chan_level(3, PWM_CHAN_A, 2300); // S1 - 2300 Pousse, 1500 Neutre
}

void Servo_came_neutre(){
    pwm_set_chan_level(3, PWM_CHAN_A, 1500); // S1 - 2300 Pousse, 1500 Neutre
}

void Servo_deplie_banderole(){
    pwm_set_chan_level(5, PWM_CHAN_A, 900); // S0 - Dépose 900 - neutre 1750
}

void Servo_plie_banderole(){
    pwm_set_chan_level(5, PWM_CHAN_A, 1750); // S0 - Dépose 900 - neutre 1750
}