From cb7188c7511c4a2026bfbbd4f3a8d72771e7e3ac Mon Sep 17 00:00:00 2001 From: Ulysse Cura Date: Sun, 4 May 2025 18:02:26 +0200 Subject: [PATCH] Coded motion controller firmware --- .../.vscode/c_cpp_properties.json | 31 +++++ motion controller code/.vscode/settings.json | 12 ++ motion controller code/.vscode/tasks.json | 25 ++++ motion controller code/CMakeLists.txt | 36 ++++++ motion controller code/Readme.md | 54 ++++++++ motion controller code/pico_sdk_import.cmake | 62 +++++++++ motion controller code/src/i2c_buffer.c | 58 +++++++++ motion controller code/src/i2c_slave.c | 103 +++++++++++++++ .../src/include/i2c_buffer.h | 20 +++ .../src/include/i2c_slave.h | 43 +++++++ motion controller code/src/include/motors.h | 55 ++++++++ motion controller code/src/include/robot.h | 22 ++++ motion controller code/src/main.c | 23 ++++ motion controller code/src/motors.c | 119 ++++++++++++++++++ motion controller code/src/robot.c | 26 ++++ 15 files changed, 689 insertions(+) create mode 100644 motion controller code/.vscode/c_cpp_properties.json create mode 100644 motion controller code/.vscode/settings.json create mode 100644 motion controller code/.vscode/tasks.json create mode 100644 motion controller code/CMakeLists.txt create mode 100644 motion controller code/Readme.md create mode 100644 motion controller code/pico_sdk_import.cmake create mode 100644 motion controller code/src/i2c_buffer.c create mode 100644 motion controller code/src/i2c_slave.c create mode 100644 motion controller code/src/include/i2c_buffer.h create mode 100644 motion controller code/src/include/i2c_slave.h create mode 100644 motion controller code/src/include/motors.h create mode 100644 motion controller code/src/include/robot.h create mode 100644 motion controller code/src/main.c create mode 100644 motion controller code/src/motors.c create mode 100644 motion controller code/src/robot.c diff --git a/motion controller code/.vscode/c_cpp_properties.json b/motion controller code/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..8cc4ce6 --- /dev/null +++ b/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/motion controller code/.vscode/settings.json b/motion controller code/.vscode/settings.json new file mode 100644 index 0000000..d1d99fd --- /dev/null +++ b/motion controller code/.vscode/settings.json @@ -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" + } +} \ No newline at end of file diff --git a/motion controller code/.vscode/tasks.json b/motion controller code/.vscode/tasks.json new file mode 100644 index 0000000..c98bf9b --- /dev/null +++ b/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/motion controller code/CMakeLists.txt b/motion controller code/CMakeLists.txt new file mode 100644 index 0000000..2f024e1 --- /dev/null +++ b/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/motion controller code/Readme.md b/motion controller code/Readme.md new file mode 100644 index 0000000..efc87d1 --- /dev/null +++ b/motion controller code/Readme.md @@ -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. \ No newline at end of file diff --git a/motion controller code/pico_sdk_import.cmake b/motion controller code/pico_sdk_import.cmake new file mode 100644 index 0000000..28efe9e --- /dev/null +++ b/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/motion controller code/src/i2c_buffer.c b/motion controller code/src/i2c_buffer.c new file mode 100644 index 0000000..075b643 --- /dev/null +++ b/motion controller code/src/i2c_buffer.c @@ -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]); + } +} \ No newline at end of file diff --git a/motion controller code/src/i2c_slave.c b/motion controller code/src/i2c_slave.c new file mode 100644 index 0000000..59b23cd --- /dev/null +++ b/motion controller code/src/i2c_slave.c @@ -0,0 +1,103 @@ +/* + * Copyright (c) 2021 Valentin Milea + * + * SPDX-License-Identifier: MIT + */ + +#include +#include +#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; +} diff --git a/motion controller code/src/include/i2c_buffer.h b/motion controller code/src/include/i2c_buffer.h new file mode 100644 index 0000000..6a74aa7 --- /dev/null +++ b/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_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 \ No newline at end of file diff --git a/motion controller code/src/include/i2c_slave.h b/motion controller code/src/include/i2c_slave.h new file mode 100644 index 0000000..2b69891 --- /dev/null +++ b/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/motion controller code/src/include/motors.h b/motion controller code/src/include/motors.h new file mode 100644 index 0000000..f7db597 --- /dev/null +++ b/motion controller code/src/include/motors.h @@ -0,0 +1,55 @@ +#ifndef MOTORS_H +#define MOTORS_H + +// Motors +typedef enum motors_enum_t { + MOTOR1, + MOTOR2, + MOTOR3, + MOTOR4, + + NB_MOTORS +} motors_enum_t; + +typedef struct motor_def_t { + uint pwm_pin; + uint dir1_pin; + uint dir2_pin; + uint8_t buffer_address; +} motor_def_t; + +extern const motor_def_t MOTORS_DEFS[]; + +// Servo Motors +typedef enum { + SERVO_MOTOR1, + SERVO_MOTOR2, + SERVO_MOTOR3, + SERVO_MOTOR4, + + NB_SERVO_MOTORS +} servo_motors_enum_t; + +typedef struct { + uint pwm_pin; + uint open_pos; + uint close_pos; + uint8_t buffer_address; +} servo_motor_def_t; + +extern const servo_motor_def_t SERVO_MOTORS_DEFS[]; + +// Init all motors defined in the MOTORS_DEF array +void init_motors(void); +// Init all servo motors defined in the SERVO_MOTORS_DEF array +void init_servo_motors(void); +// Set [motor] to 0 +void motor_zero(motors_enum_t motor); +// Set [motor] in motor_enum_t at [value] between -127 and 128 (for this config) +void motor_set(motors_enum_t motor, int value); +// Set servo motor to its open pos +void servo_motor_zero(servo_motors_enum_t servo_motor); +// Set servo to its close pos if [close] else open pos +void servo_motor_set(servo_motors_enum_t servo_motor, bool close); + +#endif // MOTORS_H diff --git a/motion controller code/src/include/robot.h b/motion controller code/src/include/robot.h new file mode 100644 index 0000000..4abde5c --- /dev/null +++ b/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/motion controller code/src/main.c b/motion controller code/src/main.c new file mode 100644 index 0000000..189c36f --- /dev/null +++ b/motion controller code/src/main.c @@ -0,0 +1,23 @@ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\ +* 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/motion controller code/src/motors.c b/motion controller code/src/motors.c new file mode 100644 index 0000000..3969bb7 --- /dev/null +++ b/motion controller code/src/motors.c @@ -0,0 +1,119 @@ +#include +#include + +#include "include/motors.h" + +const motor_def_t MOTORS_DEFS[] = { + {0, 4, 5, 0x00}, + {1, 6, 7, 0x01}, + {2, 8, 9, 0x02}, + {3, 10, 11, 0x03}, +}; + +const servo_motor_def_t SERVO_MOTORS_DEFS[] = { + {12, 0, 25000, 0x04}, + {13, 0, 25000, 0x05}, + {14, 0, 25000, 0x06}, + {15, 0, 25000, 0x07}, +}; + +// Init all motors defined in the MOTORS_DEF array +void init_motors(void) +{ + for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++) + { + const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor]; + + // Init PWM + uint slice_num = pwm_gpio_to_slice_num(motor_def->pwm_pin); + + gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM); + pwm_set_wrap(slice_num, 128); + pwm_set_enabled(slice_num, true); + + // Init dir pins + gpio_init(motor_def->dir1_pin); + gpio_set_dir(motor_def->dir1_pin, GPIO_OUT); + + gpio_init(motor_def->dir2_pin); + gpio_set_dir(motor_def->dir2_pin, GPIO_OUT); + + motor_zero(actual_motor); + } +} + +// Init all servo motors defined in the SERVO_MOTORS_DEF array +void init_servo_motors(void) +{ + for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++) + { + const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor]; + + // Init PWM // + uint slice_num = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin); + + gpio_set_function(servo_motor_def->pwm_pin, GPIO_FUNC_PWM); + pwm_set_wrap(slice_num, 25000); + pwm_set_clkdiv(slice_num, 100); + pwm_set_enabled(slice_num, true); + + servo_motor_zero(actual_servo_motor); + } +} + +// Set [motor] to 0 +void motor_zero(motors_enum_t motor) +{ + const motor_def_t *motor_def = &MOTORS_DEFS[motor]; + + // Set PWM to zero + pwm_set_gpio_level(motor_def->pwm_pin, 0); + + // Set dir pins to false + gpio_put(motor_def->dir1_pin, false); + gpio_put(motor_def->dir2_pin, false); +} + +// Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config) +void motor_set(motors_enum_t motor, int value) +{ + const motor_def_t *motor_def = &MOTORS_DEFS[motor]; + + if(value < 0) + { + gpio_put(motor_def->dir1_pin, true); + gpio_put(motor_def->dir2_pin, false); + + value = -value; + } + else if(value > 0) + { + gpio_put(motor_def->dir1_pin, false); + gpio_put(motor_def->dir2_pin, true); + } + else + { + gpio_put(motor_def->dir1_pin, false); + gpio_put(motor_def->dir2_pin, false); + } + + pwm_set_gpio_level(motor_def->pwm_pin, (uint16_t)value); +} + +// Set servo motor to its open pos +void servo_motor_zero(servo_motors_enum_t servo_motor) +{ + const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor]; + + // Set PWM to zero // + pwm_set_gpio_level(servo_motor_def->pwm_pin, servo_motor_def->open_pos); +} + +// Set servo to its close pos if [close] else open pos +void servo_motor_set(servo_motors_enum_t servo_motor, bool close) +{ + const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor]; + + // Set PWM to zero // + pwm_set_gpio_level(servo_motor_def->pwm_pin, close ? servo_motor_def->close_pos : servo_motor_def->open_pos); +} diff --git a/motion controller code/src/robot.c b/motion controller code/src/robot.c new file mode 100644 index 0000000..e1c3a85 --- /dev/null +++ b/motion controller code/src/robot.c @@ -0,0 +1,26 @@ +#include +#include "include/motors.h" +#include "include/i2c_slave.h" + +#include "include/robot.h" + +void robot_init(void) +{ + stdio_init_all(); + + init_motors(); + init_servo_motors(); + i2c_slave_init(); + + robot.is_running = true; +} + +void robot_handle_inputs_outputs(void) +{ + update_motors_from_buffer(); +} + +void robot_deinit(void) +{ + i2c_slave_deinit(); +} \ No newline at end of file