Robot_Riombotique/moteur.c

#include "moteur.h"

// Define pins servo
#define MOTEUR1_PIN_SENS1 4
#define MOTEUR1_PIN_SENS2 5
#define MOTEUR1_PIN_ACTIVATION 0
#define MOTEUR2_PIN_SENS1 6
#define MOTEUR2_PIN_SENS2 7
#define MOTEUR2_PIN_ACTIVATION 1
#define MOTEUR3_PIN_SENS1 11
#define MOTEUR3_PIN_SENS2 10
#define MOTEUR3_PIN_ACTIVATION 2
#define MOTEUR4_PIN_SENS1 8
#define MOTEUR4_PIN_SENS2 9
#define MOTEUR4_PIN_ACTIVATION 3

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

// Init all motion motors pins
void Init_motion_motor(void)
{
	// Init motion motors

	// Init 1/0 pin for control motion motors
	gpio_init(MOTEUR1_PIN_SENS1);
	gpio_init(MOTEUR1_PIN_SENS2);
	gpio_init(MOTEUR2_PIN_SENS1);
	gpio_init(MOTEUR2_PIN_SENS2);
	gpio_init(MOTEUR3_PIN_SENS1);
	gpio_init(MOTEUR3_PIN_SENS2);
	gpio_init(MOTEUR4_PIN_SENS1);
	gpio_init(MOTEUR4_PIN_SENS2);
	gpio_set_dir(MOTEUR1_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR1_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR2_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR2_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR3_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR3_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR4_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR4_PIN_SENS2, GPIO_OUT);

	// Set direction to 0 (disactivate)
	gpio_put(MOTEUR1_PIN_SENS1, 0);
	gpio_put(MOTEUR1_PIN_SENS2, 0);
	gpio_put(MOTEUR2_PIN_SENS1, 0);
	gpio_put(MOTEUR2_PIN_SENS2, 0);
	gpio_put(MOTEUR3_PIN_SENS1, 0);
	gpio_put(MOTEUR3_PIN_SENS2, 0);
	gpio_put(MOTEUR4_PIN_SENS1, 0);
	gpio_put(MOTEUR4_PIN_SENS2, 0);

	// Init pwm pins for motion motors
	gpio_init(MOTEUR1_PIN_ACTIVATION);
	gpio_init(MOTEUR2_PIN_ACTIVATION);
	gpio_init(MOTEUR3_PIN_ACTIVATION);
	gpio_init(MOTEUR4_PIN_ACTIVATION);
	gpio_set_function(MOTEUR1_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR2_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR3_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR4_PIN_ACTIVATION, GPIO_FUNC_PWM);

	// Set wrap of pwm slices
	pwm_set_wrap(0, 127);
	pwm_set_wrap(1, 127);

	// Active all pwm slices
	pwm_set_enabled(0, true);
	pwm_set_enabled(1, true);

	// Set speed to 0
	pwm_set_chan_level(0, PWM_CHAN_A, 0);
	pwm_set_chan_level(0, PWM_CHAN_B, 0);
	pwm_set_chan_level(1, PWM_CHAN_A, 0);
	pwm_set_chan_level(1, PWM_CHAN_B, 0);

	// Servo Init

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

	// Set wrap of pwm slice
	pwm_set_wrap(2, 25000);

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

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

	// Set channel level
	pwm_set_chan_level(2, PWM_CHAN_A, 1950); // Ascenseur
	pwm_set_chan_level(2, PWM_CHAN_B, 1000); // Pince
	// 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_ascenseur_haut(){
	pwm_set_chan_level(2, PWM_CHAN_A, 2550);
}
void Servo_ascenseur_bas(){
	pwm_set_chan_level(2, PWM_CHAN_A, 1550);
}
void Servo_ascenseur_lache_plante(){
	pwm_set_chan_level(2, PWM_CHAN_A, 2050);
}
void Servo_ascenseur_lache_pot_jardiniere(){
	pwm_set_chan_level(2, PWM_CHAN_A, 1950);
}

void Servo_pince_ouverte(){
	pwm_set_chan_level(2, PWM_CHAN_B, 1000);
}
void Servo_pince_pot(){
	pwm_set_chan_level(2, PWM_CHAN_B, 1700);
}
void Servo_pince_plante(){
	pwm_set_chan_level(2, PWM_CHAN_B, 1950);
}


// Set motor 1 speed forward
void Motor1_forward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_A, speed);
	gpio_put(MOTEUR1_PIN_SENS1, 0);
	gpio_put(MOTEUR1_PIN_SENS2, 1);
}

// Set motor 1 speed backward
void Motor1_backward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_A, speed);
	gpio_put(MOTEUR1_PIN_SENS1, 1);
	gpio_put(MOTEUR1_PIN_SENS2, 0);
}

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

// Set motor 2 speed forward
void Motor2_forward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_B, speed);
	gpio_put(MOTEUR2_PIN_SENS1, 0);
	gpio_put(MOTEUR2_PIN_SENS2, 1);
}

// Set motor 2 speed backward
void Motor2_backward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_B, speed);
	gpio_put(MOTEUR2_PIN_SENS1, 1);
	gpio_put(MOTEUR2_PIN_SENS2, 0);
}

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

// Set motor 3 speed forward
void Motor3_forward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_A, speed);
	gpio_put(MOTEUR3_PIN_SENS1, 0);
	gpio_put(MOTEUR3_PIN_SENS2, 1);
}

// Set motor 3 speed backward
void Motor3_backward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_A, speed);
	gpio_put(MOTEUR3_PIN_SENS1, 1);
	gpio_put(MOTEUR3_PIN_SENS2, 0);
}

// Set motor 3 speed and direction (negative value : backward / positive value : forward)
void Motor3_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor3_backward(speed);
	}
	else
	{
		Motor3_forward(speed);
	}
}

// Set motor 4 speed forward
void Motor4_forward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_B, speed);
	gpio_put(MOTEUR4_PIN_SENS1, 0);
	gpio_put(MOTEUR4_PIN_SENS2, 1);
}

// Set motor 4 speed backward
void Motor4_backward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_B, speed);
	gpio_put(MOTEUR4_PIN_SENS1, 1);
	gpio_put(MOTEUR4_PIN_SENS2, 0);
}

// Set motor 4 speed and direction (negative value : backward / positive value : forward)
void Motor4_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor4_backward(speed);
	}
	else
	{
		Motor4_forward(speed);
	}
}

// Set