ulysse
/
Robot_Principal_2024-2025
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Coded motion controller firmware

This commit is contained in:
Ulysse Cura 2025-05-04 18:02:26 +02:00
parent aa0d837c40
commit cb7188c751
15 changed files with 689 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
motion controller code/.vscode/c_cpp_properties.json vendored Normal file
View File

@ -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
}

12
motion controller code/.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,12 @@
{
"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"
}
}

25
motion controller code/.vscode/tasks.json vendored Normal file
View File

@ -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"
}

36
motion controller code/CMakeLists.txt Normal file
View File

@ -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
)

54
motion controller code/Readme.md Normal file
View File

@ -0,0 +1,54 @@
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.

62
motion controller code/pico_sdk_import.cmake Normal file
View File

@ -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})

58
motion controller code/src/i2c_buffer.c Normal file
View File

@ -0,0 +1,58 @@
#include "include/robot.h"
#include "include/motors.h"
#include "include/i2c_buffer.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_address_written)
{
// writes always start with the memory address
robot.i2c_buffer.buffer_address = i2c_slave_read_byte();
robot.i2c_buffer.buffer_address_written = true;
}
else
{
// save into memory
robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_address] = i2c_slave_read_byte();
robot.i2c_buffer.buffer_address++;
}
break;
case I2C_SLAVE_REQUEST: // master is requesting data
// load from memory
i2c_slave_write_byte(robot.i2c_buffer.buffer[robot.i2c_buffer.buffer_address]);
robot.i2c_buffer.buffer_address++;
break;
case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
robot.i2c_buffer.buffer_address_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_address]);
}
}
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_address]);
}
}

103
motion controller code/src/i2c_slave.c Normal file
View File

@ -0,0 +1,103 @@
/*
* Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
*
* SPDX-License-Identifier: MIT
*/
#include <pico/stdlib.h>
#include <hardware/irq.h>
#include "include/i2c_buffer.h"
#include "include/i2c_slave.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);
gpio_pull_up(I2C_SLAVE_SDA_PIN);
gpio_pull_up(I2C_SLAVE_SCL_PIN);
// 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;
}

20
motion controller code/src/include/i2c_buffer.h Normal file
View File

@ -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_address;
bool buffer_address_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

43
motion controller code/src/include/i2c_slave.h Normal file
View File

@ -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

55
motion controller code/src/include/motors.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef MOTORS_H
#define MOTORS_H
// Motors
typedef enum motors_enum_t {
MOTOR1,
MOTOR2,
MOTOR3,
MOTOR4,
NB_MOTORS
} motors_enum_t;
typedef struct motor_def_t {
uint pwm_pin;
uint dir1_pin;
uint dir2_pin;
uint8_t buffer_address;
} motor_def_t;
extern const motor_def_t MOTORS_DEFS[];
// Servo Motors
typedef enum {
SERVO_MOTOR1,
SERVO_MOTOR2,
SERVO_MOTOR3,
SERVO_MOTOR4,
NB_SERVO_MOTORS
} servo_motors_enum_t;
typedef struct {
uint pwm_pin;
uint open_pos;
uint close_pos;
uint8_t buffer_address;
} servo_motor_def_t;
extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
// Init all motors defined in the MOTORS_DEF array
void init_motors(void);
// Init all servo motors defined in the SERVO_MOTORS_DEF array
void init_servo_motors(void);
// Set [motor] to 0
void motor_zero(motors_enum_t motor);
// Set [motor] in motor_enum_t at [value] between -127 and 128 (for this config)
void motor_set(motors_enum_t motor, int value);
// Set servo motor to its open pos
void servo_motor_zero(servo_motors_enum_t servo_motor);
// Set servo to its close pos if [close] else open pos
void servo_motor_set(servo_motors_enum_t servo_motor, bool close);
#endif // MOTORS_H

22
motion controller code/src/include/robot.h Normal file
View File

