296 lines
6.9 KiB
C
296 lines
6.9 KiB
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 "communication.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
|
|
|
|
// Juste pour information - les broches sont re-définies dans i2c_slave
|
|
#define I2C0_SDA_PIN 16
|
|
#define I2C0_SCL_PIN 17
|
|
|
|
// Juste pour information - les broches sont re-définies dans i2c_master
|
|
#define I2C1_SDA_PIN 18
|
|
#define I2C1_SCL_PIN 19
|
|
|
|
// 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);
|
|
// Convert te joystick value to an char value
|
|
uint8_t Conversions_joystick2uint8(uint16_t joystick_value);
|
|
|
|
void main(void)
|
|
{
|
|
char message[256], reception[256];
|
|
stdio_init_all();
|
|
|
|
Init_motion_motor();
|
|
communication_init();
|
|
|
|
|
|
// Exemple de communication
|
|
message[0]='B';
|
|
message[1]='a';
|
|
message[2]='\n';
|
|
message[3]='\0';
|
|
|
|
while(1){
|
|
|
|
printf("Envoi message\n");
|
|
communication_send_message(message, 4);
|
|
|
|
printf("Lire message\n");
|
|
if (communication_read_message(reception) == I2C_ECHEC){
|
|
printf("Echec de la lecture du message\n");
|
|
}else{
|
|
printf("Succes\n");
|
|
printf("%s",reception);
|
|
}
|
|
|
|
sleep_ms(1000);
|
|
}
|
|
|
|
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
/// @brief Prend un nombre entre 0 et 4096, et sort un nombre entre 0 et 255
|
|
/// @return un nombre entre 0 et 255
|
|
uint8_t Conversions_joystick2uint8(uint16_t joystick_value)
|
|
{
|
|
uint8_t valeur = joystick_value / 4;
|
|
return valeur;
|
|
} |