From 30326baef7d43cef3d960919a85e8d3475a28ee5 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 12 Jan 2024 19:05:56 +0100 Subject: [PATCH] ... --- CMakeLists.txt | 1 + build/rien.ici | 0 main.c | 276 ++++--------------------------------------------- moteur.c | 192 ++++++++++++++++++++++++++++++++++ moteur.h | 16 +++ 5 files changed, 228 insertions(+), 257 deletions(-) delete mode 100644 build/rien.ici create mode 100644 moteur.c create mode 100644 moteur.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 53b8f99..ea233cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ add_executable(Mon_Projet i2c_maitre.c i2c_slave.c communication.c + moteur.c ) target_include_directories(Mon_Projet PRIVATE Mon_Projet_ULD_API/inc/) diff --git a/build/rien.ici b/build/rien.ici deleted file mode 100644 index e69de29..0000000 diff --git a/main.c b/main.c index 539e746..1ea9083 100644 --- a/main.c +++ b/main.c @@ -1,28 +1,8 @@ -/***** - * 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 - -// 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 +#include "moteur.h" // Juste pour information - les broches sont re-définies dans i2c_slave #define I2C0_SDA_PIN 16 @@ -32,260 +12,42 @@ #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]; + + // Init "all" stdio_init_all(); - Init_motion_motor(); communication_init(); - - - // Exemple de communication - message[0]='B'; - message[1]='a'; - message[2]='\n'; - message[3]='\0'; + uint vitesse_x = 0; + uint vitesse_y = 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); + if (!(communication_read_message(reception) == I2C_ECHEC)) + { + vitesse_x = reception[0]-128; + vitesse_y = reception[1]-128; + } + else + { + vitesse_x = 0; + vitesse_y = 0; } - - 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); + Motor1_speed(vitesse_x); + Motor2_speed(vitesse_x); + Motor3_speed(vitesse_y); + Motor4_speed(vitesse_y); } } -// 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 diff --git a/moteur.c b/moteur.c new file mode 100644 index 0000000..70b119e --- /dev/null +++ b/moteur.c @@ -0,0 +1,192 @@ +#include "moteur.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) +{ + // 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); + } +} \ No newline at end of file diff --git a/moteur.h b/moteur.h new file mode 100644 index 0000000..96cb3c7 --- /dev/null +++ b/moteur.h @@ -0,0 +1,16 @@ +#include "pico/stdlib.h" +#include "hardware/pwm.h" + +void Init_motion_motor(void); +void Motor1_forward(int speed); +void Motor1_backward(int speed); +void Motor1_speed(int speed); +void Motor2_forward(int speed); +void Motor2_backward(int speed); +void Motor2_speed(int speed); +void Motor3_forward(int speed); +void Motor3_backward(int speed); +void Motor3_speed(int speed); +void Motor2_forward(int speed); +void Motor3_backward(int speed); +void Motor4_speed(int speed); \ No newline at end of file