@ -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

23
motion controller code/src/main.c Normal file
View File

@ -0,0 +1,23 @@
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
* 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;
}

119
motion controller code/src/motors.c Normal file
View File

@ -0,0 +1,119 @@
#include <pico/stdlib.h>
#include <hardware/pwm.h>
#include "include/motors.h"
const motor_def_t MOTORS_DEFS[] = {
{0, 4, 5, 0x00},
{1, 6, 7, 0x01},
{2, 8, 9, 0x02},
{3, 10, 11, 0x03},
};
const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
{12, 0, 25000, 0x04},
{13, 0, 25000, 0x05},
{14, 0, 25000, 0x06},
{15, 0, 25000, 0x07},
};
// Init all motors defined in the MOTORS_DEF array
void init_motors(void)
{
for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++)
{
const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
// Init PWM
uint slice_num = pwm_gpio_to_slice_num(motor_def->pwm_pin);
gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM);
pwm_set_wrap(slice_num, 128);
pwm_set_enabled(slice_num, true);
// Init dir pins
gpio_init(motor_def->dir1_pin);
gpio_set_dir(motor_def->dir1_pin, GPIO_OUT);
gpio_init(motor_def->dir2_pin);
gpio_set_dir(motor_def->dir2_pin, GPIO_OUT);
motor_zero(actual_motor);
}
}
// Init all servo motors defined in the SERVO_MOTORS_DEF array
void init_servo_motors(void)
{
for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
// Init PWM //
uint slice_num = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin);
gpio_set_function(servo_motor_def->pwm_pin, GPIO_FUNC_PWM);
pwm_set_wrap(slice_num, 25000);
pwm_set_clkdiv(slice_num, 100);
pwm_set_enabled(slice_num, true);
servo_motor_zero(actual_servo_motor);
}
}
// Set [motor] to 0
void motor_zero(motors_enum_t motor)
{
const motor_def_t *motor_def = &MOTORS_DEFS[motor];
// Set PWM to zero
pwm_set_gpio_level(motor_def->pwm_pin, 0);
// Set dir pins to false
gpio_put(motor_def->dir1_pin, false);
gpio_put(motor_def->dir2_pin, false);
}
// Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config)
void motor_set(motors_enum_t motor, int value)
{
const motor_def_t *motor_def = &MOTORS_DEFS[motor];
if(value < 0)
{
gpio_put(motor_def->dir1_pin, true);
gpio_put(motor_def->dir2_pin, false);
value = -value;
}
else if(value > 0)
{
gpio_put(motor_def->dir1_pin, false);
gpio_put(motor_def->dir2_pin, true);
}
else
{
gpio_put(motor_def->dir1_pin, false);
gpio_put(motor_def->dir2_pin, false);
}
pwm_set_gpio_level(motor_def->pwm_pin, (uint16_t)value);
}
// Set servo motor to its open pos
void servo_motor_zero(servo_motors_enum_t servo_motor)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
// Set PWM to zero //
pwm_set_gpio_level(servo_motor_def->pwm_pin, servo_motor_def->open_pos);
}
// Set servo to its close pos if [close] else open pos
void servo_motor_set(servo_motors_enum_t servo_motor, bool close)
{
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
// Set PWM to zero //
pwm_set_gpio_level(servo_motor_def->pwm_pin, close ? servo_motor_def->close_pos : servo_motor_def->open_pos);
}

26
motion controller code/src/robot.c Normal file
View File

@ -0,0 +1,26 @@
#include <pico/stdlib.h>
#include "include/motors.h"
#include "include/i2c_slave.h"
#include "include/robot.h"
void robot_init(void)
{
stdio_init_all();
init_motors();
init_servo_motors();
i2c_slave_init();
robot.is_running = true;
}
void robot_handle_inputs_outputs(void)
{
update_motors_from_buffer();
}
void robot_deinit(void)
{
i2c_slave_deinit();
}