Robot_Riombotique/main.c

/*****
 * Copyright (c) 2023 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
// Include libraries
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/adc.h"
#include <stdio.h>

// Define pins
#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 8
#define MOTEUR3_PIN_SENS2 9
#define MOTEUR3_PIN_ACTIVATION 2
#define MOTEUR4_PIN_SENS1 10
#define MOTEUR4_PIN_SENS2 11
#define MOTEUR4_PIN_ACTIVATION 3

// Init all motion motors pins
void Init_motion_motor(void);
// Set motor 1 speed forward
void Motor1_forward(int speed);
// Set motor 1 speed backward
void Motor1_backward(int speed);
// Set motor 1 speed and direction (negative value : backward / positive value : forward)
void Motor1_speed(int speed);
// Set motor 2 speed forward
void Motor2_forward(int speed);
// Set motor 2 speed backward
void Motor2_backward(int speed);
// Set motor 2 speed and direction (negative value : backward / positive value : forward)
void Motor2_speed(int speed);
// Set motor 3 speed forward
void Motor3_forward(int speed);
// Set motor 3 speed backward
void Motor3_backward(int speed);
// Set motor 3 speed and direction (negative value : backward / positive value : forward)
void Motor3_speed(int speed);
// Set motor 1 speed forward
void Motor2_forward(int speed);
// Set motor 1 speed backward
void Motor3_backward(int speed);
// Set motor 1 speed and direction (negative value : backward / positive value : forward)
void Motor4_speed(int speed);

void main(void)
{
    stdio_init_all();

	Init_motion_motor();

	while(1)
	{
		Motor1_speed(-1000);
		Motor2_speed(-1000);
		Motor3_speed(-1000);
		Motor4_speed(-1000);
		sleep_ms(3000);
		Motor1_speed(1000);
		Motor2_speed(1000);
		Motor3_speed(1000);
		Motor4_speed(1000);
		sleep_ms(3000);
	}

}

// Init all motion motors pins
void Init_motion_motor(void)
{
	// 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, 1000);
	pwm_set_wrap(1, 1000);

	// 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);

}

// 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);
	}
}