From 5e32b7f95051aabb9d826fe308384cca1c3f85f9 Mon Sep 17 00:00:00 2001 From: Ulysse Cura Date: Sun, 4 May 2025 18:06:22 +0200 Subject: [PATCH] Started main controller code (copied useful files from motion controller) --- .../.vscode/c_cpp_properties.json | 31 ++ main controller code/.vscode/settings.json | 7 + main controller code/.vscode/tasks.json | 25 ++ main controller code/CMakeLists.txt | 32 ++ main controller code/Readme.md | 54 ++++ main controller code/last/communication.c | 107 +++++++ main controller code/last/communication.h | 5 + main controller code/last/i2c_fifo.h | 53 ++++ main controller code/last/i2c_maitre.c | 281 ++++++++++++++++++ main controller code/last/i2c_maitre.h | 15 + main controller code/last/i2c_slave.c | 108 +++++++ main controller code/last/i2c_slave.h | 66 ++++ main controller code/last/main.c | 172 +++++++++++ main controller code/pico_sdk_import.cmake | 62 ++++ main controller code/src/include/motors.h | 55 ++++ main controller code/src/include/robot.h | 18 ++ main controller code/src/main.c | 23 ++ main controller code/src/motors.c | 119 ++++++++ main controller code/src/robot.c | 26 ++ 19 files changed, 1259 insertions(+) create mode 100644 main controller code/.vscode/c_cpp_properties.json create mode 100644 main controller code/.vscode/settings.json create mode 100644 main controller code/.vscode/tasks.json create mode 100644 main controller code/CMakeLists.txt create mode 100644 main controller code/Readme.md create mode 100644 main controller code/last/communication.c create mode 100644 main controller code/last/communication.h create mode 100644 main controller code/last/i2c_fifo.h create mode 100644 main controller code/last/i2c_maitre.c create mode 100644 main controller code/last/i2c_maitre.h create mode 100644 main controller code/last/i2c_slave.c create mode 100644 main controller code/last/i2c_slave.h create mode 100644 main controller code/last/main.c create mode 100644 main controller code/pico_sdk_import.cmake create mode 100644 main controller code/src/include/motors.h create mode 100644 main controller code/src/include/robot.h create mode 100644 main controller code/src/main.c create mode 100644 main controller code/src/motors.c create mode 100644 main controller code/src/robot.c diff --git a/main controller code/.vscode/c_cpp_properties.json b/main controller code/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..1a9e4a1 --- /dev/null +++ b/main controller code/.vscode/c_cpp_properties.json @@ -0,0 +1,31 @@ +{ + "env": { + "myDefaultIncludePath": [ + "${env:PICO_SDK_PATH}/src/**/include/", + "${workspaceFolder}/build/generated/pico_base/" + ], + "myCompilerPath": "/usr/bin/arm-none-eabi-gcc" + }, + + "configurations": [ + { + "name": "Linux", + "intelliSenseMode": "linux-gcc-arm", + "includePath": [ + "${myDefaultIncludePath}" + ], + "compilerPath": "/usr/bin/arm-none-eabi-gcc", + "cStandard": "c11", + "cppStandard": "c++17", + "browse": { + "path": [ + "${workspaceFolder}" + ], + "limitSymbolsToIncludedHeaders": true, + "databaseFilename": "" + }, + "configurationProvider": "ms-vscode.cmake-tools" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/main controller code/.vscode/settings.json b/main controller code/.vscode/settings.json new file mode 100644 index 0000000..80f9ddc --- /dev/null +++ b/main controller code/.vscode/settings.json @@ -0,0 +1,7 @@ +{ + "files.associations": { + "binary_info.h": "c", + "i2c.h": "c", + "stdlib.h": "c" + } +} \ No newline at end of file diff --git a/main controller code/.vscode/tasks.json b/main controller code/.vscode/tasks.json new file mode 100644 index 0000000..c98bf9b --- /dev/null +++ b/main controller code/.vscode/tasks.json @@ -0,0 +1,25 @@ +{ + "tasks": [ + { + "type": "shell", + "command": "cd build; cmake ../; make", + "label": "CMake in build/", + "problemMatcher": [], + "group": { + "kind": "build", + "isDefault": false + } + }, + { + "type": "shell", + "command": "cd build; cmake ../; make Flash", + "label": "CMake & Make & Flash", + "problemMatcher": [], + "group": { + "kind": "build", + "isDefault": true + } + } + ], + "version": "2.0.0" +} \ No newline at end of file diff --git a/main controller code/CMakeLists.txt b/main controller code/CMakeLists.txt new file mode 100644 index 0000000..229c319 --- /dev/null +++ b/main controller code/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.13) + +include(pico_sdk_import.cmake) + +project(motion_controller C CXX ASM) +set(CMAKE_C_STNDARD 11) +set(CMAKE_CXX_STANDARD 17) + +set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR}) + +pico_sdk_init() + +add_executable(motion_controller + src/main.c +) + +target_link_libraries(motion_controller + hardware_i2c + hardware_pwm + hardware_uart + pico_stdlib +) + +pico_enable_stdio_usb(motion_controller 1) +pico_enable_stdio_uart(motion_controller 1) + +pico_add_extra_outputs(motion_controller) + +add_custom_target(Flash + DEPENDS motion_controller + COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/motion_controller.uf2 +) diff --git a/main controller code/Readme.md b/main controller code/Readme.md new file mode 100644 index 0000000..11abdd7 --- /dev/null +++ b/main controller code/Readme.md @@ -0,0 +1,54 @@ +Motion controler code for the RPI Pico (RP2040) +=============================================== + +This project is the main controller firmware for the RPI Pico (RP2040), designed for the Eurobot 2025 Cup. + + +I2C description +----------------------------------------------- + +The robot’s I2C communication works as follows: + * Send the device address + R/W bit (to select read or write mode). + * Send the target register address (to read from or write to). + * Read or write the register data. Multiple registers can be read/written sequentially, with the address auto-incrementing after each operation. + +This code is designed to be the master in the i2c communication; + +|Adress |R/W|Description |Encoding | +|-------|:-:|-------------------------------|:-----------------:| +| 0x00 | W | Speed motor 1 |**-128** - **127** | +| 0x01 | W | Speed motor 2 |**-128** - **127** | +| 0x02 | W | Speed motor 3 |**-128** - **127** | +| 0x03 | W | Speed motor 4 |**-128** - **127** | +| 0x04 | W | Servo 1 position selection | **0** - **1** | +| 0x05 | W | Servo 2 position selection | **0** - **1** | +| 0x06 | W | Servo 3 position selection | **0** - **1** | +| 0x07 | W | Servo 4 position selection | **0** - **1** | + + +Motors communication description +----------------------------------------------- + +Motors are «connected» to their respective I2C buffer address. + +To control a motor you need to write data to its adress of the form : + +>```C +>int8_t speed; +>``` + +Value goes from **-128** to **127**. + + +Servo motors communication description +----------------------------------------------- + +Servo motors are «connected» to their respective I2C buffer address. + +To control a servo motor you need to write data to its adress of the form : + +>```C +>uint8_t close; +>``` + +Value is 0 or 1 for the open or the close pos. \ No newline at end of file diff --git a/main controller code/last/communication.c b/main controller code/last/communication.c new file mode 100644 index 0000000..1d0b0d6 --- /dev/null +++ b/main controller code/last/communication.c @@ -0,0 +1,107 @@ +/***** + * + * Le principe est que la télécommande soit l'esclave I2C + * Pour envoyer un message, on charge le message à l'adresse 0 de la "mémoire" pour l'i2c + * + * Pour lire le message, le robot interroge la télécommande, et demande le contenu à partir de l'adresse 0 de la mémoire. + * Ainsi, en cas d'échec de la communication, le robot détectera une manette débranchée. + * Par défaut, la fonction lit 255 caractères. + * + * Copyright (c) 2024 - Club robotique de Riom + * + * + * SPDX-License-Identifier: BSD-3-Clause +*/ +#include "i2c_fifo.h" +#include "i2c_slave.h" +#include "i2c_maitre.h" +#include "string.h" + + +/// DEBUT DE LA CONFIGURATION de L'I2C + +#define I2C0_SDA_PIN 16 +#define I2C0_SCL_PIN 17 + +#define I2C_SLAVE_ADDRESS 0x17 + +static const uint I2C_SLAVE_SDA_PIN = I2C0_SDA_PIN; +static const uint I2C_SLAVE_SCL_PIN = I2C0_SCL_PIN; + +// The slave implements a 256 byte memory. To write a series of bytes, the master first +// writes the memory address, followed by the data. The address is automatically incremented +// for each byte transferred, looping back to 0 upon reaching the end. Reading is done +// sequentially from the current memory address. +static struct +{ + uint8_t mem[256]; + uint8_t mem_address; + bool mem_address_written; +} context; + +// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls / +// printing to stdio may interfere with interrupt handling. +static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) { + switch (event) { + case I2C_SLAVE_RECEIVE: // master has written some data + if (!context.mem_address_written) { + // writes always start with the memory address + context.mem_address = i2c_read_byte(i2c); + context.mem_address_written = true; + } else { + // save into memory + context.mem[context.mem_address] = i2c_read_byte(i2c); + context.mem_address++; + } + break; + case I2C_SLAVE_REQUEST: // master is requesting data + // load from memory + i2c_write_byte(i2c, context.mem[context.mem_address]); + context.mem_address++; + break; + case I2C_SLAVE_FINISH: // master has signalled Stop / Restart + context.mem_address_written = false; + break; + default: + break; + } +} + +void i2c_set_slave_mode_perso(i2c_inst_t *i2c, uint8_t addr) { + i2c->hw->enable = 0; + + //while( !(i2c->hw->enable_status & 0x1) ); + + i2c->hw->sar = addr; + i2c->hw->con = 0; + + i2c->hw->enable = 1; +} + +static void setup_slave() { + gpio_init(I2C_SLAVE_SDA_PIN); + gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C); + gpio_pull_up(I2C_SLAVE_SDA_PIN); + + gpio_init(I2C_SLAVE_SCL_PIN); + gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C); + gpio_pull_up(I2C_SLAVE_SCL_PIN); + + i2c_slave_init(i2c0, I2C_SLAVE_ADDRESS, &i2c_slave_handler); +} +/// FIN DE LA CONFIGURATION de L'I2C + + +void communication_init(void){ + setup_slave(); + i2c_maitre_init(); +} + +void communication_envoyer_message(unsigned char * message, unsigned int message_length){ + memcpy(context.mem, message, message_length); +} + +int communication_lire_message(unsigned char * message){ + i2c_lire_registre(I2C_SLAVE_ADDRESS, 0, message, 255); + +} diff --git a/main controller code/last/communication.h b/main controller code/last/communication.h new file mode 100644 index 0000000..aba618b --- /dev/null +++ b/main controller code/last/communication.h @@ -0,0 +1,5 @@ +#include "i2c_maitre.h" + +void communication_init(void); +void communication_envoyer_message(unsigned char * message, unsigned int message_length); +enum i2c_resultat_t communication_lire_message(unsigned char * message); diff --git a/main controller code/last/i2c_fifo.h b/main controller code/last/i2c_fifo.h new file mode 100644 index 0000000..805c00e --- /dev/null +++ b/main controller code/last/i2c_fifo.h @@ -0,0 +1,53 @@ +/* + * Copyright (c) 2021 Valentin Milea + * + * SPDX-License-Identifier: MIT + */ + +#ifndef _I2C_FIFO_H_ +#define _I2C_FIFO_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** \file i2c_fifo.h + * + * \brief I2C non-blocking r/w. + */ + +/** + * \brief Pop a byte from I2C Rx FIFO. + * + * This function is non-blocking and assumes the Rx FIFO isn't empty. + * + * \param i2c I2C instance. + * \return uint8_t Byte value. + */ +static inline uint8_t i2c_read_byte(i2c_inst_t *i2c) { + i2c_hw_t *hw = i2c_get_hw(i2c); + assert(hw->status & I2C_IC_STATUS_RFNE_BITS); // Rx FIFO must not be empty + return (uint8_t)hw->data_cmd; +} + +/** + * \brief Push a byte into I2C Tx FIFO. + * + * This function is non-blocking and assumes the Tx FIFO isn't full. + * + * \param i2c I2C instance. + * \param value Byte value. + */ +static inline void i2c_write_byte(i2c_inst_t *i2c, uint8_t value) { + i2c_hw_t *hw = i2c_get_hw(i2c); + assert(hw->status & I2C_IC_STATUS_TFNF_BITS); // Tx FIFO must not be full + hw->data_cmd = value; +} + +#ifdef __cplusplus +} +#endif + +#endif // _I2C_FIFO_H_ diff --git a/main controller code/last/i2c_maitre.c b/main controller code/last/i2c_maitre.c new file mode 100644 index 0000000..7975b09 --- /dev/null +++ b/main controller code/last/i2c_maitre.c @@ -0,0 +1,281 @@ +#include "i2c_maitre.h" +#include "hardware/gpio.h" +#include "hardware/i2c.h" +#include "pico/stdlib.h" +#include + +#define I2C1_SDA_PIN 18 +#define I2C1_SCL_PIN 19 + +#define I2C_NB_MAX_TAMPON 20 + +enum i2c_statu_t{ + I2C_STATU_LIBRE, + I2C_STATU_OCCUPE +} i2c_statu_i2c1; + +uint16_t I2C_tampon_envoi[I2C_NB_MAX_TAMPON]; +uint8_t I2C_tampon_reception[I2C_NB_MAX_TAMPON]; +uint16_t I2C_nb_a_envoyer, I2C_nb_a_recevoir; +uint8_t adresse_7_bits; +uint32_t i2c_error_code; // value of i2c->hw->tx_abrt_source if anything wrong happen, 0 if everything was fine. + +enum transaction_statu_t{ + TRANSACTION_EN_COURS, + TRANSACTION_TERMINEE +} statu_emission, statu_reception; + +void i2d_set_adresse_esclave(uint8_t _adresse_7bits); +void i2c_charger_tampon_envoi(uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception); +enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception); + +void i2c_maitre_init(void){ + //stdio_init_all(); + i2c_init(i2c1, 100 * 1000); + + printf("Initialisation des broches\n"); + for(int i=0; i++; i<=28){ + if(gpio_get_function(i) == GPIO_FUNC_I2C){ + printf("Borche I2C : %d\n", i); + gpio_set_function(i, GPIO_FUNC_NULL); + } + } + + printf("%d et %d en I2C\n", I2C1_SDA_PIN, I2C1_SCL_PIN); + gpio_set_function(I2C1_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C1_SCL_PIN, GPIO_FUNC_I2C); + gpio_pull_up(I2C1_SDA_PIN); + gpio_pull_up(I2C1_SCL_PIN); + + i2c_statu_i2c1 = I2C_STATU_LIBRE; +} + +/// @brief Fonction à appeler régulièrement ou en interruption. +/// @param i2c +void i2c_gestion(i2c_inst_t *i2c){ + // on veut gérer l'i2c avec cette fonction. + // 2 cas : + // - Soit écriture simple (plusieurs octets (W)) + // - Soit écriture + lecture (Adresse (W), registre (W), données (R)) + // Pour écrire 1 octet, i2c->hw->data_cmd = xxx, (avec CMD:8 à 0, ) + // Pour lire 1 octet, i2c->hw->data_cmd = xxx (avec CMD:8 à 1) + // Il faut mettre CMD:9 à 1 pour le dernier octet. + + // Envoi des données (ou des demandes de lecture) + static uint16_t index_envoi=0, index_reception=0; + + // Acquitement des erreurs, pas 100% fonctionnel ! TODO ! + if(i2c->hw->tx_abrt_source !=0){ + // Seule solution trouvée pour réinitialiser l'I2C. + char a; + i2c_read_blocking(i2c, adresse_7_bits, &a, 1, false); + + I2C_nb_a_envoyer = 0; + index_reception = 0; + I2C_nb_a_recevoir = 0; + statu_emission = TRANSACTION_TERMINEE; + statu_reception = TRANSACTION_TERMINEE; + i2c_statu_i2c1 = I2C_STATU_LIBRE; + } + + while( (index_envoi < I2C_nb_a_envoyer) && (i2c_get_write_available(i2c)) ){ + bool restart = false; + bool last = false; + + if (index_envoi == 0){ + // Début de l'envoi, assurons nous d'avoir la bonne adresse de l'esclave + i2c->hw->enable = 0; + i2c->hw->tar = adresse_7_bits; + i2c->hw->enable = 1; + }else{ + // Passage de l'écriture à la lecture, on envoie un bit de restart. + if( !(I2C_tampon_envoi[index_envoi-1] & I2C_IC_DATA_CMD_CMD_BITS) && + (I2C_tampon_envoi[index_envoi] & I2C_IC_DATA_CMD_CMD_BITS)){ + restart = true; + } + } + + if(index_envoi + 1 == I2C_nb_a_envoyer){ + // Fin de la trame, nous devons envoyer un bit de stop. + last = true; + } + + i2c->hw->data_cmd = + I2C_tampon_envoi[index_envoi] | + bool_to_bit(restart) << I2C_IC_DATA_CMD_RESTART_LSB | + bool_to_bit(last) << I2C_IC_DATA_CMD_STOP_LSB; + + if(last){ + statu_emission = TRANSACTION_TERMINEE; + index_envoi = 0; + I2C_nb_a_envoyer = 0; + //printf("I2C emission terminee\n"); + }else{ + index_envoi++; + } + + } + + // Réception des données - Lecture des données présentes dans le tampon + while( (index_reception < I2C_nb_a_recevoir) && (i2c_get_read_available(i2c)) ){ + I2C_tampon_reception[index_reception] = (uint8_t) i2c->hw->data_cmd; + index_reception++; + } + if(index_reception == I2C_nb_a_recevoir){ + statu_reception = TRANSACTION_TERMINEE; + index_reception = 0; + I2C_nb_a_recevoir = 0; + } + + if(statu_reception == TRANSACTION_TERMINEE && statu_emission == TRANSACTION_TERMINEE){ + i2c_statu_i2c1 = I2C_STATU_LIBRE; + } + +} + +/// @brief Charge le tampon d'émission pour pré-mâcher le travail à la fonction i2c_gestion +/// @param emission +/// @param nb_envoi +/// @param nb_reception +void i2c_charger_tampon_envoi(uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception){ + // Données à envoyer + for(unsigned int index=0; index Non, voir i2c_lire_registre_nb +/// @param adresse_7_bits +/// @param +/// @return I2C_SUCCES (1) ou I2C_ECHEC (2) +int i2c_lire_registre(char adresse_7_bits, char registre, unsigned char * reception, char len){ + int statu; + char emission[1]; + + emission[0] = registre; + statu = i2c_write_blocking (i2c1, adresse_7_bits, emission, 1, 0); + if(statu == PICO_ERROR_GENERIC){ + printf("I2C - Envoi registre Echec\n"); + return I2C_ECHEC; + } + + statu = i2c_read_blocking (i2c1, adresse_7_bits, reception, len, 0); + if(statu == PICO_ERROR_GENERIC){ + printf("I2C - Lecture registre Echec\n"); + return I2C_ECHEC; + } + + return I2C_SUCCES; +} + +int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre){ + int statu; + char emission[2]; + + emission[0] = registre; + emission[1] = valeur_registre; + statu = i2c_write_blocking (i2c1, adresse_7_bits, emission, 2, 0); + if(statu == PICO_ERROR_GENERIC){ + printf("Erreur ecrire registre\n"); + return I2C_ECHEC; + } + + printf("i2c Registre %x, valeur %x\n", registre, valeur_registre); + + return I2C_SUCCES; +} \ No newline at end of file diff --git a/main controller code/last/i2c_maitre.h b/main controller code/last/i2c_maitre.h new file mode 100644 index 0000000..1a42c68 --- /dev/null +++ b/main controller code/last/i2c_maitre.h @@ -0,0 +1,15 @@ +#include "pico/stdlib.h" +#include "hardware/i2c.h" + +enum i2c_resultat_t { + I2C_EN_COURS, + I2C_SUCCES, + I2C_ECHEC +}; + +void i2c_maitre_init(void); +void i2c_gestion(i2c_inst_t *i2c); +enum i2c_resultat_t i2c_lire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * reception, uint8_t len); +enum i2c_resultat_t i2c_ecrire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * _emission, uint8_t len); +int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre); +int i2c_lire_registre(char adresse_7_bits, char registre, unsigned char * reception, char len); diff --git a/main controller code/last/i2c_slave.c b/main controller code/last/i2c_slave.c new file mode 100644 index 0000000..1eb21dc --- /dev/null +++ b/main controller code/last/i2c_slave.c @@ -0,0 +1,108 @@ +/* + * Copyright (c) 2021 Valentin Milea + * + * SPDX-License-Identifier: MIT + */ + +#include "i2c_slave.h" +#include "hardware/irq.h" + +typedef struct i2c_slave_t +{ + i2c_inst_t *i2c; + i2c_slave_handler_t handler; + bool transfer_in_progress; +} i2c_slave_t; + +static i2c_slave_t i2c_slaves[2]; + +static inline void finish_transfer(i2c_slave_t *slave) { + if (slave->transfer_in_progress) { + slave->handler(slave->i2c, I2C_SLAVE_FINISH); + slave->transfer_in_progress = false; + } +} + +static void __not_in_flash_func(i2c_slave_irq_handler)(i2c_slave_t *slave) { + i2c_inst_t *i2c = slave->i2c; + i2c_hw_t *hw = i2c_get_hw(i2c); + + uint32_t intr_stat = hw->intr_stat; + if (intr_stat == 0) { + return; + } + if (intr_stat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) { + hw->clr_tx_abrt; + finish_transfer(slave); + } + if (intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) { + hw->clr_start_det; + finish_transfer(slave); + } + if (intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) { + hw->clr_stop_det; + finish_transfer(slave); + } + if (intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) { + slave->transfer_in_progress = true; + slave->handler(i2c, I2C_SLAVE_RECEIVE); + } + if (intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) { + hw->clr_rd_req; + slave->transfer_in_progress = true; + slave->handler(i2c, I2C_SLAVE_REQUEST); + } +} + +static void __not_in_flash_func(i2c0_slave_irq_handler)() { + i2c_slave_irq_handler(&i2c_slaves[0]); +} + +static void __not_in_flash_func(i2c1_slave_irq_handler)() { + i2c_slave_irq_handler(&i2c_slaves[1]); +} + +void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler) { + assert(i2c == i2c0 || i2c == i2c1); + assert(handler != NULL); + + uint i2c_index = i2c_hw_index(i2c); + i2c_slave_t *slave = &i2c_slaves[i2c_index]; + slave->i2c = i2c; + slave->handler = handler; + + // Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty. + // There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it + // disabled since the Rx FIFO should never fill up (unless slave->handler() is way too slow). + i2c_set_slave_mode(i2c, true, address); + + i2c_hw_t *hw = i2c_get_hw(i2c); + // unmask necessary interrupts + hw->intr_mask = I2C_IC_INTR_MASK_M_RX_FULL_BITS | I2C_IC_INTR_MASK_M_RD_REQ_BITS | I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS | I2C_IC_INTR_MASK_M_STOP_DET_BITS | I2C_IC_INTR_MASK_M_START_DET_BITS; + + // enable interrupt for current core + uint num = I2C0_IRQ + i2c_index; + irq_set_exclusive_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler); + irq_set_enabled(num, true); +} + +void i2c_slave_deinit(i2c_inst_t *i2c) { + assert(i2c == i2c0 || i2c == i2c1); + + uint i2c_index = i2c_hw_index(i2c); + i2c_slave_t *slave = &i2c_slaves[i2c_index]; + assert(slave->i2c == i2c); // should be called after i2c_slave_init() + + slave->i2c = NULL; + slave->handler = NULL; + slave->transfer_in_progress = false; + + uint num = I2C0_IRQ + i2c_index; + irq_set_enabled(num, false); + irq_remove_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler); + + i2c_hw_t *hw = i2c_get_hw(i2c); + hw->intr_mask = I2C_IC_INTR_MASK_RESET; + + i2c_set_slave_mode(i2c, false, 0); +} diff --git a/main controller code/last/i2c_slave.h b/main controller code/last/i2c_slave.h new file mode 100644 index 0000000..5c65e00 --- /dev/null +++ b/main controller code/last/i2c_slave.h @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2021 Valentin Milea + * + * SPDX-License-Identifier: MIT + */ + +#ifndef _I2C_SLAVE_H_ +#define _I2C_SLAVE_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** \file i2c_slave.h + * + * \brief I2C slave setup. + */ + +/** + * \brief I2C slave event types. + */ +typedef enum i2c_slave_event_t +{ + I2C_SLAVE_RECEIVE, /**< Data from master is available for reading. Slave must read from Rx FIFO. */ + I2C_SLAVE_REQUEST, /**< Master is requesting data. Slave must write into Tx FIFO. */ + I2C_SLAVE_FINISH, /**< Master has sent a Stop or Restart signal. Slave may prepare for the next transfer. */ +} i2c_slave_event_t; + +/** + * \brief I2C slave event handler + * + * The event handler will run from the I2C ISR, so it should return quickly (under 25 us at 400 kb/s). + * Avoid blocking inside the handler and split large data transfers across multiple calls for best results. + * When sending data to master, up to `i2c_get_write_available()` bytes can be written without blocking. + * When receiving data from master, up to `i2c_get_read_available()` bytes can be read without blocking. + * + * \param i2c Slave I2C instance. + * \param event Event type. + */ +typedef void (*i2c_slave_handler_t)(i2c_inst_t *i2c, i2c_slave_event_t event); + +/** + * \brief Configure I2C instance for slave mode. + * + * \param i2c I2C instance. + * \param address 7-bit slave address. + * \param handler Called on events from I2C master. It will run from the I2C ISR, on the CPU core + * where the slave was initialized. + */ +void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler); + +/** + * \brief Restore I2C instance to master mode. + * + * \param i2c I2C instance. + */ +void i2c_slave_deinit(i2c_inst_t *i2c); + +#ifdef __cplusplus +} +#endif + +#endif // _I2C_SLAVE_H_ diff --git a/main controller code/last/main.c b/main controller code/last/main.c new file mode 100644 index 0000000..7290ded --- /dev/null +++ b/main controller code/last/main.c @@ -0,0 +1,172 @@ + /***** + * Copyright (c) 2023 - Poivron Robotique + * + * SPDX-License-Identifier: BSD-3-Clause +*/ +#include "communication.h" +#include "pico/stdlib.h" +#include "hardware/pwm.h" +#include +#include "hardware/adc.h" +#define LED_VERTE 25 +#define PIN_VITESSE_M1 2 +#define PIN_SENS_A_M1 0 +#define PIN_SENS_B_M1 1 + +int result0, result1; +int joystic_clicker; +int pince; +int ascenceur; + +int M1_INITIALISE() +{ + gpio_init(PIN_VITESSE_M1); + gpio_init(PIN_SENS_A_M1); + gpio_init(PIN_SENS_B_M1); + + gpio_set_dir(PIN_VITESSE_M1, GPIO_OUT); + gpio_set_dir(PIN_SENS_A_M1, GPIO_OUT); + gpio_set_dir(PIN_SENS_B_M1, GPIO_OUT); + + gpio_set_function(PIN_VITESSE_M1, GPIO_FUNC_PWM); + pwm_set_wrap(PIN_VITESSE_M1, 100); + pwm_set_chan_level(PIN_VITESSE_M1, PWM_CHAN_B, 50); + +} + +int M1_AVANCE() +{ + + gpio_put(PIN_VITESSE_M1, 1); + gpio_put(PIN_SENS_A_M1, 1); + gpio_put(PIN_SENS_B_M1, 0); + + + +} + +int M1_RECULE() +{ + + gpio_put(PIN_VITESSE_M1, 1); + gpio_put(PIN_SENS_A_M1, 0); + gpio_put(PIN_SENS_B_M1, 1); + + + +} +int Adc_Init() +{ + adc_init(); + adc_gpio_init(26); + adc_gpio_init(27); +} +int AdcRead0() +{ + adc_select_input(0); + uint16_t resultadc = adc_read(); + return resultadc; +} + + + + + +int AdcRead1() +{ + adc_select_input(1); + uint16_t resultadc = adc_read(); + return resultadc; +} + + + + + + +void main() +{ + char message [256]; + stdio_init_all(); + + // Communication + communication_init(); + + // Pour envoyer un message + // communication_envoyer_message(message, 254); + + + // CLignottement LED + gpio_set_function(LED_VERTE, GPIO_FUNC_PWM); + pwm_set_wrap(4, 100); + pwm_set_chan_level(4, PWM_CHAN_B, 25); + pwm_set_enabled(4, true); + + // ADC + adc_init(); + adc_gpio_init(26); + adc_gpio_init(27); + + while (1) + { + // Voie X + result1 = AdcRead1(); + + message[0] = result1/16; + + // Voie Y + result0 = AdcRead0(); + + message[1] = result0/16; + + //clic sur le joystic + gpio_init(1); + gpio_pull_up(1); + gpio_set_dir(1, GPIO_IN); + joystic_clicker = gpio_get(1); + if (!joystic_clicker) + { + message[2] = result1/16; + message[1] = 128; + message[0] = 128; + } + else + { + message[2] = 128; + } + + + + //pince + gpio_init(2); + gpio_set_dir(2, GPIO_IN); + gpio_pull_up(2); + pince = gpio_get(2); + message [3] = pince; + + gpio_init(6); + gpio_set_dir(6, GPIO_IN); + gpio_pull_up(6); + ascenceur = gpio_get(6); + message[4] = ascenceur; + + + printf(">x:%d\n", message[0]); + printf(">Y:%d\n", message[1]); + printf(">Rz:%d\n", message[2]); + printf(">pince:%d\n", message[3]); + printf(">ascenceur:%d\n", message[4]); + sleep_ms(100); + communication_envoyer_message(message, 254); + } + M1_INITIALISE(); + while(1) + { + M1_AVANCE(); + sleep_ms(1000); + M1_RECULE(); + sleep_ms(1000); + + } + +} diff --git a/main controller code/pico_sdk_import.cmake b/main controller code/pico_sdk_import.cmake new file mode 100644 index 0000000..28efe9e --- /dev/null +++ b/main controller code/pico_sdk_import.cmake @@ -0,0 +1,62 @@ +# This is a copy of /external/pico_sdk_import.cmake + +# This can be dropped into an external project to help locate this SDK +# It should be include()ed prior to project() + +if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) + set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) + message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) + set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) + message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") +endif () + +if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) + set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) + message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") +endif () + +set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") +set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") +set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") + +if (NOT PICO_SDK_PATH) + if (PICO_SDK_FETCH_FROM_GIT) + include(FetchContent) + set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) + if (PICO_SDK_FETCH_FROM_GIT_PATH) + get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") + endif () + FetchContent_Declare( + pico_sdk + GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk + GIT_TAG master + ) + if (NOT pico_sdk) + message("Downloading Raspberry Pi Pico SDK") + FetchContent_Populate(pico_sdk) + set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) + endif () + set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) + else () + message(FATAL_ERROR + "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." + ) + endif () +endif () + +get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") +if (NOT EXISTS ${PICO_SDK_PATH}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") +endif () + +set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) +if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) + message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") +endif () + +set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) + +include(${PICO_SDK_INIT_CMAKE_FILE}) diff --git a/main controller code/src/include/motors.h b/main controller code/src/include/motors.h new file mode 100644 index 0000000..f7db597 --- /dev/null +++ b/main controller code/src/include/motors.h @@ -0,0 +1,55 @@ +#ifndef MOTORS_H +#define MOTORS_H + +// Motors +typedef enum motors_enum_t { + MOTOR1, + MOTOR2, + MOTOR3, + MOTOR4, + + NB_MOTORS +} motors_enum_t; + +typedef struct motor_def_t { + uint pwm_pin; + uint dir1_pin; + uint dir2_pin; + uint8_t buffer_address; +} motor_def_t; + +extern const motor_def_t MOTORS_DEFS[]; + +// Servo Motors +typedef enum { + SERVO_MOTOR1, + SERVO_MOTOR2, + SERVO_MOTOR3, + SERVO_MOTOR4, + + NB_SERVO_MOTORS +} servo_motors_enum_t; + +typedef struct { + uint pwm_pin; + uint open_pos; + uint close_pos; + uint8_t buffer_address; +} servo_motor_def_t; + +extern const servo_motor_def_t SERVO_MOTORS_DEFS[]; + +// Init all motors defined in the MOTORS_DEF array +void init_motors(void); +// Init all servo motors defined in the SERVO_MOTORS_DEF array +void init_servo_motors(void); +// Set [motor] to 0 +void motor_zero(motors_enum_t motor); +// Set [motor] in motor_enum_t at [value] between -127 and 128 (for this config) +void motor_set(motors_enum_t motor, int value); +// Set servo motor to its open pos +void servo_motor_zero(servo_motors_enum_t servo_motor); +// Set servo to its close pos if [close] else open pos +void servo_motor_set(servo_motors_enum_t servo_motor, bool close); + +#endif // MOTORS_H diff --git a/main controller code/src/include/robot.h b/main controller code/src/include/robot.h new file mode 100644 index 0000000..d23ef40 --- /dev/null +++ b/main controller code/src/include/robot.h @@ -0,0 +1,18 @@ +#ifndef ROBOT_H +#define ROBOT_H + +typedef struct robot_t { + bool is_running; + double delta_time_ms; +} robot_t; + +extern robot_t robot; + +// Init all robot's components +void robot_init(void); +// Handle inputs and outputs +void robot_handle_inputs_outputs(void); +// Deinit all robot's components +void robot_deinit(void); + +#endif // ROBOT_H \ No newline at end of file diff --git a/main controller code/src/main.c b/main controller code/src/main.c new file mode 100644 index 0000000..ac93fa7 --- /dev/null +++ b/main controller code/src/main.c @@ -0,0 +1,23 @@ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\ +* Code du RPI Pico principale gérant les differentes entrées-sorties. * +* Ce Pico est un maitre pilotant le gyroscope, l'internet et le motion controller.* +\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + +#include +#include "include/robot.h" + +robot_t robot; + +int main(void) +{ + robot_init(); + + while(robot.is_running) + { + robot_handle_inputs_outputs(); + } + + robot_deinit(); + + return 0; +} diff --git a/main controller code/src/motors.c b/main controller code/src/motors.c new file mode 100644 index 0000000..3969bb7 --- /dev/null +++ b/main controller code/src/motors.c @@ -0,0 +1,119 @@ +#include +#include + +#include "include/motors.h" + +const motor_def_t MOTORS_DEFS[] = { + {0, 4, 5, 0x00}, + {1, 6, 7, 0x01}, + {2, 8, 9, 0x02}, + {3, 10, 11, 0x03}, +}; + +const servo_motor_def_t SERVO_MOTORS_DEFS[] = { + {12, 0, 25000, 0x04}, + {13, 0, 25000, 0x05}, + {14, 0, 25000, 0x06}, + {15, 0, 25000, 0x07}, +}; + +// Init all motors defined in the MOTORS_DEF array +void init_motors(void) +{ + for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++) + { + const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor]; + + // Init PWM + uint slice_num = pwm_gpio_to_slice_num(motor_def->pwm_pin); + + gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM); + pwm_set_wrap(slice_num, 128); + pwm_set_enabled(slice_num, true); + + // Init dir pins + gpio_init(motor_def->dir1_pin); + gpio_set_dir(motor_def->dir1_pin, GPIO_OUT); + + gpio_init(motor_def->dir2_pin); + gpio_set_dir(motor_def->dir2_pin, GPIO_OUT); + + motor_zero(actual_motor); + } +} + +// Init all servo motors defined in the SERVO_MOTORS_DEF array +void init_servo_motors(void) +{ + for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++) + { + const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor]; + + // Init PWM // + uint slice_num = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin); + + gpio_set_function(servo_motor_def->pwm_pin, GPIO_FUNC_PWM); + pwm_set_wrap(slice_num, 25000); + pwm_set_clkdiv(slice_num, 100); + pwm_set_enabled(slice_num, true); + + servo_motor_zero(actual_servo_motor); + } +} + +// Set [motor] to 0 +void motor_zero(motors_enum_t motor) +{ + const motor_def_t *motor_def = &MOTORS_DEFS[motor]; + + // Set PWM to zero + pwm_set_gpio_level(motor_def->pwm_pin, 0); + + // Set dir pins to false + gpio_put(motor_def->dir1_pin, false); + gpio_put(motor_def->dir2_pin, false); +} + +// Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config) +void motor_set(motors_enum_t motor, int value) +{ + const motor_def_t *motor_def = &MOTORS_DEFS[motor]; + + if(value < 0) + { + gpio_put(motor_def->dir1_pin, true); + gpio_put(motor_def->dir2_pin, false); + + value = -value; + } + else if(value > 0) + { + gpio_put(motor_def->dir1_pin, false); + gpio_put(motor_def->dir2_pin, true); + } + else + { + gpio_put(motor_def->dir1_pin, false); + gpio_put(motor_def->dir2_pin, false); + } + + pwm_set_gpio_level(motor_def->pwm_pin, (uint16_t)value); +} + +// Set servo motor to its open pos +void servo_motor_zero(servo_motors_enum_t servo_motor) +{ + const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor]; + + // Set PWM to zero // + pwm_set_gpio_level(servo_motor_def->pwm_pin, servo_motor_def->open_pos); +} + +// Set servo to its close pos if [close] else open pos +void servo_motor_set(servo_motors_enum_t servo_motor, bool close) +{ + const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor]; + + // Set PWM to zero // + pwm_set_gpio_level(servo_motor_def->pwm_pin, close ? servo_motor_def->close_pos : servo_motor_def->open_pos); +} diff --git a/main controller code/src/robot.c b/main controller code/src/robot.c new file mode 100644 index 0000000..e1c3a85 --- /dev/null +++ b/main controller code/src/robot.c @@ -0,0 +1,26 @@ +#include +#include "include/motors.h" +#include "include/i2c_slave.h" + +#include "include/robot.h" + +void robot_init(void) +{ + stdio_init_all(); + + init_motors(); + init_servo_motors(); + i2c_slave_init(); + + robot.is_running = true; +} + +void robot_handle_inputs_outputs(void) +{ + update_motors_from_buffer(); +} + +void robot_deinit(void) +{ + i2c_slave_deinit(); +} \ No newline at end of file