Starting new pcb and copying code from last year main robot
This commit is contained in:
ulysse-cura
commit
8b8a83947f
|
|
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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": {}
|
||||
}
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
(kicad_pcb (version 20241229) (generator "pcbnew") (generator_version "9.0")
|
||||
)
|
||||
|
|
@ -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": {}
|
||||
}
|
||||
|
|
@ -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)
|
||||
)
|
||||
|
|
@ -0,0 +1 @@
|
|||
{"hostname":"aspire-laptop","username":"ulysse-cura"}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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"
|
||||
}
|
||||
}
|
||||
|
|
@ -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"
|
||||
}
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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/
|
||||
`-.....-'
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
# This is a copy of <PICO_SDK_PATH>/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})
|
||||
|
|
@ -0,0 +1,135 @@
|
|||
#include "include/gyro.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include "include/i2c_master.h"
|
||||
#include "include/robot.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
#include "include/i2c_master.h"
|
||||
|
||||
#include <pico/stdlib.h>
|
||||
#include <hardware/i2c.h>
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
#ifndef I2C_MASTER_H
|
||||
#define I2C_MASTER_H
|
||||
|
||||
#include <hardware/i2c.h>
|
||||
|
||||
#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
|
||||
|
|
@ -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__ */
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
#ifndef MOTION_CONTROL_H
|
||||
#define MOTION_CONTROL_H
|
||||
|
||||
#include <stdint.h>
|
||||
#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
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
#ifndef MOTORS_H
|
||||
#define MOTORS_H
|
||||
|
||||
#include <pico/types.h>
|
||||
|
||||
#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
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
#ifndef ROBOT_H
|
||||
#define ROBOT_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#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
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#ifndef UDP_CLIENT_H
|
||||
#define UDP_CLIENT_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <lwip/udp.h>
|
||||
|
||||
#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
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
#ifndef UDP_BUFFER_H
|
||||
#define UDP_BUFFER_H
|
||||
|
||||
#include <pico.h>
|
||||
#include <stdint.h>
|
||||
#include <lwip/udp.h>
|
||||
|
||||
#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
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
#include "include/motion_control.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#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]);
|
||||
}
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
#include "include/motors.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#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;
|
||||
}
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
#include "include/robot.h"
|
||||
|
||||
#include <pico/stdlib.h>
|
||||
#include <pico/cyw43_arch.h>
|
||||
#include <time.h>
|
||||
#include <pico/mutex.h>
|
||||
#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();
|
||||
}
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
#include "include/udp_client.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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];
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
#include "include/wifi_operator.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <pico/cyw43_arch.h>
|
||||
#include <lwip/netif.h>
|
||||
#include <lwip/ip4_addr.h>
|
||||
|
||||
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 !");
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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"
|
||||
}
|
||||
}
|
||||
|
|
@ -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"
|
||||
}
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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 |
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
# This is a copy of <PICO_SDK_PATH>/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})
|
||||
|
|
@ -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]);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
/*
|
||||
* Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
|
||||
*
|
||||
* SPDX-License-Identifier: MIT
|
||||
*/
|
||||
|
||||
#include "include/i2c_slave.h"
|
||||
|
||||
#include <pico/stdlib.h>
|
||||
#include <hardware/irq.h>
|
||||
#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;
|
||||
}
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
#ifndef I2C_BUFFER_H
|
||||
#define I2C_BUFFER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#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
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
/*
|
||||
* Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
|
||||
*
|
||||
* SPDX-License-Identifier: MIT
|
||||
*/
|
||||
|
||||
#ifndef I2C_SLAVE_H
|
||||
#define I2C_SLAVE_H
|
||||
|
||||
#include <hardware/i2c.h>
|
||||
|
||||
#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
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
#ifndef MOTORS_H
|
||||
#define MOTORS_H
|
||||
|
||||
#include <pico/stdlib.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_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
|
||||
|
|
@ -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
|
||||
|
|
@ -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 <stdbool.h>
|
||||
#include "include/robot.h"
|
||||
|
||||
robot_t robot;
|
||||
|
||||
int main(void)
|
||||
{
|
||||
robot_init();
|
||||
|
||||
while(robot.is_running)
|
||||
{
|
||||
robot_handle_inputs_outputs();
|
||||
}
|
||||
|
||||
robot_deinit();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
#include "include/motors.h"
|
||||
|
||||
#include <pico/stdlib.h>
|
||||
#include <hardware/pwm.h>
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
#include "include/robot.h"
|
||||
|
||||
#include <pico/stdlib.h>
|
||||
#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();
|
||||
}
|
||||
Loading…
Reference in New Issue