commit 8b8a83947fb21821b7483d981e95f3c4bd564070 Author: Ulysse Cura Date: Wed Oct 1 08:48:45 2025 +0200 Starting new pcb and copying code from last year main robot diff --git a/PCB/main pcb.kicad_prl b/PCB/main pcb.kicad_prl new file mode 100644 index 0000000..5a85818 --- /dev/null +++ b/PCB/main pcb.kicad_prl @@ -0,0 +1,97 @@ +{ + "board": { + "active_layer": 0, + "active_layer_preset": "", + "auto_track_width": true, + "hidden_netclasses": [], + "hidden_nets": [], + "high_contrast_mode": 0, + "net_color_mode": 1, + "opacity": { + "images": 0.6, + "pads": 1.0, + "shapes": 1.0, + "tracks": 1.0, + "vias": 1.0, + "zones": 0.6 + }, + "selection_filter": { + "dimensions": true, + "footprints": true, + "graphics": true, + "keepouts": true, + "lockedItems": false, + "otherItems": true, + "pads": true, + "text": true, + "tracks": true, + "vias": true, + "zones": true + }, + "visible_items": [ + "vias", + "footprint_text", + "footprint_anchors", + "ratsnest", + "grid", + "footprints_front", + "footprints_back", + "footprint_values", + "footprint_references", + "tracks", + "drc_errors", + "drawing_sheet", + "bitmaps", + "pads", + "zones", + "drc_warnings", + "locked_item_shadows", + "conflict_shadows", + "shapes" + ], + "visible_layers": "ffffffff_ffffffff_ffffffff_ffffffff", + "zone_display_mode": 0 + }, + "git": { + "repo_type": "", + "repo_username": "", + "ssh_key": "" + }, + "meta": { + "filename": "main pcb.kicad_prl", + "version": 5 + }, + "net_inspector_panel": { + "col_hidden": [], + "col_order": [], + "col_widths": [], + "custom_group_rules": [], + "expanded_rows": [], + "filter_by_net_name": true, + "filter_by_netclass": true, + "filter_text": "", + "group_by_constraint": false, + "group_by_netclass": false, + "show_unconnected_nets": false, + "show_zero_pad_nets": false, + "sort_ascending": true, + "sorting_column": -1 + }, + "open_jobsets": [], + "project": { + "files": [] + }, + "schematic": { + "selection_filter": { + "graphics": true, + "images": true, + "labels": true, + "lockedItems": false, + "otherItems": true, + "pins": true, + "symbols": true, + "text": true, + "wires": true + } + } +} diff --git a/PCB/main pcb.kicad_pro b/PCB/main pcb.kicad_pro new file mode 100644 index 0000000..6440a30 --- /dev/null +++ b/PCB/main pcb.kicad_pro @@ -0,0 +1,83 @@ +{ + "board": { + "3dviewports": [], + "design_settings": { + "defaults": {}, + "diff_pair_dimensions": [], + "drc_exclusions": [], + "rules": {}, + "track_widths": [], + "via_dimensions": [] + }, + "ipc2581": { + "dist": "", + "distpn": "", + "internal_id": "", + "mfg": "", + "mpn": "" + }, + "layer_pairs": [], + "layer_presets": [], + "viewports": [] + }, + "boards": [], + "cvpcb": { + "equivalence_files": [] + }, + "libraries": { + "pinned_footprint_libs": [], + "pinned_symbol_libs": [] + }, + "meta": { + "filename": "main pcb.kicad_pro", + "version": 3 + }, + "net_settings": { + "classes": [ + { + "bus_width": 12, + "clearance": 0.2, + "diff_pair_gap": 0.25, + "diff_pair_via_gap": 0.25, + "diff_pair_width": 0.2, + "line_style": 0, + "microvia_diameter": 0.3, + "microvia_drill": 0.1, + "name": "Default", + "pcb_color": "rgba(0, 0, 0, 0.000)", + "priority": 2147483647, + "schematic_color": "rgba(0, 0, 0, 0.000)", + "track_width": 0.2, + "via_diameter": 0.6, + "via_drill": 0.3, + "wire_width": 6 + } + ], + "meta": { + "version": 4 + }, + "net_colors": null, + "netclass_assignments": null, + "netclass_patterns": [] + }, + "pcbnew": { + "last_paths": { + "gencad": "", + "idf": "", + "netlist": "", + "plot": "", + "pos_files": "", + "specctra_dsn": "", + "step": "", + "svg": "", + "vrml": "" + }, + "page_layout_descr_file": "" + }, + "schematic": { + "legacy_lib_dir": "", + "legacy_lib_list": [] + }, + "sheets": [], + "text_variables": {} +} diff --git a/pcb/main board/main board.kicad_pcb b/pcb/main board/main board.kicad_pcb new file mode 100644 index 0000000..c547853 --- /dev/null +++ b/pcb/main board/main board.kicad_pcb @@ -0,0 +1,2 @@ +(kicad_pcb (version 20241229) (generator "pcbnew") (generator_version "9.0") +) \ No newline at end of file diff --git a/pcb/main board/main board.kicad_pro b/pcb/main board/main board.kicad_pro new file mode 100644 index 0000000..f8c65e3 --- /dev/null +++ b/pcb/main board/main board.kicad_pro @@ -0,0 +1,32 @@ +{ + "board": { + "design_settings": { + "defaults": {}, + "diff_pair_dimensions": [], + "drc_exclusions": [], + "rules": {}, + "track_widths": [], + "via_dimensions": [] + } + }, + "boards": [], + "libraries": { + "pinned_footprint_libs": [], + "pinned_symbol_libs": [] + }, + "meta": { + "filename": "kicad.kicad_pro", + "version": 1 + }, + "net_settings": { + "classes": [], + "meta": { + "version": 0 + } + }, + "pcbnew": { + "page_layout_descr_file": "" + }, + "sheets": [], + "text_variables": {} +} diff --git a/pcb/main board/main board.kicad_sch b/pcb/main board/main board.kicad_sch new file mode 100644 index 0000000..2d7aad0 --- /dev/null +++ b/pcb/main board/main board.kicad_sch @@ -0,0 +1,14 @@ +(kicad_sch + (version 20250114) + (generator "eeschema") + (generator_version "9.0") + (uuid 2b41feff-4875-412f-ac81-a28835b7fe51) + (paper "A4") + (lib_symbols) + (sheet_instances + (path "/" + (page "1") + ) + ) + (embedded_fonts no) +) \ No newline at end of file diff --git a/pcb/main board/~main board.kicad_sch.lck b/pcb/main board/~main board.kicad_sch.lck new file mode 100644 index 0000000..59fb61b --- /dev/null +++ b/pcb/main board/~main board.kicad_sch.lck @@ -0,0 +1 @@ +{"hostname":"aspire-laptop","username":"ulysse-cura"} \ No newline at end of file diff --git a/program/main controller code/.vscode/c_cpp_properties.json b/program/main controller code/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..643d42c --- /dev/null +++ b/program/main controller code/.vscode/c_cpp_properties.json @@ -0,0 +1,33 @@ +{ + "env": { + "myDefaultIncludePath": [ + "${workspaceFolder}/src/include/", + "${env:PICO_SDK_PATH}/src/**/include/", + "${env:PICO_SDK_PATH}/lib/**/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/program/main controller code/.vscode/settings.json b/program/main controller code/.vscode/settings.json new file mode 100644 index 0000000..cb0aebb --- /dev/null +++ b/program/main controller code/.vscode/settings.json @@ -0,0 +1,19 @@ +{ + "files.associations": { + "*.md": "markdown", + "binary_info.h": "c", + "i2c.h": "c", + "stdlib.h": "c", + "stdint.h": "c", + "gyro.h": "c", + "motors.h": "c", + "motion_control.h": "c", + "i2c_master.h": "c", + "udp_client.h": "c", + "udp_payload.h": "c", + "udp.h": "c", + "opt.h": "c", + "cyw43_arch.h": "c", + "wifi_operator.h": "c" + } +} \ No newline at end of file diff --git a/program/main controller code/.vscode/tasks.json b/program/main controller code/.vscode/tasks.json new file mode 100644 index 0000000..1255bf5 --- /dev/null +++ b/program/main controller code/.vscode/tasks.json @@ -0,0 +1,25 @@ +{ + "tasks": [ + { + "type": "shell", + "command": "cd build; cmake -DPICO_BOARD=pico_w ..; make", + "label": "CMake in build/", + "problemMatcher": [], + "group": { + "kind": "build", + "isDefault": false + } + }, + { + "type": "shell", + "command": "cd build; cmake -DPICO_BOARD=pico_w ..; 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/program/main controller code/CMakeLists.txt b/program/main controller code/CMakeLists.txt new file mode 100644 index 0000000..e3b07dc --- /dev/null +++ b/program/main controller code/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.13) + +include(pico_sdk_import.cmake) + +project(main_controller C CXX ASM) +set(CMAKE_C_STANDARD 11) +set(CMAKE_CXX_STANDARD 17) + +set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR}) + +# Définir explicitement la carte comme Pico W +set(PICO_BOARD pico_w) +if(NOT DEFINED PICO_BOARD) + add_definitions(-DPICO_BOARD=${PICO_BOARD}) +endif() + +pico_sdk_init() + +add_executable(main_controller + src/main.c + src/robot.c + src/motors.c + src/gyro.c + src/motion_control.c + src/i2c_master.c + src/wifi_operator.c + src/udp_client.c + src/udp_payload.c +) + +target_include_directories(main_controller PRIVATE + ${CMAKE_CURRENT_LIST_DIR}/src + ${CMAKE_CURRENT_LIST_DIR}/src/include +) + +target_link_libraries(main_controller + hardware_i2c + hardware_pwm + hardware_uart + pico_stdlib + pico_cyw43_arch_lwip_poll +) + +pico_enable_stdio_usb(main_controller 1) +pico_enable_stdio_uart(main_controller 1) + +pico_add_extra_outputs(main_controller) + +add_custom_target(Flash + DEPENDS main_controller + COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/main_controller.uf2 +) diff --git a/program/main controller code/Readme.md b/program/main controller code/Readme.md new file mode 100644 index 0000000..76447b8 --- /dev/null +++ b/program/main controller code/Readme.md @@ -0,0 +1,95 @@ +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. + +|Register |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. + + +Internet communication description +----------------------------------------------- + +The robot main_controller is a client connected to the wireless controller which is an udp server host. A buffer is used to store data received from host. + +Speed on X and Y axis are not depending of the robot orientation. + +Servo motors keep the same byte address in i2c buffer and udp payload. + +|Byte |Description |Encoding | +|---------|-------------------------------------------------|:-----------------:| +| 0x00-01 | Robot angle (0x00 is the last significant byte) |**-180** - **180** | +| 0x02 | Speed x axis |**-128** - **127** | +| 0x03 | Speed y axis |**-128** - **127** | +| 0x04 | Servo 1 position selection | **0** - **1** | +| 0x05 | Servo 2 position selection | **0** - **1** | +| 0x06 | Servo 3 position selection | **0** - **1** | +| 0x07 | Servo 4 position selection | **0** - **1** | + + +Pinout description +----------------------------------------------- + +|Pin |Description |GPIO Type | +|----|----------------------------------|-----------| +| 4 | I2C Bus SDA | I2C | +| 5 | I2C Bus SCL | I2C | + + +Motors placement +----------------------------------------------- + + ,-~***~-, + /1 2\ + | | + | | + | | + \3 4/ + `-.....-' diff --git a/program/main controller code/pico_sdk_import.cmake b/program/main controller code/pico_sdk_import.cmake new file mode 100644 index 0000000..28efe9e --- /dev/null +++ b/program/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/program/main controller code/src/gyro.c b/program/main controller code/src/gyro.c new file mode 100644 index 0000000..87c92ec --- /dev/null +++ b/program/main controller code/src/gyro.c @@ -0,0 +1,135 @@ +#include "include/gyro.h" + +#include +#include "include/i2c_master.h" +#include "include/robot.h" + +#include + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\ +* Thank you Keuronde ! * +* https://git.poivron-robotique.fr/Keuronde/Holonome_2024/src/branch/Demo_2025_03/gyro_L3GD20H.c * +\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + +#define SAMPLE_MIN_ELAPSED_TIME 2 // ms +#define DPS_PER_DIGIT 0.00875f + +int init_gyro(void) +{ + // Verify gyro initialisation + uint8_t data; + i2c_master_read_reg(I2C_GYRO_ADDRESS, 0x0F, &data, 1); + + if(data != 0xd7) return -1; + + // Configure gyro + const uint8_t CTRL1_REG = 0x20; + const uint8_t CTRL1_CONFIG = 0b11101111; // DR : 11 // BW : 10 // PD : 1 // Zen : 1 // Xen : 1 // Yen : 1 // + + const uint8_t CONFIG[] = {CTRL1_REG, CTRL1_CONFIG}; + i2c_master_write(I2C_GYRO_ADDRESS, CONFIG, 2); + + uint8_t config_verification; + i2c_master_read_reg(I2C_GYRO_ADDRESS, CTRL1_REG, &config_verification, 1); + + if(config_verification != config[1]) return -1; + + robot.gyro_data.x_angle = 0.0f; + robot.gyro_data.y_angle = 0.0f; + robot.gyro_data.z_angle = 0.0f; + + sleep_ms(1000); + + return 0; +} + +static inline void __attribute__((always_inline)) gyro_read(int16_t *x, int16_t *y, int16_t *z) +{ + const uint8_t X_OUT_L_REG = 0x28; + + uint8_t data[6]; + i2c_master_read_reg(I2C_GYRO_ADDRESS, X_OUT_L_REG | 0x80, data, 6); // 0x80 for auto incrementing + + *x = (int16_t)((data[1] << 8) | data[0]); + *y = (int16_t)((data[3] << 8) | data[2]); + *z = (int16_t)((data[5] << 8) | data[4]); +} + +void gyro_calibrate(void) +{ + const uint nb_samples = 1000; + + int16_t x, y, z; + int32_t x_sum = 0, y_sum = 0, z_sum = 0; + + for(uint i = 0; i < nb_samples; i++) + { + gyro_read(&x, &y, &z); + + x_sum += x; + y_sum += y; + z_sum += z; + + sleep_ms(SAMPLE_MIN_ELAPSED_TIME); + + //printf(">cal_x:%d\n", x); + //printf(">cal_y:%d\n", y); + //printf(">cal_z:%d\n", z); + } + + robot.gyro_data.x_offset = (float)x_sum / (float)nb_samples * DPS_PER_DIGIT; + robot.gyro_data.y_offset = (float)y_sum / (float)nb_samples * DPS_PER_DIGIT; + robot.gyro_data.z_offset = (float)z_sum / (float)nb_samples * DPS_PER_DIGIT; + + //printf("\nx_cal:%.5f\n", robot.gyro_data.x_offset); + //printf("\ny_cal:%.5f\n", robot.gyro_data.y_offset); + //printf("\nz_cal:%.5f\n", robot.gyro_data.z_offset); +} + +static inline void __attribute__((always_inline)) gyro_get_dps(double* x_dps, double* y_dps, double* z_dps) +{ + int16_t x, y, z; + gyro_read(&x, &y, &z); + + *x_dps = x * DPS_PER_DIGIT; + *y_dps = y * DPS_PER_DIGIT; + *z_dps = z * DPS_PER_DIGIT; + + //printf(">x_dps:%f\n", *x_dps); + //printf(">y_dps:%f\n", *y_dps); + //printf(">z_dps:%f\n", *z_dps); +} + +void gyro_update(void) +{ + static double elapsed_since_sample_ms = 10.0; + + elapsed_since_sample_ms += robot.delta_time_ms; + + if(elapsed_since_sample_ms >= SAMPLE_MIN_ELAPSED_TIME) + { + double x_dps, y_dps, z_dps; + gyro_get_dps(&x_dps, &y_dps, &z_dps); + + x_dps -= robot.gyro_data.x_offset; + y_dps -= robot.gyro_data.y_offset; + z_dps -= robot.gyro_data.z_offset; + + robot.gyro_data.x_angle += x_dps * elapsed_since_sample_ms / 1000.0f; + robot.gyro_data.y_angle += y_dps * elapsed_since_sample_ms / 1000.0f; + robot.gyro_data.z_angle += z_dps * elapsed_since_sample_ms / 1000.0f; + + while(robot.gyro_data.x_angle > 180) robot.gyro_data.x_angle -= 360; + while(robot.gyro_data.x_angle < -180) robot.gyro_data.x_angle += 360; + while(robot.gyro_data.y_angle > 180) robot.gyro_data.y_angle -= 360; + while(robot.gyro_data.y_angle < -180) robot.gyro_data.y_angle += 360; + while(robot.gyro_data.z_angle > 180) robot.gyro_data.z_angle -= 360; + while(robot.gyro_data.z_angle < -180) robot.gyro_data.z_angle += 360; + + //printf(">gyro_x_angle:%f\n", robot.gyro_data.x_angle); + //printf(">gyro_y_angle:%f\n", robot.gyro_data.y_angle); + //printf(">gyro_z_angle:%f\n", robot.gyro_data.z_angle); + + elapsed_since_sample_ms = 0.0; + } +} diff --git a/program/main controller code/src/i2c_master.c b/program/main controller code/src/i2c_master.c new file mode 100644 index 0000000..b8fa79e --- /dev/null +++ b/program/main controller code/src/i2c_master.c @@ -0,0 +1,36 @@ +#include "include/i2c_master.h" + +#include +#include + +void i2c_master_init(void) +{ + gpio_set_function(I2C_MASTER_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C_MASTER_SCL_PIN, GPIO_FUNC_I2C); + + gpio_pull_up(I2C_MASTER_SDA_PIN); + gpio_pull_up(I2C_MASTER_SCL_PIN); + + i2c_init(I2C_MASTER_INSTANCE, I2C_MASTER_BAUD_RATE); +} + +void i2c_master_deinit(void) +{ + i2c_deinit(I2C_MASTER_INSTANCE); +} + +inline void i2c_master_write(uint8_t address, const uint8_t *src, size_t len) +{ + i2c_write_blocking(I2C_MASTER_INSTANCE, address, src, len, false); +} + +inline void i2c_master_read(uint8_t address, uint8_t *dst, size_t len) +{ + i2c_read_blocking(I2C_MASTER_INSTANCE, address, dst, len, false); +} + +inline void i2c_master_read_reg(uint8_t address, uint8_t reg, uint8_t *dst, size_t len) +{ + i2c_master_write(address, ®, 1); + i2c_master_read(address, dst, len); +} \ No newline at end of file diff --git a/program/main controller code/src/include/gyro.h b/program/main controller code/src/include/gyro.h new file mode 100644 index 0000000..77c7faf --- /dev/null +++ b/program/main controller code/src/include/gyro.h @@ -0,0 +1,18 @@ +#ifndef GYRO_H +#define GYRO_H + +#define I2C_GYRO_ADDRESS 0x6b + +typedef struct gyro_data_t { + float x_offset, y_offset, z_offset; + float x_angle, y_angle, z_angle; +} gyro_data_t; + +// Check if gyro has correctly initialised and configure it for simple use +int init_gyro(void); +// Calibrate gyro +void gyro_calibrate(void); +// Update gyro data +void gyro_update(void); + +#endif // GYRO_H \ No newline at end of file diff --git a/program/main controller code/src/include/i2c_master.h b/program/main controller code/src/include/i2c_master.h new file mode 100644 index 0000000..1a0ec0f --- /dev/null +++ b/program/main controller code/src/include/i2c_master.h @@ -0,0 +1,22 @@ +#ifndef I2C_MASTER_H +#define I2C_MASTER_H + +#include + +#define I2C_MASTER_SDA_PIN 4 +#define I2C_MASTER_SCL_PIN 5 +#define I2C_MASTER_INSTANCE i2c0 +#define I2C_MASTER_BAUD_RATE 300 * 1000 + +// Init master i2c +void i2c_master_init(void); +// Deinit master i2c +void i2c_master_deinit(void); +// Send [src] of [len] to [address] and close communication +void i2c_master_write(uint8_t address, const uint8_t *src, size_t len); +// Receive [dst] of [len] from [address] and close communication +void i2c_master_read(uint8_t address, uint8_t *dst, size_t len); +// Send [reg] and receive data in [dst] and close communition +void i2c_master_read_reg(uint8_t address, uint8_t reg, uint8_t *dst, size_t len); + +#endif // I2C_MASTER_H \ No newline at end of file diff --git a/program/main controller code/src/include/lwipopts.h b/program/main controller code/src/include/lwipopts.h new file mode 100644 index 0000000..91c3b05 --- /dev/null +++ b/program/main controller code/src/include/lwipopts.h @@ -0,0 +1,97 @@ +#ifndef _LWIPOPTS_EXAMPLE_COMMONH_H +#define _LWIPOPTS_EXAMPLE_COMMONH_H + + +// Common settings used in most of the pico_w examples +// (see https://www.nongnu.org/lwip/2_1_x/group__lwip__opts.html for details) + +// allow override in some examples +#ifndef NO_SYS +#define NO_SYS 1 +#endif +// allow override in some examples +#ifndef LWIP_SOCKET +#define LWIP_SOCKET 0 +#endif +#if PICO_CYW43_ARCH_POLL +#define MEM_LIBC_MALLOC 1 +#else +// MEM_LIBC_MALLOC is incompatible with non polling versions +#define MEM_LIBC_MALLOC 0 +#endif +#define MEM_ALIGNMENT 4 +#ifndef MEM_SIZE +#define MEM_SIZE 32768 // Augmenté pour plus de mémoire disponible +#endif +#define MEMP_NUM_TCP_SEG 32 +#define MEMP_NUM_ARP_QUEUE 10 +#define PBUF_POOL_SIZE 32 // Augmenté pour réduire les allocations +#define LWIP_ARP 1 +#define LWIP_ETHERNET 1 +#define LWIP_ICMP 1 +#define LWIP_RAW 1 +#define TCP_WND (16 * TCP_MSS) // Augmenté pour de meilleures performances +#define TCP_MSS 1460 +#define TCP_SND_BUF (8 * TCP_MSS) // Augmenté pour de meilleures performances +#define TCP_SND_QUEUELEN ((4 * (TCP_SND_BUF) + (TCP_MSS - 1)) / (TCP_MSS)) +#define LWIP_NETIF_STATUS_CALLBACK 1 +#define LWIP_NETIF_LINK_CALLBACK 1 +#define LWIP_NETIF_HOSTNAME 1 +#define LWIP_NETCONN 0 +#define MEM_STATS 0 +#define SYS_STATS 0 +#define MEMP_STATS 0 +#define LINK_STATS 0 +// #define ETH_PAD_SIZE 2 +#define LWIP_CHKSUM_ALGORITHM 3 +#define LWIP_DHCP 0 +#define LWIP_DHCP_SERVER 0 +#define LWIP_IPV4 1 +#define LWIP_TCP 1 +#define LWIP_UDP 1 +#define LWIP_DNS 1 +#define LWIP_TCP_KEEPALIVE 1 +#define LWIP_NETIF_TX_SINGLE_PBUF 1 +#define DHCP_DOES_ARP_CHECK 0 +#define LWIP_DHCP_DOES_ACD_CHECK 0 + +#ifndef NDEBUG +#define LWIP_DEBUG 1 +#define LWIP_STATS 1 +#define LWIP_STATS_DISPLAY 1 +#endif + +#define ETHARP_DEBUG LWIP_DBG_OFF +#define NETIF_DEBUG LWIP_DBG_OFF +#define PBUF_DEBUG LWIP_DBG_OFF +#define API_LIB_DEBUG LWIP_DBG_OFF +#define API_MSG_DEBUG LWIP_DBG_OFF +#define SOCKETS_DEBUG LWIP_DBG_OFF +#define ICMP_DEBUG LWIP_DBG_OFF +#define INET_DEBUG LWIP_DBG_OFF +#define IP_DEBUG LWIP_DBG_OFF +#define IP_REASS_DEBUG LWIP_DBG_OFF +#define RAW_DEBUG LWIP_DBG_OFF +#define MEM_DEBUG LWIP_DBG_OFF +#define MEMP_DEBUG LWIP_DBG_OFF +#define SYS_DEBUG LWIP_DBG_OFF +#define TCP_DEBUG LWIP_DBG_OFF +#define TCP_INPUT_DEBUG LWIP_DBG_OFF +#define TCP_OUTPUT_DEBUG LWIP_DBG_OFF +#define TCP_RTO_DEBUG LWIP_DBG_OFF +#define TCP_CWND_DEBUG LWIP_DBG_OFF +#define TCP_WND_DEBUG LWIP_DBG_OFF +#define TCP_FR_DEBUG LWIP_DBG_OFF +#define TCP_QLEN_DEBUG LWIP_DBG_OFF +#define TCP_RST_DEBUG LWIP_DBG_OFF +#define UDP_DEBUG LWIP_DBG_OFF +#define TCPIP_DEBUG LWIP_DBG_OFF +#define PPP_DEBUG LWIP_DBG_OFF +#define SLIP_DEBUG LWIP_DBG_OFF +#define DHCP_DEBUG LWIP_DBG_OFF + +#define SYS_LIGHTWEIGHT_PROT 1 // Protection pour le multicore +#define MEMP_NUM_PBUF 32 // Augmenté pour les buffers +#define ICMP_TTL 255 // Augmenté pour la fiabilité + +#endif /* __LWIPOPTS_H__ */ \ No newline at end of file diff --git a/program/main controller code/src/include/motion_control.h b/program/main controller code/src/include/motion_control.h new file mode 100644 index 0000000..0363efb --- /dev/null +++ b/program/main controller code/src/include/motion_control.h @@ -0,0 +1,21 @@ +#ifndef MOTION_CONTROL_H +#define MOTION_CONTROL_H + +#include +#include "motors.h" + +typedef struct motion_control_data_t { + int16_t angle; + int8_t x_axis_speed; + int8_t y_axis_speed; + uint8_t servo_motors_pos[NB_SERVO_MOTORS]; +} motion_control_data_t; + +// Init values for motion control +void init_motion_control(void); +// Update motion control buffer from motion control data and gyro data +void i2c_update_motion_control(void); +// Update servo motors from motion control data +void i2c_update_servo_motors(void); + +#endif // MOTION_CONTROL_H \ No newline at end of file diff --git a/program/main controller code/src/include/motors.h b/program/main controller code/src/include/motors.h new file mode 100644 index 0000000..dc71d1f --- /dev/null +++ b/program/main controller code/src/include/motors.h @@ -0,0 +1,55 @@ +#ifndef MOTORS_H +#define MOTORS_H + +#include + +#define I2C_MOTION_CONTROLLER_ADDRESS 0x09 + +// 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_reg; +} 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_reg_and_payload_byte; +} servo_motor_def_t; + +extern const servo_motor_def_t SERVO_MOTORS_DEFS[]; + +// Send [motor] to [value] through i2c to motion controller +void i2c_set_motor(motors_enum_t motor, int8_t value); +// Get [motor] speed from motion controller through i2c +int8_t i2c_get_motor(motors_enum_t motor); +// Set [servo motor] to [value] through i2c +void i2c_set_servo_motor(servo_motors_enum_t servo_motor, uint8_t value); +// Get [servo motor] value from i2c +uint8_t i2c_get_servo_motor(servo_motors_enum_t servo_motor); + +#endif // MOTORS_H diff --git a/program/main controller code/src/include/robot.h b/program/main controller code/src/include/robot.h new file mode 100644 index 0000000..61fe232 --- /dev/null +++ b/program/main controller code/src/include/robot.h @@ -0,0 +1,25 @@ +#ifndef ROBOT_H +#define ROBOT_H + +#include +#include "gyro.h" +#include "motion_control.h" + +typedef struct robot_t { + gyro_data_t gyro_data; + motion_control_data_t motion_control_data; + + bool is_running; + double delta_time_ms; +} robot_t; + +extern robot_t robot; + +// Init all robot's components +int 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/program/main controller code/src/include/udp_client.h b/program/main controller code/src/include/udp_client.h new file mode 100644 index 0000000..7a9bea2 --- /dev/null +++ b/program/main controller code/src/include/udp_client.h @@ -0,0 +1,28 @@ +#ifndef UDP_CLIENT_H +#define UDP_CLIENT_H + +#include +#include + +#define UDP_CLIENT_PORT 4243 + +#define BUFFER_SIZE 1024 + +// Message callback deffinition +typedef void (*message_callback_t)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, uint16_t port); + +// Data in here is used by the hardware +typedef struct udp_client_t { + struct udp_pcb *pcb; // Like this + ip_addr_t local_addr; // Or this + uint16_t local_port; // So don't remove them, even if they are not used explicitely in the program + uint8_t recv_buffer[BUFFER_SIZE]; // Please (Not even change their position) + message_callback_t message_callback; +} udp_client_t; + +// Init udp client, set callback to NULL for the default callback +void udp_client_init(void); +// Exit udp client +void udp_client_exit(void); + +#endif // UDP_CLIENT_H \ No newline at end of file diff --git a/program/main controller code/src/include/udp_payload.h b/program/main controller code/src/include/udp_payload.h new file mode 100644 index 0000000..4d7097a --- /dev/null +++ b/program/main controller code/src/include/udp_payload.h @@ -0,0 +1,16 @@ +#ifndef UDP_BUFFER_H +#define UDP_BUFFER_H + +#include +#include +#include + +#define UDP_PAYLOAD_ANGLE_L_BYTE 0x00 +#define UDP_PAYLOAD_ANGLE_H_BYTE 0x01 +#define UDP_PAYLOAD_X_AXIS_SPEED_BYTE 0x02 +#define UDP_PAYLOAD_Y_AXIS_SPEED_BYTE 0x03 + +// Callback function for writing data when udp package received +void __not_in_flash_func(udp_client_message_handler)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, u16_t port); + +#endif // UDP_BUFFER_H \ No newline at end of file diff --git a/program/main controller code/src/include/wifi_operator.h b/program/main controller code/src/include/wifi_operator.h new file mode 100644 index 0000000..ecf22f5 --- /dev/null +++ b/program/main controller code/src/include/wifi_operator.h @@ -0,0 +1,9 @@ +#ifndef WIFI_OPERATOR_H +#define WIFI_OPERATOR_H + +#define WIFI_OPERATOR_SSID "RiombotiqueAP" +#define WIFI_OPERATOR_PASSWORD "x4ptSLpPuJFcpzbLEhDoZ5J7dz" + +void wifi_operator_init(void); + +#endif // WIFI_OPERATOR_H \ No newline at end of file diff --git a/program/main controller code/src/main.c b/program/main controller code/src/main.c new file mode 100644 index 0000000..aaa0d49 --- /dev/null +++ b/program/main controller code/src/main.c @@ -0,0 +1,27 @@ +/* *\ + Copyrights 2025 + Riombotique +\* */ + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\ +* 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/robot.h" + +robot_t robot; + +int main(void) +{ + if(robot_init()) return -1; + + while(robot.is_running) + { + robot_handle_inputs_outputs(); + } + + robot_deinit(); + + return 0; +} diff --git a/program/main controller code/src/motion_control.c b/program/main controller code/src/motion_control.c new file mode 100644 index 0000000..7ea6e1c --- /dev/null +++ b/program/main controller code/src/motion_control.c @@ -0,0 +1,67 @@ +#include "include/motion_control.h" + +#include +#include +#include "include/motors.h" +#include "include/robot.h" + +#define GAIN_KD 10 + +void init_motion_control(void) +{ + robot.motion_control_data.angle = 0; + robot.motion_control_data.x_axis_speed = 0; + robot.motion_control_data.y_axis_speed = 0; +} + +void i2c_update_motion_control(void) +{ + // Motion control work as follow : + // - Motors are rotated on-board at 45*. + // - we calculate the error of the targeted angle and the actual angle + // - First we estimate the targeted speed on irl board on X and Y axis + // - Then we calculate motors speed from targeted speed and the error + // - And we put limits because motors speed are send by i2c on 1 byte + + float actual_angle = robot.gyro_data.x_angle - 45.0f; + + float target_angle = (float)robot.motion_control_data.angle - 45.0f; + float target_angle_radian = target_angle / 180.0f * M_PI; + + float error = target_angle - actual_angle; + if(error > 180) error -= 360; + if(error < -180) error += 360; + + float correction = error * GAIN_KD; + + float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed + + sinf(target_angle_radian) * robot.motion_control_data.y_axis_speed; + float target_y_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.y_axis_speed - + sinf(target_angle_radian) * robot.motion_control_data.x_axis_speed; + + int motor1_speed = target_x_axis_speed + (int)correction; + int motor2_speed = target_x_axis_speed - (int)correction; + int motor3_speed = target_y_axis_speed + (int)correction; + int motor4_speed = target_y_axis_speed - (int)correction; + + if(motor1_speed > 127) motor1_speed = 127; + if(motor2_speed > 127) motor2_speed = 127; + if(motor3_speed > 127) motor3_speed = 127; + if(motor4_speed > 127) motor4_speed = 127; + + if(motor1_speed < -128) motor1_speed = -128; + if(motor2_speed < -128) motor2_speed = -128; + if(motor3_speed < -128) motor3_speed = -128; + if(motor4_speed < -128) motor4_speed = -128; + + i2c_set_motor(MOTOR1, motor1_speed); + i2c_set_motor(MOTOR2, motor2_speed); + i2c_set_motor(MOTOR3, motor3_speed); + i2c_set_motor(MOTOR4, motor4_speed); +} + +void i2c_update_servo_motors(void) +{ + for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++) + i2c_set_servo_motor(actual_servo_motor, robot.motion_control_data.servo_motors_pos[actual_servo_motor]); +} diff --git a/program/main controller code/src/motors.c b/program/main controller code/src/motors.c new file mode 100644 index 0000000..617e313 --- /dev/null +++ b/program/main controller code/src/motors.c @@ -0,0 +1,58 @@ +#include "include/motors.h" + +#include +#include "include/i2c_master.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}, +}; + +void i2c_set_motor(motors_enum_t motor, int8_t value) +{ + const motor_def_t *MOTOR_DEF = &MOTORS_DEFS[motor]; + + uint8_t x = *(uint8_t *)&value; + uint8_t data[] = {MOTOR_DEF->buffer_reg, x}; + + i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2); +} + +int8_t i2c_get_motor(motors_enum_t motor) +{ + const motor_def_t *MOTOR_DEF = &MOTORS_DEFS[motor]; + + uint8_t data; + i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, MOTOR_DEF->buffer_reg, &data, 1); + + int8_t value = *(int8_t *)&data; + return value; +} + +void i2c_set_servo_motor(servo_motors_enum_t servo_motor, uint8_t value) +{ + const servo_motor_def_t *SERVO_MOTOR_DEF = &SERVO_MOTORS_DEFS[servo_motor]; + + uint8_t data[] = {SERVO_MOTOR_DEF->buffer_reg_and_payload_byte, value}; + + i2c_master_write(I2C_MOTION_CONTROLLER_ADDRESS, data, 2); +} + +uint8_t i2c_get_servo_motor(servo_motors_enum_t servo_motor) +{ + const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor]; + + uint8_t value; + i2c_master_read_reg(I2C_MOTION_CONTROLLER_ADDRESS, servo_motor_def->buffer_reg_and_payload_byte, &value, 1); + + return value; +} diff --git a/program/main controller code/src/robot.c b/program/main controller code/src/robot.c new file mode 100644 index 0000000..f6eab3c --- /dev/null +++ b/program/main controller code/src/robot.c @@ -0,0 +1,84 @@ +#include "include/robot.h" + +#include +#include +#include +#include +#include "include/i2c_master.h" +#include "include/udp_client.h" +#include "include/wifi_operator.h" + +//#include "include/udp_fake_client.h" + +auto_init_mutex(wifi_mutex); + +int robot_init(void) +{ + stdio_init_all(); + + if(cyw43_arch_init()) + { + return -1; + } + + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1); + + wifi_operator_init(); + udp_client_init(); + + i2c_master_init(); + if(init_gyro()) return -1; + gyro_calibrate(); + + init_motion_control(); + + robot.is_running = true; + + return 0; +} + +static inline void update_time(void) +{ + static bool led_state = false; + static double last_time = 0.0; + double start_time = (double)clock() * 1000.0 / (double)CLOCKS_PER_SEC; + robot.delta_time_ms = start_time - last_time; + last_time = start_time; + + static double elapsed_time = 0.0; + elapsed_time += robot.delta_time_ms; + + if(elapsed_time >= 1000) + { + elapsed_time = 0; + + mutex_enter_blocking(&wifi_mutex); + cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_state); + mutex_exit(&wifi_mutex); + + led_state = !led_state; + } +} + +void robot_handle_inputs_outputs(void) +{ + mutex_enter_blocking(&wifi_mutex); + cyw43_arch_poll(); + mutex_exit(&wifi_mutex); + + update_time(); + + gyro_update(); + + i2c_update_motion_control(); + + i2c_update_servo_motors(); + + tight_loop_contents(); +} + +void robot_deinit(void) +{ + //udp_client_exit(); + i2c_master_deinit(); +} \ No newline at end of file diff --git a/program/main controller code/src/udp_client.c b/program/main controller code/src/udp_client.c new file mode 100644 index 0000000..9cc5258 --- /dev/null +++ b/program/main controller code/src/udp_client.c @@ -0,0 +1,71 @@ +#include "include/udp_client.h" + +#include +#include "include/udp_payload.h" + +udp_client_t udp_client; + +static inline void __not_in_flash_func(handle_receive)(struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + if(p->len >= 2) + { + uint8_t *payload = (uint8_t *)p->payload; + uint16_t len = p->len; + + udp_client.message_callback(payload, len, addr, port); + } + + pbuf_free(p); +} + +static void __not_in_flash_func(udp_receive_callback)(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) +{ + udp_client_t *udp_client_received_data = (udp_client_t *)arg; + handle_receive(p, addr, port); +} + +// Default callback func +static void __not_in_flash_func(default_message_callback)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, uint16_t port) +{ + //printf("Received: len=%d from %s:%d\n", len, ipaddr_ntoa(addr), port); + + //for(uint i = 0; i < len; i++) printf("payload[%d]=%d | ", i, payload[i]); + //puts("\n"); + + printf(">Robot angle : %d\n", (int16_t)((payload[UDP_PAYLOAD_ANGLE_H_BYTE] << 8) | payload[UDP_PAYLOAD_ANGLE_L_BYTE])); + printf(">Robot x speed : %d\n", (int8_t)payload[UDP_PAYLOAD_X_AXIS_SPEED_BYTE]); + printf(">Robot y speed : %d\n", (int8_t)payload[UDP_PAYLOAD_Y_AXIS_SPEED_BYTE]); +} + +void udp_client_init(void) +{ + //udp_client.message_callback = udp_client_message_handler; + udp_client.message_callback = default_message_callback; + + udp_client.pcb = udp_new(); + if(udp_client.pcb == NULL) + { + puts("Error creating UDP client"); + return; + } + + udp_recv(udp_client.pcb, udp_receive_callback, &udp_client); + + err_t err = udp_bind(udp_client.pcb, IP_ADDR_ANY, UDP_CLIENT_PORT); + if(err != ERR_OK) + { + printf("Erreur bind UDP client: %d\n", err); + return; + } + + printf("UDP client started on port %d\n", UDP_CLIENT_PORT); +} + +void udp_client_exit(void) +{ + if(udp_client.pcb) + { + udp_remove(udp_client.pcb); + udp_client.pcb = NULL; + } +} diff --git a/program/main controller code/src/udp_payload.c b/program/main controller code/src/udp_payload.c new file mode 100644 index 0000000..b4e0ad8 --- /dev/null +++ b/program/main controller code/src/udp_payload.c @@ -0,0 +1,21 @@ +#include "include/udp_payload.h" + +#include "include/motors.h" +#include "include/robot.h" + +void __not_in_flash_func(udp_client_message_handler)(uint8_t *payload, uint16_t len, const ip_addr_t *addr, u16_t port) +{ + if(len != 16) return; + + robot.motion_control_data.angle = ((payload[UDP_PAYLOAD_ANGLE_H_BYTE] << 8) | payload[UDP_PAYLOAD_ANGLE_L_BYTE]); + + robot.motion_control_data.x_axis_speed = payload[UDP_PAYLOAD_X_AXIS_SPEED_BYTE]; + robot.motion_control_data.y_axis_speed = payload[UDP_PAYLOAD_Y_AXIS_SPEED_BYTE]; + + 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]; + + robot.motion_control_data.servo_motors_pos[actual_servo_motor] = payload[SERVO_MOTOR_DEF->buffer_reg_and_payload_byte]; + } +} diff --git a/program/main controller code/src/wifi_operator.c b/program/main controller code/src/wifi_operator.c new file mode 100644 index 0000000..9831fb4 --- /dev/null +++ b/program/main controller code/src/wifi_operator.c @@ -0,0 +1,78 @@ +#include "include/wifi_operator.h" + +#include +#include +#include +#include + +void wifi_operator_init(void) +{ + // Mode client + cyw43_arch_enable_sta_mode(); + + // Désactiver le mode d'économie d'énergie + cyw43_wifi_pm(&cyw43_state, CYW43_NO_POWERSAVE_MODE); + + // Afficher les paramètres de connexion + printf("Tentative de connexion à: SSID='%s'\n", WIFI_OPERATOR_SSID); + + // Configuration IP + ip4_addr_t ip, netmask, gateway; + IP4_ADDR(&ip, 192, 168, 128, 2); + IP4_ADDR(&netmask, 255, 255, 255, 0); + IP4_ADDR(&gateway, 192, 168, 128, 1); + + // Configuration réseau avant connexion + netif_set_up(netif_default); + netif_set_addr(netif_default, &ip, &netmask, &gateway); + + puts("Configuration IP effectuée"); + + int connection_return = 1; + + // Tentativs de connexion + do + { + puts("Tentative de connexion au réseau Wi-Fi..."); + connection_return = cyw43_arch_wifi_connect_timeout_ms(WIFI_OPERATOR_SSID, WIFI_OPERATOR_PASSWORD, CYW43_AUTH_WPA2_AES_PSK, 10000); + + if(connection_return) + { + const char *error_description; + + switch(connection_return) + { + case -1: + error_description = "General error"; + break; + + case -2: + error_description = "Point d'accès non trouvé"; + break; + + case -3: + error_description = "Incorrect password"; + break; + + default: + error_description = "Unknow error"; + } + + printf("Error: WiFi can't be connected - Error code: %d - %s\n", connection_return, error_description); + } + } + while(connection_return); + + // Configuration de l'interface réseau + if(!netif_default) + { + puts("Error: WiFi interface isn't accessible"); + return; + } + + netif_set_up(netif_default); + netif_set_link_up(netif_default); + netif_set_addr(netif_default, &ip, &netmask, &gateway); + + puts("Connexion successfully etablished !"); +} diff --git a/program/motion controller code/.vscode/c_cpp_properties.json b/program/motion controller code/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..8cc4ce6 --- /dev/null +++ b/program/motion 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}", + "${workspaceFolder}/build/" + ], + "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/program/motion controller code/.vscode/settings.json b/program/motion controller code/.vscode/settings.json new file mode 100644 index 0000000..9fcb311 --- /dev/null +++ b/program/motion controller code/.vscode/settings.json @@ -0,0 +1,13 @@ +{ + "files.associations": { + "*.md": "markdown", + "binary_info.h": "c", + "i2c.h": "c", + "time.h": "c", + "stdlib.h": "c", + "robot.h": "c", + "stdio.h": "c", + "i2c_buffer.h": "c", + "motors.h": "c" + } +} \ No newline at end of file diff --git a/program/motion controller code/.vscode/tasks.json b/program/motion controller code/.vscode/tasks.json new file mode 100644 index 0000000..c98bf9b --- /dev/null +++ b/program/motion 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/program/motion controller code/CMakeLists.txt b/program/motion controller code/CMakeLists.txt new file mode 100644 index 0000000..2f024e1 --- /dev/null +++ b/program/motion controller code/CMakeLists.txt @@ -0,0 +1,36 @@ +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 + src/robot.c + src/motors.c + src/i2c_slave.c + src/i2c_buffer.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/program/motion controller code/Readme.md b/program/motion controller code/Readme.md new file mode 100644 index 0000000..bb0c0ef --- /dev/null +++ b/program/motion controller code/Readme.md @@ -0,0 +1,79 @@ +Motion controler code for the RPI Pico (RP2040) +=============================================== + +This project is a motion controller firmware for the RPI Pico (RP2040), designed for plug-and-play use with modular code and a clear architecture. + + +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 firmware is coded to be a slave when adressed. Its address is **0x09** which you can change if there is any conflict whit other hardware. + +|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 pos or the close pos. + + +Pinout description +----------------------------------------------- + +|Pin |Description |GPIO Type | +|----|----------------------------------|-----------| +| 0 | Motor1 Speed | PWM | +| 1 | Motor2 Speed | PWM | +| 2 | Motor3 Speed | PWM | +| 3 | Motor4 Speed | PWM | +| 4 | Motor1 Dir1 | OUTPUT | +| 5 | Motor1 Dir2 | OUTPUT | +| 6 | Motor2 Dir1 | OUTPUT | +| 7 | Motor2 Dir2 | OUTPUT | +| 8 | Motor3 Dir1 | OUTPUT | +| 9 | Motor3 Dir2 | OUTPUT | +| 10 | Motor4 Dir1 | OUTPUT | +| 11 | Motor5 Dir2 | OUTPUT | +| 12 | Servo1 Angle | PWM | +| 13 | Servo2 Angle | PWM | +| 14 | Servo3 Angle | PWM | +| 15 | Servo4 Angle | PWM | +| 21 | I2C Bus SDA | I2C | +| 20 | I2C Bus SCL | I2C | diff --git a/program/motion controller code/pico_sdk_import.cmake b/program/motion controller code/pico_sdk_import.cmake new file mode 100644 index 0000000..28efe9e --- /dev/null +++ b/program/motion 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/program/motion controller code/src/i2c_buffer.c b/program/motion controller code/src/i2c_buffer.c new file mode 100644 index 0000000..22d8000 --- /dev/null +++ b/program/motion controller code/src/i2c_buffer.c @@ -0,0 +1,58 @@ +#include "include/i2c_buffer.h" + +#include "include/robot.h" +#include "include/motors.h" + +void __not_in_flash_func(i2c_slave_buffer_handler)(i2c_slave_event_t event) +{ + switch(event) + { + case I2C_SLAVE_RECEIVE: // master has written some data + if(!robot.i2c_buffer.buffer_reg_written) + { + // writes always start with the memory address + robot.i2c_buffer.buffer_reg = i2c_slave_read_byte(); + robot.i2c_buffer.buffer_reg_written = true; + } + else + { + // save into memory + robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg] = i2c_slave_read_byte(); + robot.i2c_buffer.buffer_reg++; + } + break; + + case I2C_SLAVE_REQUEST: // master is requesting data + // load from memory + i2c_slave_write_byte(robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_reg]); + robot.i2c_buffer.buffer_reg++; + break; + + case I2C_SLAVE_FINISH: // master has signalled Stop / Restart + robot.i2c_buffer.buffer_reg_written = false; + break; + + default: + break; + } +} + +void update_motors_from_buffer(void) +{ + for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++) + { + const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor]; + + motor_set(actual_motor, robot.i2c_buffer.buffer[motor_def->buffer_reg]); + } +} + +void update_servo_motors_from_buffer(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]; + + servo_motor_set(actual_servo_motor, robot.i2c_buffer.buffer[servo_motor_def->buffer_reg]); + } +} \ No newline at end of file diff --git a/program/motion controller code/src/i2c_slave.c b/program/motion controller code/src/i2c_slave.c new file mode 100644 index 0000000..960e194 --- /dev/null +++ b/program/motion controller code/src/i2c_slave.c @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2021 Valentin Milea + * + * SPDX-License-Identifier: MIT + */ + +#include "include/i2c_slave.h" + +#include +#include +#include "include/i2c_buffer.h" + +static bool transfer_in_progress; + +static inline void finish_transfer(void) +{ + if(transfer_in_progress) + { + i2c_slave_buffer_handler(I2C_SLAVE_FINISH); + transfer_in_progress = false; + } +} + +static void __not_in_flash_func(i2c_slave_irq_handler)(void) +{ + i2c_hw_t *hw = i2c_get_hw(I2C_SLAVE_INSTANCE); + + 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(); + } + + if(intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) + { + hw->clr_start_det; + finish_transfer(); + } + + if(intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) + { + hw->clr_stop_det; + finish_transfer(); + } + + if(intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) + { + transfer_in_progress = true; + i2c_slave_buffer_handler(I2C_SLAVE_RECEIVE); + } + + if(intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) + { + hw->clr_rd_req; + transfer_in_progress = true; + i2c_slave_buffer_handler(I2C_SLAVE_REQUEST); + } +} + +void i2c_slave_init(void) +{ + // Init GPIO pins + gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C); + gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C); + + // 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 i2c_slave.handler() is way too slow). + i2c_set_slave_mode(I2C_SLAVE_INSTANCE, true, I2C_SLAVE_ADDRESS); + + i2c_hw_t *hw = i2c_get_hw(I2C_SLAVE_INSTANCE); + // 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_get_index(I2C_SLAVE_INSTANCE); + irq_set_exclusive_handler(num, i2c_slave_irq_handler); + irq_set_enabled(num, true); +} + +void i2c_slave_deinit(void) +{ + uint num = I2C0_IRQ + i2c_get_index(I2C_SLAVE_INSTANCE); + irq_set_enabled(num, false); + irq_remove_handler(num, i2c_slave_irq_handler); + + i2c_set_slave_mode(I2C_SLAVE_INSTANCE, false, 0); + + transfer_in_progress = false; + + i2c_hw_t *hw = i2c_get_hw(I2C_SLAVE_INSTANCE); + hw->intr_mask = I2C_IC_INTR_MASK_RESET; +} diff --git a/program/motion controller code/src/include/i2c_buffer.h b/program/motion controller code/src/include/i2c_buffer.h new file mode 100644 index 0000000..8bfb859 --- /dev/null +++ b/program/motion controller code/src/include/i2c_buffer.h @@ -0,0 +1,20 @@ +#ifndef I2C_BUFFER_H +#define I2C_BUFFER_H + +#include +#include "i2c_slave.h" + +typedef struct i2c_buffer_t { + uint8_t buffer[256]; + uint8_t buffer_reg; + bool buffer_reg_written; +} i2c_buffer_t; + +// I2c slave buffer handler for writing and reading data to the buffer +void __not_in_flash_func(i2c_slave_buffer_handler)(i2c_slave_event_t event); +// Update motors from the data in the i2c buffer +void update_motors_from_buffer(void); +// Update servo motors from the data in the i2c buffer +void update_servo_motors_from_buffer(void); + +#endif // I2C_BUFFER_H \ No newline at end of file diff --git a/program/motion controller code/src/include/i2c_slave.h b/program/motion controller code/src/include/i2c_slave.h new file mode 100644 index 0000000..2b69891 --- /dev/null +++ b/program/motion controller code/src/include/i2c_slave.h @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2021 Valentin Milea + * + * SPDX-License-Identifier: MIT + */ + +#ifndef I2C_SLAVE_H +#define I2C_SLAVE_H + +#include + +#define I2C_SLAVE_SDA_PIN 21 +#define I2C_SLAVE_SCL_PIN 20 +#define I2C_SLAVE_INSTANCE i2c0 +#define I2C_SLAVE_ADDRESS 0x09 + +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; + +static inline uint8_t i2c_slave_read_byte(void) +{ + i2c_hw_t *hw = i2c_get_hw(I2C_SLAVE_INSTANCE); + assert(hw->status & I2C_IC_STATUS_RFNE_BITS); // Rx FIFO must not be empty + return (uint8_t)hw->data_cmd; +} + +static inline void i2c_slave_write_byte(uint8_t value) +{ + i2c_hw_t *hw = i2c_get_hw(I2C_SLAVE_INSTANCE); + assert(hw->status & I2C_IC_STATUS_TFNF_BITS); // Tx FIFO must not be full + hw->data_cmd = value; +} + +// Init I2C with default parameters +void i2c_slave_init(void); + +// Deinit I2C with default parameters +void i2c_slave_deinit(void); + +#endif // I2C_SLAVE_H \ No newline at end of file diff --git a/program/motion controller code/src/include/motors.h b/program/motion controller code/src/include/motors.h new file mode 100644 index 0000000..ed9d860 --- /dev/null +++ b/program/motion controller code/src/include/motors.h @@ -0,0 +1,57 @@ +#ifndef MOTORS_H +#define MOTORS_H + +#include + +// 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_reg; +} 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_reg; +} 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 +void motor_set(motors_enum_t motor, int8_t 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/program/motion controller code/src/include/robot.h b/program/motion controller code/src/include/robot.h new file mode 100644 index 0000000..4abde5c --- /dev/null +++ b/program/motion controller code/src/include/robot.h @@ -0,0 +1,22 @@ +#ifndef ROBOT_H +#define ROBOT_H + +#include "i2c_buffer.h" + +typedef struct robot_t { + i2c_buffer_t i2c_buffer; + + 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/program/motion controller code/src/main.c b/program/motion controller code/src/main.c new file mode 100644 index 0000000..120d987 --- /dev/null +++ b/program/motion controller code/src/main.c @@ -0,0 +1,28 @@ +/* *\ + Copyrights 2025 + Riombotique +\* */ + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\ +* Code du RPI Pico gérant les different Actionneurs. * +* Ce Pico est un esclave piloté par le Pico Principal. * +\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + +#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/program/motion controller code/src/motors.c b/program/motion controller code/src/motors.c new file mode 100644 index 0000000..d34432b --- /dev/null +++ b/program/motion controller code/src/motors.c @@ -0,0 +1,119 @@ +#include "include/motors.h" + +#include +#include + +const motor_def_t MOTORS_DEFS[] = { + {0, 4, 5, 0x00}, + {1, 6, 7, 0x01}, + {2, 9, 8, 0x02}, + {3, 11, 10, 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 + const 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 // + const 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, int8_t 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/program/motion controller code/src/robot.c b/program/motion controller code/src/robot.c new file mode 100644 index 0000000..80d6b28 --- /dev/null +++ b/program/motion controller code/src/robot.c @@ -0,0 +1,26 @@ +#include "include/robot.h" + +#include +#include "include/motors.h" +#include "include/i2c_slave.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