├── cmake ├── templates │ ├── jlink.in │ ├── tasks.json.in │ ├── run_clang_tidy.sh.in │ ├── launch.json.in │ └── helpme.in └── utilities.cmake ├── .gitmodules ├── .docker ├── .dockerignore ├── scripts │ └── check_format.sh └── Dockerfile ├── micras_core ├── CMakeLists.txt ├── include │ └── micras │ │ └── core │ │ ├── types.hpp │ │ ├── serializable.hpp │ │ ├── vector.hpp │ │ ├── butterworth_filter.hpp │ │ ├── fsm.hpp │ │ ├── pid_controller.hpp │ │ └── utils.hpp └── src │ ├── fsm.cpp │ ├── vector.cpp │ ├── butterworth_filter.cpp │ └── pid_controller.cpp ├── micras_hal ├── src │ ├── crc.cpp │ ├── mcu.cpp │ ├── gpio.cpp │ ├── encoder.cpp │ ├── timer.cpp │ ├── adc_dma.cpp │ ├── spi.cpp │ ├── pwm.cpp │ ├── pwm_dma.cpp │ └── flash.cpp ├── include │ └── micras │ │ └── hal │ │ ├── mcu.hpp │ │ ├── crc.hpp │ │ ├── encoder.hpp │ │ ├── timer.hpp │ │ ├── gpio.hpp │ │ ├── pwm.hpp │ │ ├── spi.hpp │ │ ├── pwm_dma.hpp │ │ ├── adc_dma.hpp │ │ └── flash.hpp └── CMakeLists.txt ├── micras_nav ├── CMakeLists.txt ├── src │ ├── state.cpp │ ├── grid_pose.cpp │ ├── speed_controller.cpp │ ├── odometry.cpp │ ├── action_queuer.cpp │ └── follow_wall.cpp └── include │ └── micras │ └── nav │ ├── actions │ ├── base.hpp │ └── move.hpp │ ├── state.hpp │ ├── odometry.hpp │ ├── action_queuer.hpp │ ├── speed_controller.hpp │ ├── grid_pose.hpp │ └── costmap.hpp ├── src ├── main.cpp └── interface.cpp ├── micras_proxy ├── src │ ├── led.cpp │ ├── battery.cpp │ ├── dip_switch.cpp │ ├── motor.cpp │ ├── buzzer.cpp │ ├── stopwatch.cpp │ ├── locomotion.cpp │ ├── fan.cpp │ ├── argb.cpp │ ├── button.cpp │ ├── torque_sensors.cpp │ ├── wall_sensors.cpp │ └── rotary_sensor.cpp ├── CMakeLists.txt └── include │ └── micras │ └── proxy │ ├── led.hpp │ ├── dip_switch.hpp │ ├── motor.hpp │ ├── buzzer.hpp │ ├── stopwatch.hpp │ ├── battery.hpp │ ├── locomotion.hpp │ ├── fan.hpp │ ├── rotary_sensor.hpp │ ├── rotary_sensor_reg.hpp │ ├── torque_sensors.hpp │ ├── button.hpp │ └── wall_sensors.hpp ├── tests ├── src │ ├── proxy │ │ ├── test_led.cpp │ │ ├── test_stopwatch.cpp │ │ ├── test_battery.cpp │ │ ├── test_fan.cpp │ │ ├── test_button.cpp │ │ ├── test_dip_switch.cpp │ │ ├── test_wall_sensors.cpp │ │ ├── test_buzzer.cpp │ │ ├── test_argb.cpp │ │ ├── test_imu.cpp │ │ ├── test_torque_sensors.cpp │ │ ├── test_locomotion.cpp │ │ └── test_rotary_sensors.cpp │ └── nav │ │ └── test_odometry.cpp └── include │ └── test_core.hpp ├── include └── micras │ ├── states │ ├── init.hpp │ ├── error.hpp │ ├── calibrate.hpp │ ├── base.hpp │ ├── idle.hpp │ ├── run.hpp │ └── wait.hpp │ └── interface.hpp ├── .vscode ├── settings.json └── extensions.json ├── Doxyfile ├── .devcontainer.json ├── compose.yaml ├── LICENSE ├── .gitignore ├── .github └── workflows │ ├── release.yaml │ └── ci.yaml ├── .clang-format ├── .clang-tidy └── CMakeLists.txt /cmake/templates/jlink.in: -------------------------------------------------------------------------------- 1 | device ${DEVICE} 2 | si SWD 3 | speed 4000 4 | connect 5 | r 6 | h 7 | loadfile ${CMAKE_CURRENT_BINARY_DIR}/${TARGET}.hex 8 | r 9 | g 10 | exit 11 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "micras_proxy/libs/lsm6dsv-pid"] 2 | path = micras_proxy/libs/lsm6dsv-pid 3 | url = https://github.com/STMicroelectronics/lsm6dsv-pid.git 4 | branch = master 5 | -------------------------------------------------------------------------------- /.docker/.dockerignore: -------------------------------------------------------------------------------- 1 | .git/ 2 | .github/ 3 | .vscode/ 4 | .cache/ 5 | .idea/ 6 | build/ 7 | cube/ 8 | !**.ioc 9 | 10 | .devcontainer 11 | .gitignore 12 | .gitmodules 13 | Doxyfile 14 | LICENSE 15 | 16 | *.yaml 17 | *.md 18 | *.iml 19 | -------------------------------------------------------------------------------- /micras_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(micras_core) 2 | 3 | file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") 4 | 5 | add_library(${PROJECT_NAME} STATIC 6 | ${PROJECT_SOURCES} 7 | ) 8 | 9 | add_library(micras::core ALIAS ${PROJECT_NAME}) 10 | 11 | target_include_directories(${PROJECT_NAME} PUBLIC 12 | include 13 | ) 14 | -------------------------------------------------------------------------------- /micras_hal/src/crc.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/hal/crc.hpp" 6 | 7 | namespace micras::hal { 8 | Crc::Crc(const Config& config) : handle{config.handle} { } 9 | 10 | // NOLINTNEXTLINE(*-avoid-c-arrays) 11 | uint32_t Crc::calculate(uint32_t data[], uint32_t size) { 12 | return HAL_CRC_Calculate(this->handle, data, size); 13 | } 14 | } // namespace micras::hal 15 | -------------------------------------------------------------------------------- /micras_nav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(micras_nav) 2 | 3 | file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") 4 | 5 | add_library(${PROJECT_NAME} STATIC 6 | ${PROJECT_SOURCES} 7 | ) 8 | 9 | add_library(micras::nav ALIAS ${PROJECT_NAME}) 10 | 11 | target_include_directories(${PROJECT_NAME} PUBLIC 12 | include 13 | ) 14 | 15 | target_link_libraries(${PROJECT_NAME} PUBLIC 16 | micras::proxy 17 | ) 18 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/micras.hpp" 6 | #include "micras/hal/mcu.hpp" 7 | 8 | /***************************************** 9 | * Main Function 10 | *****************************************/ 11 | 12 | int main() { 13 | micras::hal::Mcu::init(); 14 | micras::Micras micras; 15 | 16 | while (true) { 17 | micras.update(); 18 | } 19 | 20 | return 0; 21 | } 22 | -------------------------------------------------------------------------------- /micras_proxy/src/led.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/proxy/led.hpp" 6 | 7 | namespace micras::proxy { 8 | Led::Led(const Config& config) : gpio{config.gpio} { } 9 | 10 | void Led::turn_on() { 11 | this->gpio.write(true); 12 | } 13 | 14 | void Led::turn_off() { 15 | this->gpio.write(false); 16 | } 17 | 18 | void Led::toggle() { 19 | this->gpio.toggle(); 20 | } 21 | } // namespace micras::proxy 22 | -------------------------------------------------------------------------------- /tests/src/proxy/test_led.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | int main(int argc, char* argv[]) { 10 | TestCore::init(argc, argv); 11 | proxy::Led led{led_config}; 12 | 13 | TestCore::loop([&led]() { 14 | led.toggle(); 15 | 16 | proxy::Stopwatch::sleep_ms(500); 17 | }); 18 | 19 | return 0; 20 | } 21 | -------------------------------------------------------------------------------- /micras_hal/src/mcu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "micras/hal/mcu.hpp" 10 | 11 | extern "C" { 12 | /** 13 | * @brief Initialize System Clock. 14 | * 15 | * @note Defined by cube. 16 | */ 17 | void SystemClock_Config(); 18 | } 19 | 20 | namespace micras::hal { 21 | void Mcu::init() { 22 | HAL_Init(); 23 | 24 | SystemClock_Config(); 25 | 26 | MX_GPIO_Init(); 27 | MX_DMA_Init(); 28 | MX_CRC_Init(); 29 | } 30 | } // namespace micras::hal 31 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/mcu.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_MCU_HPP 6 | #define MICRAS_HAL_MCU_HPP 7 | 8 | namespace micras::hal { 9 | /** 10 | * @brief Microcontroller unit class. 11 | */ 12 | class Mcu { 13 | public: 14 | /** 15 | * @brief Deleted constructor for static class. 16 | */ 17 | Mcu() = delete; 18 | 19 | /** 20 | * @brief Initialize MCU and some peripherals. 21 | */ 22 | static void init(); 23 | }; 24 | } // namespace micras::hal 25 | 26 | #endif // MICRAS_HAL_MCU_HPP 27 | -------------------------------------------------------------------------------- /micras_hal/src/gpio.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/hal/gpio.hpp" 6 | 7 | namespace micras::hal { 8 | Gpio::Gpio(const Config& config) : port{config.port}, pin{config.pin} { } 9 | 10 | bool Gpio::read() const { 11 | return HAL_GPIO_ReadPin(this->port, this->pin) == GPIO_PIN_SET; 12 | } 13 | 14 | void Gpio::write(bool state) { 15 | HAL_GPIO_WritePin(this->port, this->pin, static_cast(state)); 16 | } 17 | 18 | void Gpio::toggle() { 19 | HAL_GPIO_TogglePin(this->port, this->pin); 20 | } 21 | } // namespace micras::hal 22 | -------------------------------------------------------------------------------- /.docker/scripts/check_format.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | FILES=$(find . \( -name "*.c" -o -name "*.cpp" -o -name "*.h" -o -name "*.hpp" \) \ 4 | -not \( -path "*/cube/*" -o -path "*/build/*" -o -path "*/libs/*" \)) 5 | 6 | for FILE in $FILES; do 7 | clang-format -style=file -output-replacements-xml $FILE | grep "/dev/null 8 | if [ $? -eq 0 ]; then 9 | echo "Code not properly formatted (File: $FILE). Please run make format." | sed 's/.*/\x1b[31m&\x1b[0m/' 10 | exit 1 11 | fi 12 | done 13 | 14 | echo "Code properly formatted." | sed 's/.*/\x1b[32m&\x1b[0m/' 15 | -------------------------------------------------------------------------------- /tests/src/proxy/test_stopwatch.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | static volatile uint32_t test_stopwatch_value{}; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) 10 | 11 | int main(int argc, char* argv[]) { 12 | TestCore::init(argc, argv); 13 | proxy::Stopwatch stopwatch{stopwatch_config}; 14 | 15 | TestCore::loop([&stopwatch]() { 16 | test_stopwatch_value += stopwatch.elapsed_time_us(); 17 | stopwatch.reset_us(); 18 | proxy::Stopwatch::sleep_ms(2); 19 | }); 20 | 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /include/micras/states/init.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef INIT_STATE_HPP 6 | #define INIT_STATE_HPP 7 | 8 | #include "micras/states/base.hpp" 9 | 10 | namespace micras { 11 | class InitState : public BaseState { 12 | public: 13 | using BaseState::BaseState; 14 | 15 | /** 16 | * @brief Execute this state. 17 | * 18 | * @return The id of the next state. 19 | */ 20 | uint8_t execute() override { 21 | if (not this->micras.check_initialization()) { 22 | return Micras::State::ERROR; 23 | } 24 | 25 | return Micras::State::IDLE; 26 | } 27 | }; 28 | } // namespace micras 29 | 30 | #endif // INIT_STATE_HPP 31 | -------------------------------------------------------------------------------- /cmake/templates/tasks.json.in: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2.0.0", 3 | "tasks": [ 4 | { 5 | "label": "Launch OpenOCD", 6 | "command": "${OPENOCD_CMD} -f ${OPENOCD_SCRIPTS_PATH}/interface/stlink.cfg -f ${OPENOCD_SCRIPTS_PATH}/target/${TARGET_CFG}x.cfg", 7 | "type": "shell", 8 | "problemMatcher": { 9 | "pattern": [ 10 | { 11 | "regexp": "", 12 | "file": 1, 13 | "location": 2, 14 | "message": 3 15 | } 16 | ], 17 | "background": { 18 | "activeOnStart": true, 19 | "beginsPattern": "Open On-Chip Debugger", 20 | "endsPattern": "Listening on port" 21 | } 22 | }, 23 | "isBackground": true 24 | } 25 | ] 26 | } 27 | -------------------------------------------------------------------------------- /micras_hal/src/encoder.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/hal/encoder.hpp" 8 | 9 | namespace micras::hal { 10 | Encoder::Encoder(const Config& config) : handle{config.handle} { 11 | config.init_function(); 12 | HAL_TIM_Encoder_Start(this->handle, config.timer_channel); 13 | 14 | // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) 15 | this->start_count = __HAL_TIM_GET_AUTORELOAD(this->handle) / 2; 16 | __HAL_TIM_SET_COUNTER(this->handle, this->start_count); 17 | } 18 | 19 | int32_t Encoder::get_counter() const { 20 | return std::bit_cast(__HAL_TIM_GET_COUNTER(this->handle) - this->start_count); 21 | } 22 | } // namespace micras::hal 23 | -------------------------------------------------------------------------------- /micras_proxy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(micras_proxy) 2 | 3 | file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") 4 | file(GLOB_RECURSE PROJECT_LIBS_SOURCES CONFIGURE_DEPENDS "libs/*.c*") 5 | 6 | # Remove warnings from libraries sources 7 | set_source_files_properties( 8 | ${PROJECT_LIBS_SOURCES} 9 | PROPERTIES 10 | COMPILE_FLAGS "-w" 11 | ) 12 | 13 | add_library(${PROJECT_NAME} STATIC 14 | ${PROJECT_SOURCES} 15 | ${PROJECT_LIBS_SOURCES} 16 | ) 17 | 18 | add_library(micras::proxy ALIAS ${PROJECT_NAME}) 19 | 20 | target_include_directories(${PROJECT_NAME} PUBLIC 21 | include 22 | libs/lsm6dsv-pid 23 | ) 24 | 25 | target_link_libraries(${PROJECT_NAME} PUBLIC 26 | micras::hal 27 | ) 28 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "editor.tabSize": 4, 3 | "editor.detectIndentation": false, 4 | "editor.insertSpaces": true, 5 | 6 | "files.trimFinalNewlines": true, 7 | "files.trimTrailingWhitespace": true, 8 | "files.insertFinalNewline": true, 9 | "files.eol": "\n", 10 | "files.encoding": "utf8", 11 | 12 | "editor.defaultFormatter": "xaver.clang-format", 13 | "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools", 14 | "C_Cpp.autoAddFileAssociations": false, 15 | 16 | "doxdocgen.file.fileOrder": [ 17 | "file", 18 | ], 19 | 20 | "clangd.arguments": [ 21 | "--pretty", 22 | "--compile-commands-dir=${workspaceFolder}/build/", 23 | ] 24 | } 25 | -------------------------------------------------------------------------------- /include/micras/states/error.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef ERROR_STATE_HPP 6 | #define ERROR_STATE_HPP 7 | 8 | #include "micras/states/base.hpp" 9 | 10 | namespace micras { 11 | class ErrorState : public BaseState { 12 | public: 13 | using BaseState::BaseState; 14 | 15 | /** 16 | * @brief Execute the entry function of this state. 17 | */ 18 | void on_entry() override { 19 | this->micras.stop(); 20 | this->micras.send_event(Interface::Event::ERROR); 21 | } 22 | 23 | /** 24 | * @brief Execute this state. 25 | * 26 | * @return The id of the next state. 27 | */ 28 | uint8_t execute() override { return this->get_id(); } 29 | }; 30 | } // namespace micras 31 | 32 | #endif // ERROR_STATE_HPP 33 | -------------------------------------------------------------------------------- /tests/src/proxy/test_battery.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | static volatile float test_battery_voltage_raw{}; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) 10 | static volatile float test_battery_voltage{}; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) 11 | 12 | int main(int argc, char* argv[]) { 13 | TestCore::init(argc, argv); 14 | proxy::Battery battery{battery_config}; 15 | 16 | TestCore::loop([&battery]() { 17 | battery.update(); 18 | test_battery_voltage_raw = battery.get_voltage_raw(); 19 | test_battery_voltage = battery.get_voltage(); 20 | proxy::Stopwatch::sleep_ms(2); 21 | }); 22 | 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /micras_hal/src/timer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/hal/timer.hpp" 6 | 7 | namespace micras::hal { 8 | Timer::Timer(const Config& config) : handle{config.handle} { 9 | config.init_function(); 10 | 11 | const uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); 12 | const uint32_t prescaler = this->handle->Instance->PSC; 13 | 14 | if (base_freq / (prescaler + 1) == 1000000) { 15 | this->enable_microseconds = true; 16 | HAL_TIM_Base_Start(this->handle); 17 | } 18 | } 19 | 20 | uint32_t Timer::get_counter_ms() { 21 | return HAL_GetTick(); 22 | } 23 | 24 | uint32_t Timer::get_counter_us() const { 25 | if (this->enable_microseconds) { 26 | return __HAL_TIM_GET_COUNTER(this->handle); 27 | } 28 | 29 | return 1000 * HAL_GetTick(); 30 | } 31 | } // namespace micras::hal 32 | -------------------------------------------------------------------------------- /include/micras/states/calibrate.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef CALIBRATE_STATE_HPP 6 | #define CALIBRATE_STATE_HPP 7 | 8 | #include "micras/states/base.hpp" 9 | 10 | namespace micras { 11 | class CalibrateState : public BaseState { 12 | public: 13 | using BaseState::BaseState; 14 | 15 | /** 16 | * @brief Execute the entry function of this state. 17 | */ 18 | void on_entry() override { this->micras.init(); } 19 | 20 | /** 21 | * @brief Execute this state. 22 | * 23 | * @return The id of the next state. 24 | */ 25 | uint8_t execute() override { 26 | if (this->micras.calibrate()) { 27 | return Micras::State::IDLE; 28 | } 29 | 30 | return Micras::State::WAIT_FOR_CALIBRATE; 31 | } 32 | }; 33 | } // namespace micras 34 | 35 | #endif // CALIBRATE_STATE_HPP 36 | -------------------------------------------------------------------------------- /micras_hal/src/adc_dma.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/hal/adc_dma.hpp" 8 | 9 | namespace micras::hal { 10 | AdcDma::AdcDma(const Config& config) : max_reading{config.max_reading}, handle{config.handle} { 11 | config.init_function(); 12 | HAL_ADCEx_Calibration_Start(this->handle, ADC_SINGLE_ENDED); 13 | } 14 | 15 | void AdcDma::start_dma(std::span buffer) { 16 | HAL_ADC_Start_DMA(this->handle, buffer.data(), buffer.size()); 17 | } 18 | 19 | void AdcDma::start_dma(std::span buffer) { 20 | this->start_dma({std::bit_cast(buffer.data()), buffer.size()}); 21 | } 22 | 23 | void AdcDma::stop_dma() { 24 | HAL_ADC_Stop_DMA(this->handle); 25 | } 26 | 27 | uint16_t AdcDma::get_max_reading() const { 28 | return this->max_reading; 29 | } 30 | } // namespace micras::hal 31 | -------------------------------------------------------------------------------- /cmake/templates/run_clang_tidy.sh.in: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | FIX_FLAG="" 4 | 5 | if [[ "$1" == "--fix" ]]; then 6 | FIX_FLAG="--fix" 7 | shift 8 | fi 9 | 10 | echo "Running clang-tidy on all source files..." 11 | 12 | ERRORS=$( 13 | { 14 | for file in "$@"; do 15 | { 16 | clang-tidy $FIX_FLAG --use-color --quiet --warnings-as-errors="*" \ 17 | --extra-arg-before="--sysroot=@SYSROOT@/" \ 18 | --extra-arg-before="-I@CXX_INCLUDE_DIR@/" \ 19 | --extra-arg-before="-I@CXX_TRIPLE_INCLUDE_DIR@/" \ 20 | -p @CMAKE_CURRENT_BINARY_DIR@ "$file" 2>&1 21 | } & 22 | done 23 | 24 | wait 25 | } | grep -v ' warnings generated\.$' | awk '!seen[$0]++' 26 | ) 27 | 28 | if [[ -n "$ERRORS" ]]; then 29 | echo -e "\n$ERRORS\033[0m" 30 | exit 1 31 | fi 32 | -------------------------------------------------------------------------------- /micras_core/include/micras/core/types.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_CORE_TYPES_HPP 6 | #define MICRAS_CORE_TYPES_HPP 7 | 8 | #include 9 | 10 | namespace micras::core { 11 | /** 12 | * @brief Possible values for the observation from the sensors. 13 | */ 14 | struct Observation { 15 | bool left{}; 16 | bool front{}; 17 | bool right{}; 18 | }; 19 | 20 | /** 21 | * @brief Possible objectives of the robot. 22 | */ 23 | enum Objective : uint8_t { 24 | EXPLORE = 0, 25 | RETURN = 1, 26 | SOLVE = 2 27 | }; 28 | 29 | /** 30 | * @brief Struct to store the index of each of the wall sensors. 31 | */ 32 | struct WallSensorsIndex { 33 | uint8_t left_front{}; 34 | uint8_t left{}; 35 | uint8_t right{}; 36 | uint8_t right_front{}; 37 | }; 38 | } // namespace micras::core 39 | 40 | #endif // MICRAS_CORE_TYPES_HPP 41 | -------------------------------------------------------------------------------- /micras_core/src/fsm.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/core/fsm.hpp" 6 | 7 | namespace micras::core { 8 | FSM::State::State(uint8_t id) : id{id} { } 9 | 10 | uint8_t FSM::State::get_id() const { 11 | return this->id; 12 | } 13 | 14 | FSM::FSM(uint8_t initial_state_id) : current_state_id{initial_state_id} { } 15 | 16 | void FSM::add_state(std::unique_ptr state) { 17 | this->states.emplace(state->get_id(), std::move(state)); 18 | } 19 | 20 | void FSM::update() { 21 | if (this->current_state_id != this->previous_state_id) { 22 | this->states.at(this->current_state_id)->on_entry(); 23 | } 24 | 25 | const uint8_t next_state = this->states.at(this->current_state_id)->execute(); 26 | 27 | this->previous_state_id = this->current_state_id; 28 | this->current_state_id = next_state; 29 | } 30 | } // namespace micras::core 31 | -------------------------------------------------------------------------------- /micras_hal/src/spi.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/hal/spi.hpp" 6 | 7 | namespace micras::hal { 8 | Spi::Spi(const Config& config) : handle{config.handle}, cs_gpio{config.cs_gpio}, timeout{config.timeout} { 9 | config.init_function(); 10 | } 11 | 12 | bool Spi::select_device() { 13 | if (HAL_SPI_GetState(this->handle) != HAL_SPI_STATE_READY) { 14 | return false; 15 | } 16 | 17 | this->cs_gpio.write(false); 18 | return true; 19 | } 20 | 21 | void Spi::unselect_device() { 22 | this->cs_gpio.write(true); 23 | } 24 | 25 | void Spi::transmit(std::span data) { 26 | HAL_SPI_Transmit(this->handle, data.data(), data.size(), this->timeout); 27 | } 28 | 29 | void Spi::receive(std::span data) { 30 | HAL_SPI_Receive(this->handle, data.data(), data.size(), this->timeout); 31 | } 32 | } // namespace micras::hal 33 | -------------------------------------------------------------------------------- /micras_proxy/src/battery.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/proxy/battery.hpp" 6 | 7 | namespace micras::proxy { 8 | Battery::Battery(const Config& config) : 9 | adc{config.adc}, 10 | max_voltage{hal::AdcDma::reference_voltage * config.voltage_divider}, 11 | filter{config.filter_cutoff} { 12 | this->adc.start_dma({&(this->raw_reading), 1}); 13 | } 14 | 15 | void Battery::update() { 16 | this->filter.update(this->get_adc_reading()); 17 | } 18 | 19 | float Battery::get_voltage() const { 20 | return this->filter.get_last() * this->max_voltage; 21 | } 22 | 23 | float Battery::get_voltage_raw() const { 24 | return this->get_adc_reading() * this->max_voltage; 25 | } 26 | 27 | float Battery::get_adc_reading() const { 28 | return static_cast(this->raw_reading) / this->adc.get_max_reading(); 29 | } 30 | } // namespace micras::proxy 31 | -------------------------------------------------------------------------------- /Doxyfile: -------------------------------------------------------------------------------- 1 | PROJECT_NAME = "Micras Firmware" 2 | OUTPUT_DIRECTORY = docs/ 3 | DISTRIBUTE_GROUP_DOC = YES 4 | NUM_PROC_THREADS = 0 5 | EXTRACT_PRIVATE = YES 6 | EXTRACT_PRIV_VIRTUAL = YES 7 | EXTRACT_STATIC = YES 8 | CASE_SENSE_NAMES = YES 9 | SHOW_INCLUDE_FILES = NO 10 | RECURSIVE = YES 11 | EXCLUDE = "tests/" \ 12 | "cube/" \ 13 | "build/" 14 | EXCLUDE_PATTERNS = "*/libs/*" 15 | INPUT_FILTER = "sed -e 's/#include <.*>/ /'" 16 | VERBATIM_HEADERS = NO 17 | GENERATE_HTML = NO 18 | UML_LOOK = YES 19 | UML_LIMIT_NUM_FIELDS = 50 20 | DOT_WRAP_THRESHOLD = 100 21 | TEMPLATE_RELATIONS = YES 22 | CALL_GRAPH = YES 23 | CALLER_GRAPH = YES 24 | DOT_GRAPH_MAX_NODES = 100 25 | MAX_DOT_GRAPH_DEPTH = 1 26 | DOT_MULTI_TARGETS = YES 27 | -------------------------------------------------------------------------------- /include/micras/states/base.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef BASE_STATE_HPP 6 | #define BASE_STATE_HPP 7 | 8 | #include "micras/core/fsm.hpp" 9 | #include "micras/micras.hpp" 10 | 11 | namespace micras { 12 | class BaseState : public core::FSM::State { 13 | public: 14 | /** 15 | * @brief Construct a new BaseState object. 16 | * 17 | * @param id The id of the state. 18 | * @param micras The Micras object. 19 | */ 20 | BaseState(uint8_t id, Micras& micras) : State(id), micras{micras} {}; 21 | 22 | /** 23 | * @brief Do nothing by default. 24 | */ 25 | void on_entry() override { } 26 | 27 | protected: 28 | /** 29 | * @brief A reference to the Micras object. 30 | */ 31 | Micras& micras; // NOLINT(*-non-private-member-variables-in-classes, *-avoid-const-or-ref-data-members) 32 | }; 33 | } // namespace micras 34 | 35 | #endif // BASE_STATE_HPP 36 | -------------------------------------------------------------------------------- /.devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Micras Dev Container", 3 | "dockerComposeFile": "compose.yaml", 4 | "service": "dev", 5 | "shutdownAction": "stopCompose", 6 | "workspaceFolder": "/project", 7 | "overrideCommand": true, 8 | "customizations": { 9 | "vscode": { 10 | "extensions": [ 11 | "ms-vscode.cpptools", 12 | "cschlosser.doxdocgen", 13 | "davidanson.vscode-markdownlint", 14 | "marus25.Cortex-Debug", 15 | "dan-c-underwood.arm", 16 | "streetsidesoftware.code-spell-checker", 17 | "streetsidesoftware.code-spell-checker-portuguese-brazilian", 18 | "twxs.cmake", 19 | "ms-vscode.cmake-tools", 20 | "seatonjiang.gitmoji-vscode", 21 | "mhutchie.git-graph", 22 | "xaver.clang-format" 23 | ] 24 | } 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations. 3 | // Extension identifier format: ${publisher}.${name}. Example: vscode.csharp 4 | 5 | // List of extensions which should be recommended for users of this workspace. 6 | "recommendations": [ 7 | "ms-vscode.cpptools", 8 | "cschlosser.doxdocgen", 9 | "davidanson.vscode-markdownlint", 10 | "marus25.Cortex-Debug", 11 | "dan-c-underwood.arm", 12 | "streetsidesoftware.code-spell-checker", 13 | "streetsidesoftware.code-spell-checker-portuguese-brazilian", 14 | "twxs.cmake", 15 | "ms-vscode.cmake-tools", 16 | "seatonjiang.gitmoji-vscode", 17 | "mhutchie.git-graph", 18 | "xaver.clang-format", 19 | ], 20 | 21 | // List of extensions recommended by VS Code that should not be recommended for users of this workspace. 22 | "unwantedRecommendations": [], 23 | } 24 | -------------------------------------------------------------------------------- /compose.yaml: -------------------------------------------------------------------------------- 1 | services: 2 | base: 3 | build: 4 | dockerfile: .docker/Dockerfile 5 | target: base 6 | 7 | build: 8 | command: make -C build --no-print-directory -j 9 | build: 10 | dockerfile: .docker/Dockerfile 11 | target: build 12 | 13 | lint: 14 | command: make -C build --no-print-directory lint 15 | build: 16 | dockerfile: .docker/Dockerfile 17 | target: build 18 | 19 | check-format: 20 | command: /project/.docker/scripts/check_format.sh 21 | build: 22 | dockerfile: .docker/Dockerfile 23 | target: build 24 | 25 | dev: 26 | command: bash 27 | build: 28 | dockerfile: .docker/Dockerfile 29 | target: base 30 | privileged: True 31 | environment: 32 | - DISPLAY=${DISPLAY} 33 | - QT_X11_NO_MITSHM=1 34 | - NVIDIA_DRIVER_CAPABILITIES=all 35 | volumes: 36 | - .:/project:rw 37 | - /tmp/.X11-unix:/tmp/.X11-unix:rw 38 | - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority 39 | -------------------------------------------------------------------------------- /micras_proxy/src/dip_switch.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_DIP_SWITCH_CPP 6 | #define MICRAS_PROXY_DIP_SWITCH_CPP 7 | 8 | #include "micras/core/utils.hpp" 9 | #include "micras/proxy/dip_switch.hpp" 10 | 11 | namespace micras::proxy { 12 | template 13 | TDipSwitch::TDipSwitch(const Config& config) : 14 | gpio_array{core::make_array(config.gpio_array)} { } 15 | 16 | template 17 | bool TDipSwitch::get_switch_state(uint8_t switch_index) const { 18 | return gpio_array.at(switch_index).read(); 19 | } 20 | 21 | template 22 | uint8_t TDipSwitch::get_switches_value() const { 23 | uint8_t switches_value = 0; 24 | 25 | for (uint8_t i = 0; i < num_of_sensors; i++) { 26 | switches_value |= (gpio_array[i].read() << i); 27 | } 28 | 29 | return switches_value; 30 | } 31 | } // namespace micras::proxy 32 | 33 | #endif // MICRAS_PROXY_DIP_SWITCH_CPP 34 | -------------------------------------------------------------------------------- /micras_hal/src/pwm.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/hal/pwm.hpp" 8 | 9 | namespace micras::hal { 10 | Pwm::Pwm(const Config& config) : handle{config.handle}, channel{config.timer_channel} { 11 | config.init_function(); 12 | HAL_TIM_PWM_Start(this->handle, this->channel); 13 | __HAL_TIM_SET_COMPARE(this->handle, this->channel, 0); 14 | } 15 | 16 | void Pwm::set_duty_cycle(float duty_cycle) { 17 | const uint32_t compare = std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100.0F)); 18 | __HAL_TIM_SET_COMPARE(this->handle, this->channel, compare); 19 | } 20 | 21 | void Pwm::set_frequency(uint32_t frequency) { 22 | const uint32_t base_freq = HAL_RCC_GetPCLK1Freq(); 23 | const uint32_t prescaler = this->handle->Instance->PSC; 24 | 25 | const uint32_t autoreload = base_freq / ((prescaler + 1) * frequency) - 1; 26 | __HAL_TIM_SET_AUTORELOAD(this->handle, autoreload); 27 | __HAL_TIM_SET_COUNTER(this->handle, 0); 28 | } 29 | } // namespace micras::hal 30 | -------------------------------------------------------------------------------- /tests/src/proxy/test_fan.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | static constexpr float max_speed{50.0F}; 10 | 11 | static volatile float test_fan_speed{}; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) 12 | 13 | int main(int argc, char* argv[]) { 14 | TestCore::init(argc, argv); 15 | proxy::Button button{button_config}; 16 | proxy::Fan fan{fan_config}; 17 | 18 | TestCore::loop([&button, &fan]() { 19 | while (button.get_status() == proxy::Button::Status::NO_PRESS) { 20 | button.update(); 21 | } 22 | 23 | fan.set_speed(max_speed); 24 | 25 | while (fan.update() < max_speed) { 26 | test_fan_speed = fan.update(); 27 | } 28 | 29 | proxy::Stopwatch::sleep_ms(3000); 30 | 31 | fan.set_speed(0.0F); 32 | 33 | while (fan.update() > 0.0F) { 34 | test_fan_speed = fan.update(); 35 | } 36 | }); 37 | 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /tests/src/proxy/test_button.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | int main(int argc, char* argv[]) { 10 | TestCore::init(argc, argv); 11 | proxy::Button button{button_config}; 12 | proxy::Led led{led_config}; 13 | proxy::Argb argb{argb_config}; 14 | 15 | proxy::Argb::Color color{}; 16 | 17 | TestCore::loop([&button, &led, &argb, &color]() { 18 | button.update(); 19 | button.is_pressed() ? led.turn_on() : led.turn_off(); 20 | 21 | if (button.get_status() == proxy::Button::Status::SHORT_PRESS) { 22 | color = proxy::Argb::Colors::red; 23 | } else if (button.get_status() == proxy::Button::Status::LONG_PRESS) { 24 | color = proxy::Argb::Colors::green; 25 | } else if (button.get_status() == proxy::Button::Status::EXTRA_LONG_PRESS) { 26 | color = proxy::Argb::Colors::blue; 27 | } 28 | 29 | argb.set_color(color); 30 | proxy::Stopwatch::sleep_ms(2); 31 | }); 32 | } 33 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/led.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_LED_HPP 6 | #define MICRAS_PROXY_LED_HPP 7 | 8 | #include "micras/hal/gpio.hpp" 9 | 10 | namespace micras::proxy { 11 | /** 12 | * @brief Class for controlling an LED. 13 | */ 14 | class Led { 15 | public: 16 | /** 17 | * @brief Configuration struct for LED. 18 | */ 19 | struct Config { 20 | hal::Gpio::Config gpio; 21 | }; 22 | 23 | /** 24 | * @brief Construct a new Led object. 25 | * 26 | * @param config Configuration for the LED. 27 | */ 28 | explicit Led(const Config& config); 29 | 30 | /** 31 | * @brief Turn the LED on. 32 | */ 33 | void turn_on(); 34 | 35 | /** 36 | * @brief Turn the LED off. 37 | */ 38 | void turn_off(); 39 | 40 | /** 41 | * @brief Toggle the LED. 42 | */ 43 | void toggle(); 44 | 45 | private: 46 | /** 47 | * @brief Gpio object for the LED. 48 | */ 49 | hal::Gpio gpio; 50 | }; 51 | } // namespace micras::proxy 52 | 53 | #endif // MICRAS_PROXY_LED_HPP 54 | -------------------------------------------------------------------------------- /micras_proxy/src/motor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/core/utils.hpp" 8 | #include "micras/proxy/motor.hpp" 9 | 10 | namespace micras::proxy { 11 | Motor::Motor(const Config& config) : 12 | backwards_pwm{config.backwards_pwm}, 13 | forward_pwm{config.forward_pwm}, 14 | max_stopped_command{config.max_stopped_command}, 15 | deadzone{config.deadzone} { 16 | this->set_command(0.0F); 17 | } 18 | 19 | void Motor::set_command(float command) { 20 | const bool is_positive = (command >= 0.0F); 21 | command = std::abs(command); 22 | 23 | if (command <= max_stopped_command) { 24 | command = 0.0F; 25 | } else { 26 | command = core::remap(command, 0.0F, 100.0F, this->deadzone, 100.0F); 27 | } 28 | 29 | if (is_positive) { 30 | this->forward_pwm.set_duty_cycle(command); 31 | this->backwards_pwm.set_duty_cycle(0.0F); 32 | } else { 33 | this->forward_pwm.set_duty_cycle(0.0F); 34 | this->backwards_pwm.set_duty_cycle(command); 35 | } 36 | } 37 | } // namespace micras::proxy 38 | -------------------------------------------------------------------------------- /micras_hal/src/pwm_dma.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | #include "micras/hal/pwm_dma.hpp" 9 | 10 | namespace micras::hal { 11 | PwmDma::PwmDma(const Config& config) : handle{config.handle}, channel{config.timer_channel} { 12 | config.init_function(); 13 | } 14 | 15 | void PwmDma::start_dma(std::span buffer) { 16 | if (this->is_busy()) { 17 | return; 18 | } 19 | 20 | HAL_TIM_PWM_Start_DMA(this->handle, this->channel, buffer.data(), buffer.size()); 21 | } 22 | 23 | void PwmDma::start_dma(std::span buffer) { 24 | start_dma({std::bit_cast(buffer.data()), buffer.size()}); 25 | } 26 | 27 | void PwmDma::stop_dma() { 28 | HAL_TIM_PWM_Stop_DMA(this->handle, this->channel); 29 | } 30 | 31 | uint32_t PwmDma::get_compare(float duty_cycle) const { 32 | return std::lround((duty_cycle * (__HAL_TIM_GET_AUTORELOAD(this->handle) + 1) / 100) - 1); 33 | } 34 | 35 | bool PwmDma::is_busy() { 36 | return TIM_CHANNEL_STATE_GET(this->handle, this->channel) == HAL_TIM_CHANNEL_STATE_BUSY; 37 | } 38 | } // namespace micras::hal 39 | -------------------------------------------------------------------------------- /micras_proxy/src/buzzer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/proxy/buzzer.hpp" 6 | 7 | namespace micras::proxy { 8 | Buzzer::Buzzer(const Config& config) : pwm{config.pwm} { 9 | this->stop(); 10 | } 11 | 12 | void Buzzer::play(uint32_t frequency, uint32_t duration) { 13 | this->pwm.set_frequency(frequency); 14 | this->pwm.set_duty_cycle(50.0F); 15 | this->is_playing = true; 16 | this->duration = duration; 17 | 18 | if (duration > 0) { 19 | this->stopwatch.reset_ms(); 20 | } 21 | } 22 | 23 | void Buzzer::update() { 24 | if (this->is_playing and this->duration > 0 and this->stopwatch.elapsed_time_ms() > this->duration) { 25 | this->stop(); 26 | } 27 | } 28 | 29 | void Buzzer::wait(uint32_t interval) { 30 | while (this->duration > 0 and this->is_playing) { 31 | this->update(); 32 | } 33 | 34 | this->stopwatch.reset_ms(); 35 | 36 | while (this->stopwatch.elapsed_time_ms() < interval) { } 37 | } 38 | 39 | void Buzzer::stop() { 40 | this->is_playing = false; 41 | this->pwm.set_duty_cycle(0.0F); 42 | } 43 | } // namespace micras::proxy 44 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Team Micras 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/crc.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_CRC_HPP 6 | #define MICRAS_HAL_CRC_HPP 7 | 8 | #include 9 | 10 | namespace micras::hal { 11 | /** 12 | * @brief Class to handle the cyclic redundancy check peripheral on STM32 microcontrollers. 13 | */ 14 | class Crc { 15 | public: 16 | /** 17 | * @brief CRC configuration struct. 18 | */ 19 | struct Config { 20 | CRC_HandleTypeDef* handle; 21 | }; 22 | 23 | /** 24 | * @brief Construct a new Crc object. 25 | * 26 | * @param config Configuration for the CRC. 27 | */ 28 | explicit Crc(const Config& config); 29 | 30 | /** 31 | * @brief Calculate the CRC value. 32 | * 33 | * @param data Data to calculate the CRC. 34 | * @param size Size of the buffer. 35 | * @return CRC value. 36 | */ 37 | uint32_t calculate(uint32_t data[], uint32_t size); // NOLINT(*-avoid-c-arrays) 38 | 39 | private: 40 | /** 41 | * @brief CRC handle. 42 | */ 43 | CRC_HandleTypeDef* handle; 44 | }; 45 | } // namespace micras::hal 46 | 47 | #endif // MICRAS_HAL_CRC_HPP 48 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | docs/ 2 | build/ 3 | cube/* 4 | !**/*.ioc 5 | 6 | .cache/ 7 | 8 | # Created by https://www.toptal.com/developers/gitignore/api/visualstudiocode,c++ 9 | # Edit at https://www.toptal.com/developers/gitignore?templates=visualstudiocode,c++ 10 | 11 | ### C++ ### 12 | # Prerequisites 13 | *.d 14 | 15 | # Compiled Object files 16 | *.slo 17 | *.lo 18 | *.o 19 | *.obj 20 | 21 | # Precompiled Headers 22 | *.gch 23 | *.pch 24 | 25 | # Compiled Dynamic libraries 26 | *.so 27 | *.dylib 28 | *.dll 29 | 30 | # Fortran module files 31 | *.mod 32 | *.smod 33 | 34 | # Compiled Static libraries 35 | *.lai 36 | *.la 37 | *.a 38 | *.lib 39 | 40 | # Executables 41 | *.exe 42 | *.out 43 | *.app 44 | 45 | ### VisualStudioCode ### 46 | .vscode/* 47 | !.vscode/settings.json 48 | !.vscode/extensions.json 49 | 50 | # Local History for Visual Studio Code 51 | .history/ 52 | 53 | # Built Visual Studio Code Extensions 54 | *.vsix 55 | 56 | ### VisualStudioCode Patch ### 57 | # Ignore all local history of files 58 | .history 59 | .ionide 60 | 61 | # End of https://www.toptal.com/developers/gitignore/api/visualstudiocode,c++ 62 | 63 | ### Clangd ### 64 | .clangd 65 | -------------------------------------------------------------------------------- /tests/include/test_core.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_TEST_CORE_HPP 6 | #define MICRAS_TEST_CORE_HPP 7 | 8 | #include "micras/hal/mcu.hpp" 9 | #include "target.hpp" 10 | 11 | namespace micras { 12 | template 13 | concept VoidFunction = requires(F void_function) { 14 | { void_function() } -> std::same_as; 15 | }; 16 | 17 | /** 18 | * @brief Core class to the tests. 19 | */ 20 | class TestCore { 21 | public: 22 | /** 23 | * @brief Delete the default constructor. 24 | */ 25 | TestCore() = delete; 26 | 27 | /** 28 | * @brief Initialize the test core. 29 | * 30 | * @param argc Number of main arguments. 31 | * @param argv Main arguments. 32 | */ 33 | static void init(int /*argc*/ = 0, char** /*argv*/ = nullptr) { hal::Mcu::init(); } 34 | 35 | /** 36 | * @brief Loop the test core with a custom function. 37 | * 38 | * @param loop_func Custom loop function. 39 | */ 40 | static void loop(VoidFunction auto loop_func) { 41 | while (true) { 42 | loop_func(); 43 | } 44 | } 45 | }; 46 | } // namespace micras 47 | 48 | #endif // MICRAS_TEST_CORE_HPP 49 | -------------------------------------------------------------------------------- /.docker/Dockerfile: -------------------------------------------------------------------------------- 1 | ################################## 2 | # Base image for Micras Firmware # 3 | ################################## 4 | FROM thunderatz/stm32cubemx:6.13.0-g4 AS base 5 | 6 | RUN apt-get update -y > /dev/null && \ 7 | apt-get upgrade -y > /dev/null && \ 8 | apt-get install -y \ 9 | gcc-arm-none-eabi \ 10 | make \ 11 | cmake \ 12 | clang-tidy \ 13 | clang-format > /dev/null\ 14 | && apt-get clean > /dev/null 15 | 16 | WORKDIR /project 17 | COPY ./cube/*.ioc /project/cube/ 18 | 19 | ENV CUBE_CMD=${CUBE_PATH}/STM32CubeMX 20 | 21 | ENV DISPLAY=:10 22 | ARG PROJECT_NAME=micras 23 | 24 | RUN Xvfb :10 -ac & \ 25 | echo "config load /project/cube/${PROJECT_NAME}.ioc\nproject generate\nexit\n" > .cube && \ 26 | $CUBE_CMD -q /project/.cube && \ 27 | rm .cube && \ 28 | pkill -f Xvfb 29 | 30 | RUN rm /tmp/.X10-lock 31 | 32 | RUN echo "trap 'chown -R ubuntu /project' EXIT" >> "/root/.bashrc" 33 | 34 | ################################### 35 | # Build image for Micras Firmware # 36 | ################################### 37 | FROM base AS build 38 | 39 | COPY . /project 40 | 41 | RUN mkdir -p build && \ 42 | cmake -B build -DBUILD_TYPE=Release 43 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/encoder.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_ENCODER_HPP 6 | #define MICRAS_HAL_ENCODER_HPP 7 | 8 | #include 9 | 10 | namespace micras::hal { 11 | /** 12 | * @brief Class to handle encoder peripheral on STM32 microcontrollers. 13 | */ 14 | class Encoder { 15 | public: 16 | /** 17 | * @brief Encoder configuration struct. 18 | */ 19 | struct Config { 20 | void (*init_function)(); 21 | TIM_HandleTypeDef* handle; 22 | uint32_t timer_channel; 23 | }; 24 | 25 | /** 26 | * @brief Construct a new Encoder object. 27 | * 28 | * @param config Configuration for the encoder. 29 | */ 30 | explicit Encoder(const Config& config); 31 | 32 | /** 33 | * @brief Get the counter value. 34 | * 35 | * @return Current value of the counter. 36 | */ 37 | int32_t get_counter() const; 38 | 39 | private: 40 | /** 41 | * @brief Timer handle. 42 | */ 43 | TIM_HandleTypeDef* handle; 44 | 45 | /** 46 | * @brief Start value of the timer counter. 47 | */ 48 | uint32_t start_count; 49 | }; 50 | } // namespace micras::hal 51 | 52 | #endif // MICRAS_HAL_ENCODER_HPP 53 | -------------------------------------------------------------------------------- /.github/workflows/release.yaml: -------------------------------------------------------------------------------- 1 | name: Add binaries to release 2 | 3 | on: 4 | release: 5 | types: [published] 6 | 7 | permissions: 8 | contents: write 9 | 10 | jobs: 11 | build: 12 | runs-on: ubuntu-latest 13 | steps: 14 | - name: 🔀 Checkout 15 | uses: actions/checkout@v4 16 | with: 17 | submodules: recursive 18 | 19 | - name: 🔧 Set up Docker Buildx 20 | uses: docker/setup-buildx-action@v3 21 | 22 | - name: 🐋 Build Docker image 23 | timeout-minutes: 10 24 | uses: docker/build-push-action@v6 25 | with: 26 | context: . 27 | file: .docker/Dockerfile 28 | target: build 29 | load: true 30 | cache-from: type=gha 31 | cache-to: type=gha,mode=max 32 | no-cache-filters: build 33 | tags: project:build 34 | 35 | - name: 🔨 Build project 36 | run: | 37 | docker run --name release project:build make -C build --no-print-directory -j 38 | docker cp release:/project/build/ output/ 39 | 40 | - name: ⬆️ Upload binaries 41 | run: gh release upload ${{ github.event.release.tag_name }} output/*.hex output/*.elf 42 | env: 43 | GITHUB_TOKEN: ${{ github.token }} 44 | -------------------------------------------------------------------------------- /tests/src/proxy/test_dip_switch.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | static volatile uint8_t test_dip_switch_value{}; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) 10 | 11 | int main(int argc, char* argv[]) { 12 | TestCore::init(argc, argv); 13 | proxy::DipSwitch dip_switch{dip_switch_config}; 14 | proxy::Argb argb{argb_config}; 15 | proxy::Argb::Color color{}; 16 | 17 | TestCore::loop([&dip_switch, &argb, &color]() { 18 | color = {0, 0, 0}; 19 | test_dip_switch_value = dip_switch.get_switches_value(); 20 | 21 | if (dip_switch.get_switch_state(0)) { 22 | color.red = 127; 23 | } 24 | 25 | if (dip_switch.get_switch_state(1)) { 26 | color.green = 127; 27 | } 28 | 29 | if (dip_switch.get_switch_state(2)) { 30 | color.blue = 127; 31 | } 32 | 33 | if (dip_switch.get_switch_state(3)) { 34 | color.red *= 2; 35 | color.green *= 2; 36 | color.blue *= 2; 37 | } 38 | 39 | argb.set_color(color); 40 | proxy::Stopwatch::sleep_ms(2); 41 | }); 42 | 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /micras_core/src/vector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/core/vector.hpp" 8 | 9 | namespace micras::core { 10 | float Vector::distance(const Vector& other) const { 11 | return std::hypot(other.x - this->x, other.y - this->y); 12 | } 13 | 14 | float Vector::magnitude() const { 15 | return std::hypot(this->x, this->y); 16 | } 17 | 18 | float Vector::angle_between(const Vector& other) const { 19 | return std::atan2(other.y - this->y, other.x - this->x); 20 | } 21 | 22 | Vector Vector::move_towards(const Vector& other, float distance) const { 23 | const float angle = this->angle_between(other); 24 | return {this->x + distance * std::cos(angle), this->y + distance * std::sin(angle)}; 25 | } 26 | 27 | Vector Vector::operator+(const Vector& other) const { 28 | return {this->x + other.x, this->y + other.y}; 29 | } 30 | 31 | Vector Vector::operator-(const Vector& other) const { 32 | return {this->x - other.x, this->y - other.y}; 33 | } 34 | 35 | bool Vector::operator==(const Vector& other) const { 36 | return this->x == other.x and this->y == other.y; 37 | } 38 | 39 | Vector Vector::operator%(float value) const { 40 | return {std::fmod(this->x, value), std::fmod(this->y, value)}; 41 | } 42 | } // namespace micras::core 43 | -------------------------------------------------------------------------------- /micras_hal/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(micras_hal) 2 | 3 | get_target_property(CUBE_INCLUDE_DIRECTORIES stm32cubemx INTERFACE_INCLUDE_DIRECTORIES) 4 | get_target_property(CUBE_SOURCES stm32cubemx INTERFACE_SOURCES) 5 | get_target_property(CUBE_COMPILE_DEFINITIONS stm32cubemx INTERFACE_COMPILE_DEFINITIONS) 6 | 7 | # Remove warnings from Cube sources 8 | set_source_files_properties( 9 | ${CUBE_SOURCES} 10 | PROPERTIES 11 | COMPILE_FLAGS "-w" 12 | ) 13 | 14 | file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") 15 | 16 | add_library(${PROJECT_NAME} STATIC 17 | ${PROJECT_SOURCES} 18 | ) 19 | 20 | target_include_directories(${PROJECT_NAME} PUBLIC 21 | include 22 | ) 23 | 24 | target_link_libraries(${PROJECT_NAME} PRIVATE 25 | stm32cubemx 26 | ) 27 | 28 | target_link_libraries(${PROJECT_NAME} PUBLIC 29 | micras::core 30 | ) 31 | 32 | target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC 33 | ${CUBE_INCLUDE_DIRECTORIES} 34 | ) 35 | 36 | target_compile_definitions(${PROJECT_NAME} PUBLIC 37 | ${CUBE_COMPILE_DEFINITIONS} 38 | ) 39 | 40 | add_library(micras_hal_whole_archive INTERFACE) 41 | add_library(micras::hal ALIAS micras_hal_whole_archive) 42 | target_link_libraries(micras_hal_whole_archive INTERFACE 43 | "-Wl,--whole-archive" ${PROJECT_NAME} "-Wl,--no-whole-archive" 44 | ) 45 | -------------------------------------------------------------------------------- /tests/src/proxy/test_wall_sensors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | // NOLINTBEGIN(*-avoid-c-arrays, cppcoreguidelines-avoid-non-const-global-variables) 10 | static volatile float test_reading[4]; 11 | static volatile float test_adc_reading[4]; 12 | 13 | // NOLINTEND(*-avoid-c-arrays, cppcoreguidelines-avoid-non-const-global-variables) 14 | 15 | int main(int argc, char* argv[]) { 16 | TestCore::init(argc, argv); 17 | proxy::WallSensors wall_sensors{wall_sensors_config}; 18 | proxy::Argb argb{argb_config}; 19 | 20 | wall_sensors.turn_on(); 21 | 22 | TestCore::loop([&wall_sensors, &argb]() { 23 | wall_sensors.update(); 24 | 25 | for (uint8_t i = 0; i < 4; i++) { 26 | test_reading[i] = wall_sensors.get_reading(i); 27 | test_adc_reading[i] = wall_sensors.get_adc_reading(i); 28 | } 29 | 30 | for (uint8_t i = 0; i < 2; i++) { 31 | proxy::Argb::Color color{}; 32 | color.red = wall_sensors.get_wall(3 - 3 * i) ? 255 : 0; 33 | color.blue = wall_sensors.get_wall(2 - i) ? 255 : 0; 34 | 35 | argb.set_color(color, i); 36 | } 37 | 38 | proxy::Stopwatch::sleep_ms(2); 39 | }); 40 | 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /micras_proxy/src/stopwatch.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/proxy/stopwatch.hpp" 8 | 9 | namespace micras::proxy { 10 | Stopwatch::Stopwatch() { 11 | this->reset_ms(); 12 | } 13 | 14 | Stopwatch::Stopwatch(const Config& config) : timer{config.timer} { 15 | this->reset_us(); 16 | } 17 | 18 | void Stopwatch::reset_ms() { 19 | this->counter = hal::Timer::get_counter_ms(); 20 | } 21 | 22 | void Stopwatch::reset_us() { 23 | this->counter = this->timer.get_counter_us(); 24 | } 25 | 26 | uint32_t Stopwatch::elapsed_time_ms() const { 27 | return hal::Timer::get_counter_ms() - this->counter; 28 | } 29 | 30 | uint32_t Stopwatch::elapsed_time_us() const { 31 | const uint32_t counter = this->timer.get_counter_us(); 32 | 33 | if (counter < this->counter) { 34 | return std::numeric_limits::max() - this->counter + counter; 35 | } 36 | 37 | return counter - this->counter; 38 | } 39 | 40 | void Stopwatch::sleep_ms(uint32_t time) { 41 | const uint32_t start = HAL_GetTick(); 42 | 43 | while (HAL_GetTick() - start < time) { } 44 | } 45 | 46 | void Stopwatch::sleep_us(uint32_t time) const { 47 | const uint32_t start = this->timer.get_counter_us(); 48 | 49 | while (this->timer.get_counter_us() - start < time) { } 50 | } 51 | } // namespace micras::proxy 52 | -------------------------------------------------------------------------------- /include/micras/states/idle.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef IDLE_STATE_HPP 6 | #define IDLE_STATE_HPP 7 | 8 | #include "micras/states/base.hpp" 9 | 10 | namespace micras { 11 | class IdleState : public BaseState { 12 | public: 13 | using BaseState::BaseState; 14 | 15 | /** 16 | * @brief Execute the entry function of this state. 17 | */ 18 | void on_entry() override { 19 | this->micras.stop(); 20 | this->micras.reset(); 21 | } 22 | 23 | /** 24 | * @brief Execute this state. 25 | * 26 | * @return The id of the next state. 27 | */ 28 | uint8_t execute() override { 29 | if (this->micras.acknowledge_event(Interface::Event::EXPLORE)) { 30 | this->micras.set_objective(core::Objective::EXPLORE); 31 | 32 | return Micras::State::WAIT_FOR_RUN; 33 | } 34 | 35 | if (this->micras.acknowledge_event(Interface::Event::SOLVE)) { 36 | this->micras.set_objective(core::Objective::SOLVE); 37 | this->micras.load_best_route(); 38 | 39 | return Micras::State::WAIT_FOR_RUN; 40 | } 41 | 42 | if (this->micras.acknowledge_event(Interface::Event::CALIBRATE)) { 43 | return Micras::State::WAIT_FOR_CALIBRATE; 44 | } 45 | 46 | return this->get_id(); 47 | } 48 | }; 49 | } // namespace micras 50 | 51 | #endif // IDLE_STATE_HPP 52 | -------------------------------------------------------------------------------- /tests/src/proxy/test_buzzer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "test_core.hpp" 8 | 9 | using namespace micras; // NOLINT(google-build-using-namespace) 10 | 11 | static constexpr std::array frequencies{ 12 | 987, 987, 987, 987, 987, 1046, 1318, 987, 987, 987, 1046, 1318, 987, 987, 987, 1046, 1318, 1479, 1479, 1760, 13 | 1567, 1479, 1318, 1318, 1760, 1567, 1479, 1318, 1318, 987, 1046, 1318, 987, 987, 987, 1046, 1318, 987, 987, 14 | }; 15 | static constexpr std::array intervals{ 16 | 333, 333, 333, 333, 250, 250, 167, 333, 333, 250, 250, 167, 333, 333, 250, 250, 167, 333, 333, 250, 17 | 250, 167, 333, 333, 250, 250, 167, 333, 333, 250, 250, 167, 333, 333, 250, 250, 167, 333, 0, 18 | }; 19 | static constexpr uint32_t duration{133}; 20 | 21 | int main(int argc, char* argv[]) { 22 | TestCore::init(argc, argv); 23 | proxy::Button button{button_config}; 24 | proxy::Buzzer buzzer{buzzer_config}; 25 | 26 | TestCore::loop([&button, &buzzer]() { 27 | while (button.get_status() == proxy::Button::Status::NO_PRESS) { 28 | button.update(); 29 | } 30 | 31 | for (uint32_t i = 0; i < frequencies.size(); i++) { 32 | buzzer.play(frequencies.at(i), duration); 33 | buzzer.wait(intervals.at(i)); 34 | } 35 | }); 36 | 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | AccessModifierOffset: -4 4 | AlignAfterOpenBracket: BlockIndent 5 | AlignConsecutiveBitFields: 6 | Enabled: true 7 | PadOperators: true 8 | AlignConsecutiveDeclarations: 9 | Enabled: true 10 | PadOperators: true 11 | AlignConsecutiveMacros: 12 | PadOperators: true 13 | AlignEscapedNewlines: Left 14 | AllowShortBlocksOnASingleLine: Empty 15 | AllowShortEnumsOnASingleLine: false 16 | AllowShortFunctionsOnASingleLine: Inline 17 | AlwaysBreakTemplateDeclarations: Yes 18 | BreakBeforeTernaryOperators: false 19 | BreakConstructorInitializers: AfterColon 20 | BreakInheritanceList: AfterColon 21 | ColumnLimit: 120 22 | EmptyLineBeforeAccessModifier: Always 23 | IncludeCategories: 24 | - Regex: '^<.*>' 25 | Priority: 1 26 | SortPriority: 1 27 | - Regex: '^".*"' 28 | SortPriority: 2 29 | IncludeIsMainRegex: '$' 30 | IndentCaseLabels: true 31 | IndentExternBlock: NoIndent 32 | IndentPPDirectives: BeforeHash 33 | IndentRequiresClause: false 34 | IndentWidth: 4 35 | IndentWrappedFunctionNames: true 36 | KeepEmptyLinesAtTheStartOfBlocks: false 37 | PackConstructorInitializers: NextLine 38 | PointerAlignment: Left 39 | SeparateDefinitionBlocks: Always 40 | ShortNamespaceLines: 0 41 | SortIncludes: Never 42 | SpaceInEmptyBlock: true 43 | SpacesBeforeTrailingComments: 2 44 | TabWidth: 4 45 | ... 46 | -------------------------------------------------------------------------------- /micras_core/include/micras/core/serializable.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_CORE_SERIALIZABLE_HPP 6 | #define MICRAS_CORE_SERIALIZABLE_HPP 7 | 8 | #include 9 | #include 10 | 11 | namespace micras::core { 12 | /** 13 | * @brief Interface class for serializable classes. 14 | */ 15 | class ISerializable { 16 | public: 17 | /** 18 | * @brief Virtual destructor for the ISerializable class. 19 | */ 20 | virtual ~ISerializable() = default; 21 | 22 | /** 23 | * @brief Serialize the class instance. 24 | * 25 | * @return Serialized data. 26 | */ 27 | virtual std::vector serialize() const = 0; 28 | 29 | /** 30 | * @brief Deserialize the class instance. 31 | * 32 | * @param serial_data Serialized data. 33 | * @param size Size of the serialized data. 34 | */ 35 | virtual void deserialize(const uint8_t* serial_data, uint16_t size) = 0; 36 | 37 | protected: 38 | /** 39 | * @brief Special member functions declared as default. 40 | */ 41 | ///@{ 42 | ISerializable() = default; 43 | ISerializable(const ISerializable&) = default; 44 | ISerializable(ISerializable&&) = default; 45 | ISerializable& operator=(const ISerializable&) = default; 46 | ISerializable& operator=(ISerializable&&) = default; 47 | ///@} 48 | }; 49 | } // namespace micras::core 50 | 51 | #endif // MICRAS_CORE_SERIALIZABLE_HPP 52 | -------------------------------------------------------------------------------- /micras_proxy/src/locomotion.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/proxy/locomotion.hpp" 8 | 9 | namespace micras::proxy { 10 | Locomotion::Locomotion(const Config& config) : 11 | left_motor{config.left_motor}, right_motor{config.right_motor}, enable_gpio{config.enable_gpio} { 12 | this->stop(); 13 | this->disable(); 14 | } 15 | 16 | void Locomotion::enable() { 17 | this->enable_gpio.write(true); 18 | } 19 | 20 | void Locomotion::disable() { 21 | this->enable_gpio.write(false); 22 | } 23 | 24 | void Locomotion::set_wheel_command(float left_command, float right_command) { 25 | this->left_motor.set_command(left_command); 26 | this->right_motor.set_command(right_command); 27 | } 28 | 29 | void Locomotion::set_command(float linear, float angular) { 30 | float left_command = linear - angular; 31 | float right_command = linear + angular; 32 | 33 | if (std::abs(left_command) > 100.0F) { 34 | left_command *= 100.0F / std::abs(left_command); 35 | right_command *= 100.0F / std::abs(left_command); 36 | } 37 | 38 | if (std::abs(right_command) > 100.0F) { 39 | left_command *= 100.0F / std::abs(right_command); 40 | right_command *= 100.0F / std::abs(right_command); 41 | } 42 | 43 | this->set_wheel_command(left_command, right_command); 44 | } 45 | 46 | void Locomotion::stop() { 47 | this->set_wheel_command(0.0F, 0.0F); 48 | } 49 | } // namespace micras::proxy 50 | -------------------------------------------------------------------------------- /src/interface.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/interface.hpp" 6 | 7 | namespace micras { 8 | Interface::Interface( 9 | const std::shared_ptr>& Argb, const std::shared_ptr& button, 10 | const std::shared_ptr& buzzer, const std::shared_ptr>& dip_switch, 11 | const std::shared_ptr& led 12 | ) : 13 | argb{Argb}, button{button}, buzzer{buzzer}, dip_switch{dip_switch}, led{led} { } 14 | 15 | void Interface::update() { 16 | if (this->button->get_status() == proxy::Button::Status::SHORT_PRESS) { 17 | this->send_event(Event::EXPLORE); 18 | } else if (this->button->get_status() == proxy::Button::Status::LONG_PRESS) { 19 | this->send_event(Event::SOLVE); 20 | } else if (this->button->get_status() == proxy::Button::Status::EXTRA_LONG_PRESS) { 21 | this->send_event(Event::CALIBRATE); 22 | } 23 | 24 | if (this->acknowledge_event(Event::ERROR)) { 25 | this->led->turn_on(); 26 | } 27 | } 28 | 29 | void Interface::send_event(Event event) { 30 | this->events.at(event) = true; 31 | } 32 | 33 | bool Interface::acknowledge_event(Event event) { 34 | if (this->events.at(event)) { 35 | this->events.at(event) = false; 36 | return true; 37 | } 38 | 39 | return false; 40 | } 41 | 42 | bool Interface::peek_event(Event event) const { 43 | return this->events.at(event); 44 | } 45 | } // namespace micras 46 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/timer.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_TIMER_HPP 6 | #define MICRAS_HAL_TIMER_HPP 7 | 8 | #include 9 | #include 10 | 11 | namespace micras::hal { 12 | /** 13 | * @brief Class to handle timer peripheral on STM32 microcontrollers. 14 | */ 15 | class Timer { 16 | public: 17 | /** 18 | * @brief Timer configuration struct. 19 | */ 20 | struct Config { 21 | void (*init_function)(); 22 | TIM_HandleTypeDef* handle; 23 | }; 24 | 25 | /** 26 | * @brief Construct a new Timer object. 27 | */ 28 | Timer() = default; 29 | 30 | /** 31 | * @brief Construct a new Timer object. 32 | * 33 | * @param config Configuration for the timer. 34 | */ 35 | explicit Timer(const Config& config); 36 | 37 | /** 38 | * @brief Get the current timer counter. 39 | * 40 | * @return Current timer counter in milliseconds. 41 | */ 42 | static uint32_t get_counter_ms(); 43 | 44 | /** 45 | * @brief Get the current timer counter. 46 | * 47 | * @return Current timer counter in microseconds. 48 | */ 49 | uint32_t get_counter_us() const; 50 | 51 | private: 52 | /** 53 | * @brief Timer handle. 54 | */ 55 | TIM_HandleTypeDef* handle{}; 56 | 57 | /** 58 | * @brief Flag to enable microseconds. 59 | */ 60 | bool enable_microseconds{false}; 61 | }; 62 | } // namespace micras::hal 63 | 64 | #endif // MICRAS_HAL_TIMER_HPP 65 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/gpio.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_GPIO_HPP 6 | #define MICRAS_HAL_GPIO_HPP 7 | 8 | #include 9 | #include 10 | 11 | namespace micras::hal { 12 | /** 13 | * @brief Class to handle GPIO pins on STM32 microcontrollers. 14 | */ 15 | class Gpio { 16 | public: 17 | /** 18 | * @brief Configuration struct for GPIO pin. 19 | */ 20 | struct Config { 21 | GPIO_TypeDef* port; 22 | uint16_t pin; 23 | }; 24 | 25 | /** 26 | * @brief Construct a new Gpio object. 27 | * 28 | * @param config Configuration for the GPIO pin. 29 | */ 30 | explicit Gpio(const Config& config); 31 | 32 | /** 33 | * @brief Read the current state of the GPIO pin. 34 | * 35 | * @return True if the current state of the GPIO pin is high, false otherwise. 36 | */ 37 | bool read() const; 38 | 39 | /** 40 | * @brief Write a new state to the GPIO pin. 41 | * 42 | * @param pin_state The state to be written (true for high, false for low). 43 | */ 44 | void write(bool state); 45 | 46 | /** 47 | * @brief Toggle the state of the GPIO pin. 48 | */ 49 | void toggle(); 50 | 51 | private: 52 | /** 53 | * @brief The port of the GPIO. 54 | */ 55 | GPIO_TypeDef* port; 56 | 57 | /** 58 | * @brief The pin number of the GPIO. 59 | */ 60 | uint16_t pin; 61 | }; 62 | } // namespace micras::hal 63 | 64 | #endif // MICRAS_HAL_GPIO_HPP 65 | -------------------------------------------------------------------------------- /tests/src/proxy/test_argb.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | int main(int argc, char* argv[]) { 10 | TestCore::init(argc, argv); 11 | 12 | proxy::Argb argb{argb_config}; 13 | proxy::Argb::Color color{255, 0, 0}; 14 | 15 | TestCore::loop([&argb, &color]() { 16 | for (uint8_t i = 1; i > 0; i++) { 17 | color.blue = i; 18 | argb.set_color(color); 19 | proxy::Stopwatch::sleep_ms(2); 20 | } 21 | 22 | for (uint8_t i = 254; i < 255; i--) { 23 | color.red = i; 24 | argb.set_color(color); 25 | proxy::Stopwatch::sleep_ms(2); 26 | } 27 | 28 | for (uint8_t i = 1; i > 0; i++) { 29 | color.green = i; 30 | argb.set_color(color); 31 | proxy::Stopwatch::sleep_ms(2); 32 | } 33 | 34 | for (uint8_t i = 254; i < 255; i--) { 35 | color.blue = i; 36 | argb.set_color(color); 37 | proxy::Stopwatch::sleep_ms(2); 38 | } 39 | 40 | for (uint8_t i = 1; i > 0; i++) { 41 | color.red = i; 42 | argb.set_color(color); 43 | proxy::Stopwatch::sleep_ms(2); 44 | } 45 | 46 | for (uint8_t i = 254; i < 255; i--) { 47 | color.green = i; 48 | argb.set_color(color); 49 | proxy::Stopwatch::sleep_ms(2); 50 | } 51 | }); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | --- 2 | Checks: 3 | -*, 4 | bugprone-*, 5 | -bugprone-easily-swappable-parameters, 6 | -bugprone-dynamic-static-initializers, 7 | -bugprone-bitwise-pointer-cast, 8 | clang-*, 9 | cppcoreguidelines-*, 10 | -cppcoreguidelines-avoid-magic-numbers, 11 | -cppcoreguidelines-avoid-do-while, 12 | -cppcoreguidelines-narrowing-conversions, 13 | -cppcoreguidelines-pro-type-union-access, 14 | google-*, 15 | llvm-*, 16 | -llvm-header-guard, 17 | misc-*, 18 | -misc-include-cleaner, 19 | -misc-use-anonymous-namespace, 20 | modernize-*, 21 | -modernize-use-nodiscard, 22 | -modernize-use-trailing-return-type, 23 | performance-*, 24 | readability-*, 25 | -readability-duplicate-include, 26 | -readability-magic-numbers, 27 | -readability-math-missing-parentheses, 28 | HeaderFileExtensions: 29 | - '' 30 | - h 31 | - hh 32 | - hpp 33 | - hxx 34 | ImplementationFileExtensions: 35 | - c 36 | - cc 37 | - cpp 38 | - cxx 39 | HeaderFilterRegex: '(include|config)/.*.hpp' 40 | FormatStyle: none 41 | CheckOptions: 42 | readability-identifier-length.MinimumVariableNameLength: 2 43 | readability-identifier-length.IgnoredVariableNames: ^[nxyz]$ 44 | readability-identifier-length.MinimumParameterNameLength: 2 45 | readability-identifier-length.IgnoredParameterNames: ^[nxyz]$ 46 | misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic: true 47 | bugprone-narrowing-conversions.WarnOnIntegerToFloatingPointNarrowingConversion: false 48 | SystemHeaders: false 49 | ... 50 | -------------------------------------------------------------------------------- /include/micras/states/run.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef RUN_STATE_HPP 6 | #define RUN_STATE_HPP 7 | 8 | #include "micras/states/base.hpp" 9 | 10 | namespace micras { 11 | class RunState : public BaseState { 12 | public: 13 | using BaseState::BaseState; 14 | 15 | /** 16 | * @brief Execute the entry function of this state. 17 | */ 18 | void on_entry() override { 19 | this->micras.init(); 20 | this->micras.prepare(); 21 | } 22 | 23 | /** 24 | * @brief Execute this state. 25 | * 26 | * @return The id of the next state. 27 | */ 28 | uint8_t execute() override { 29 | if (this->micras.check_crash()) { 30 | return Micras::State::ERROR; 31 | } 32 | 33 | if (not this->micras.run()) { 34 | return this->get_id(); 35 | } 36 | 37 | switch (this->micras.get_objective()) { 38 | case core::Objective::EXPLORE: 39 | this->micras.set_objective(core::Objective::RETURN); 40 | this->micras.save_best_route(); 41 | return Micras::State::WAIT_FOR_RUN; 42 | 43 | case core::Objective::RETURN: 44 | this->micras.set_objective(core::Objective::SOLVE); 45 | this->micras.save_best_route(); 46 | return Micras::State::IDLE; 47 | 48 | case core::Objective::SOLVE: 49 | return Micras::State::IDLE; 50 | } 51 | 52 | return Micras::State::ERROR; 53 | } 54 | }; 55 | } // namespace micras 56 | 57 | #endif // RUN_STATE_HPP 58 | -------------------------------------------------------------------------------- /cmake/templates/launch.json.in: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | { 5 | "name": "Cortex Debug (OpenOCD)", 6 | "type": "cortex-debug", 7 | "request": "launch", 8 | "servertype": "external", 9 | "cwd": "${CMAKE_CURRENT_SOURCE_DIR}", 10 | "executable": "${CMAKE_CURRENT_BINARY_DIR}/${DEBUG_FILE_NAME}.elf", 11 | "gdbPath": "gdb-multiarch", 12 | "runToEntryPoint": "main", 13 | "preLaunchTask": "Launch OpenOCD", 14 | "gdbTarget": "localhost:3333", 15 | }, 16 | { 17 | "name": "Cortex Debug (J-Link)", 18 | "type": "cortex-debug", 19 | "request": "launch", 20 | "servertype": "jlink", 21 | "cwd": "${CMAKE_CURRENT_SOURCE_DIR}", 22 | "executable": "${CMAKE_CURRENT_BINARY_DIR}/${DEBUG_FILE_NAME}.elf", 23 | "gdbPath": "gdb-multiarch", 24 | "runToEntryPoint": "main", 25 | "device": "${DEVICE}", 26 | "interface": "swd", 27 | }, 28 | { 29 | "name": "Cortex Debug (ST-Util)", 30 | "type": "cortex-debug", 31 | "request": "launch", 32 | "servertype": "stutil", 33 | "cwd": "${CMAKE_CURRENT_SOURCE_DIR}", 34 | "executable": "${CMAKE_CURRENT_BINARY_DIR}/${DEBUG_FILE_NAME}.elf", 35 | "gdbPath": "gdb-multiarch", 36 | "runToEntryPoint": "main", 37 | "device": "${DEVICE}", 38 | "v1": false 39 | }, 40 | ] 41 | } 42 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/dip_switch.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_DIP_SWITCH_HPP 6 | #define MICRAS_PROXY_DIP_SWITCH_HPP 7 | 8 | #include 9 | #include 10 | 11 | #include "micras/hal/gpio.hpp" 12 | 13 | namespace micras::proxy { 14 | /** 15 | * @brief Class for acquiring dip switch data. 16 | */ 17 | template 18 | class TDipSwitch { 19 | public: 20 | /** 21 | * @brief Configuration struct for the Dip Switch. 22 | */ 23 | struct Config { 24 | std::array gpio_array; 25 | }; 26 | 27 | /** 28 | * @brief Construct a new Dip Switch object. 29 | * 30 | * @param config Configuration struct for the DipSwitch. 31 | */ 32 | explicit TDipSwitch(const Config& config); 33 | 34 | /** 35 | * @brief Get the state of a switch. 36 | * 37 | * @param switch_index Index of the switch. 38 | * @return True if the switch is on, false otherwise. 39 | */ 40 | bool get_switch_state(uint8_t switch_index) const; 41 | 42 | /** 43 | * @brief Get the value of all switches. 44 | * 45 | * @return Value of all switches. 46 | */ 47 | uint8_t get_switches_value() const; 48 | 49 | private: 50 | /** 51 | * @brief Array of GPIOs for the switches. 52 | */ 53 | std::array gpio_array; 54 | }; 55 | } // namespace micras::proxy 56 | 57 | #include "../src/dip_switch.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) 58 | 59 | #endif // MICRAS_PROXY_DIP_SWITCH_HPP 60 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/motor.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_MOTOR_HPP 6 | #define MICRAS_PROXY_MOTOR_HPP 7 | 8 | #include "micras/hal/pwm.hpp" 9 | 10 | namespace micras::proxy { 11 | /** 12 | * @brief Class for controlling a motor driver. 13 | */ 14 | class Motor { 15 | public: 16 | /** 17 | * @brief Configuration struct for the motor. 18 | */ 19 | struct Config { 20 | hal::Pwm::Config backwards_pwm; 21 | hal::Pwm::Config forward_pwm; 22 | float max_stopped_command; 23 | float deadzone; 24 | }; 25 | 26 | /** 27 | * @brief Construct a new motor object. 28 | * 29 | * @param config Configuration for the motor driver. 30 | */ 31 | explicit Motor(const Config& config); 32 | 33 | /** 34 | * @brief Set the command for the motor. 35 | * 36 | * @param command Command for the motor in percentage. 37 | */ 38 | void set_command(float command); 39 | 40 | private: 41 | /** 42 | * @brief PWM object for controlling the motor in the backwards direction. 43 | */ 44 | hal::Pwm backwards_pwm; 45 | 46 | /** 47 | * @brief PWM object for controlling the motor in the forward direction. 48 | */ 49 | hal::Pwm forward_pwm; 50 | 51 | /** 52 | * @brief Maximum command value for the motor to be considered stopped. 53 | */ 54 | float max_stopped_command; 55 | 56 | /** 57 | * @brief Deadzone for the motor. 58 | */ 59 | float deadzone; 60 | }; 61 | } // namespace micras::proxy 62 | 63 | #endif // MICRAS_PROXY_MOTOR_HPP 64 | -------------------------------------------------------------------------------- /micras_proxy/src/fan.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/core/utils.hpp" 6 | #include "micras/proxy/fan.hpp" 7 | 8 | namespace micras::proxy { 9 | Fan::Fan(const Config& config) : 10 | pwm{config.pwm}, 11 | direction_gpio{config.direction_gpio}, 12 | enable_gpio{config.enable_gpio}, 13 | max_acceleration{config.max_acceleration} { 14 | this->stop(); 15 | this->enable(); 16 | } 17 | 18 | void Fan::enable() { 19 | this->enable_gpio.write(true); 20 | } 21 | 22 | void Fan::disable() { 23 | this->enable_gpio.write(false); 24 | } 25 | 26 | void Fan::set_speed(float speed) { 27 | this->update(); 28 | this->target_speed = speed; 29 | } 30 | 31 | float Fan::update() { 32 | this->current_speed = core::move_towards( 33 | this->current_speed, this->target_speed, this->acceleration_stopwatch.elapsed_time_ms() * this->max_acceleration 34 | ); 35 | 36 | this->acceleration_stopwatch.reset_ms(); 37 | 38 | if (this->current_speed > 0.0F) { 39 | this->set_direction(RotationDirection::FORWARD); 40 | this->pwm.set_duty_cycle(this->current_speed); 41 | } else if (this->current_speed < 0.0F) { 42 | this->set_direction(RotationDirection::BACKWARDS); 43 | this->pwm.set_duty_cycle(-this->current_speed); 44 | } else { 45 | this->stop(); 46 | } 47 | 48 | return this->current_speed; 49 | } 50 | 51 | void Fan::stop() { 52 | this->pwm.set_duty_cycle(0.0F); 53 | } 54 | 55 | void Fan::set_direction(RotationDirection direction) { 56 | this->direction_gpio.write(static_cast(direction)); 57 | } 58 | } // namespace micras::proxy 59 | -------------------------------------------------------------------------------- /include/micras/states/wait.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef WAIT_STATE_HPP 6 | #define WAIT_STATE_HPP 7 | 8 | #include "micras/states/base.hpp" 9 | 10 | namespace micras { 11 | class WaitState : public BaseState { 12 | public: 13 | /** 14 | * @brief Construct a new WaitState object. 15 | * 16 | * @param id The id of the state. 17 | * @param micras The Micras object. 18 | * @param next_state_id The id of the state to go after ending the wait. 19 | * @param wait_time_ms The time to wait in milliseconds. 20 | */ 21 | WaitState(uint8_t id, Micras& micras, uint8_t next_state_id, uint16_t wait_time_ms = 3000) : 22 | BaseState{id, micras}, next_state_id{next_state_id}, wait_time_ms{wait_time_ms} { } 23 | 24 | /** 25 | * @brief Execute the entry function of this state. 26 | */ 27 | void on_entry() override { this->wait_stopwatch.reset_ms(); } 28 | 29 | /** 30 | * @brief Execute this state. 31 | * 32 | * @return The id of the next state. 33 | */ 34 | uint8_t execute() override { 35 | if (this->wait_stopwatch.elapsed_time_ms() > this->wait_time_ms) { 36 | return this->next_state_id; 37 | } 38 | 39 | return this->get_id(); 40 | } 41 | 42 | private: 43 | /** 44 | * @brief Stopwatch for the wait status. 45 | */ 46 | proxy::Stopwatch wait_stopwatch; 47 | 48 | /** 49 | * @brief Id of the state to go after ending the wait. 50 | */ 51 | uint8_t next_state_id; 52 | 53 | /** 54 | * @brief Time to wait in milliseconds. 55 | */ 56 | uint16_t wait_time_ms; 57 | }; 58 | } // namespace micras 59 | 60 | #endif // WAIT_STATE_HPP 61 | -------------------------------------------------------------------------------- /tests/src/proxy/test_imu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | // NOLINTBEGIN(*-avoid-c-arrays, cppcoreguidelines-avoid-non-const-global-variables) 10 | static volatile float test_angular_velocity[3]{}; 11 | static volatile float test_linear_acceleration[3]{}; 12 | 13 | // NOLINTEND(*-avoid-c-arrays, cppcoreguidelines-avoid-non-const-global-variables) 14 | 15 | int main(int argc, char* argv[]) { 16 | TestCore::init(argc, argv); 17 | 18 | proxy::Imu imu{imu_config}; 19 | proxy::Argb argb{argb_config}; 20 | proxy::Stopwatch stopwatch{stopwatch_config}; 21 | 22 | proxy::Stopwatch::sleep_ms(2); 23 | 24 | if (imu.was_initialized()) { 25 | argb.set_color(proxy::Argb::Colors::green); 26 | 27 | } else { 28 | argb.set_color(proxy::Argb::Colors::red); 29 | 30 | while (true) { } 31 | } 32 | 33 | proxy::Stopwatch::sleep_ms(1000); 34 | imu.calibrate(); 35 | 36 | TestCore::loop([&imu, &stopwatch]() { 37 | stopwatch.reset_us(); 38 | imu.update(); 39 | 40 | test_angular_velocity[0] = imu.get_angular_velocity(proxy::Imu::Axis::X); 41 | test_angular_velocity[1] = imu.get_angular_velocity(proxy::Imu::Axis::Y); 42 | test_angular_velocity[2] = imu.get_angular_velocity(proxy::Imu::Axis::Z); 43 | 44 | test_linear_acceleration[0] = imu.get_linear_acceleration(proxy::Imu::Axis::X); 45 | test_linear_acceleration[1] = imu.get_linear_acceleration(proxy::Imu::Axis::Y); 46 | test_linear_acceleration[2] = imu.get_linear_acceleration(proxy::Imu::Axis::Z); 47 | 48 | while (stopwatch.elapsed_time_us() < 1050) { } 49 | }); 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /micras_nav/src/state.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/nav/state.hpp" 8 | 9 | namespace micras::nav { 10 | GridPose Pose::to_grid(float cell_size) const { 11 | return {GridPoint::from_vector(this->position, cell_size), angle_to_grid(this->orientation)}; 12 | }; 13 | 14 | core::Vector Pose::to_cell(float cell_size) const { 15 | const core::Vector cell_position = this->position % cell_size; 16 | 17 | float cell_advance = 0.0F; 18 | float cell_alignment = 0.0F; 19 | 20 | switch (angle_to_grid(this->orientation)) { 21 | case Side::RIGHT: 22 | cell_advance = cell_position.x; 23 | cell_alignment = cell_size / 2.0F - cell_position.y; 24 | break; 25 | case Side::UP: 26 | cell_advance = cell_position.y; 27 | cell_alignment = cell_position.x - cell_size / 2.0F; 28 | break; 29 | case Side::LEFT: 30 | cell_advance = cell_size - cell_position.x; 31 | cell_alignment = cell_position.y - cell_size / 2.0F; 32 | break; 33 | case Side::DOWN: 34 | cell_advance = cell_size - cell_position.y; 35 | cell_alignment = cell_size / 2.0F - cell_position.x; 36 | break; 37 | } 38 | 39 | return {cell_alignment, cell_advance}; 40 | } 41 | 42 | RelativePose::RelativePose(const Pose& absolute_pose) : absolute_pose{&absolute_pose} { } 43 | 44 | Pose RelativePose::get() const { 45 | return { 46 | this->absolute_pose->position - this->reference_pose.position, 47 | this->absolute_pose->orientation - this->reference_pose.orientation 48 | }; 49 | } 50 | 51 | void RelativePose::reset_reference() { 52 | this->reference_pose = *(this->absolute_pose); 53 | } 54 | } // namespace micras::nav 55 | -------------------------------------------------------------------------------- /tests/src/proxy/test_torque_sensors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | static constexpr float linear_speed{80.0F}; 10 | 11 | // NOLINTBEGIN(*-avoid-c-arrays, cppcoreguidelines-avoid-non-const-global-variables) 12 | static volatile float test_torque[2]; 13 | static volatile float test_torque_raw[2]; 14 | static volatile float test_current[2]; 15 | static volatile float test_current_raw[2]; 16 | 17 | // NOLINTEND(*-avoid-c-arrays, cppcoreguidelines-avoid-non-const-global-variables) 18 | 19 | int main(int argc, char* argv[]) { 20 | TestCore::init(argc, argv); 21 | bool running = false; 22 | proxy::Locomotion locomotion{locomotion_config}; 23 | proxy::TorqueSensors torque_sensors{torque_sensors_config}; 24 | proxy::Button button{button_config}; 25 | 26 | locomotion.enable(); 27 | 28 | TestCore::loop([&torque_sensors, &locomotion, &button, &running]() { 29 | button.update(); 30 | 31 | if (button.get_status() != proxy::Button::Status::NO_PRESS) { 32 | running = not running; 33 | 34 | if (running) { 35 | locomotion.set_command(linear_speed, 0.0F); 36 | } else { 37 | locomotion.set_command(-linear_speed, 0.0F); 38 | } 39 | } 40 | 41 | torque_sensors.update(); 42 | 43 | for (uint8_t i = 0; i < 2; i++) { 44 | test_torque[i] = torque_sensors.get_torque(i); 45 | test_torque_raw[i] = torque_sensors.get_torque_raw(i); 46 | test_current[i] = torque_sensors.get_current(i); 47 | test_current_raw[i] = torque_sensors.get_current_raw(i); 48 | } 49 | 50 | proxy::Stopwatch::sleep_ms(2); 51 | }); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /tests/src/nav/test_odometry.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "constants.hpp" 6 | #include "micras/nav/odometry.hpp" 7 | #include "test_core.hpp" 8 | 9 | using namespace micras; // NOLINT(google-build-using-namespace) 10 | 11 | // NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) 12 | static volatile float test_position_x{}; 13 | static volatile float test_position_y{}; 14 | static volatile float test_orientation{}; 15 | static volatile float test_linear_velocity{}; 16 | static volatile float test_angular_velocity{}; 17 | 18 | // NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) 19 | 20 | int main(int argc, char* argv[]) { 21 | TestCore::init(argc, argv); 22 | auto imu{std::make_shared(imu_config)}; 23 | 24 | proxy::Stopwatch stopwatch{stopwatch_config}; 25 | nav::Odometry odometry{ 26 | std::make_shared(rotary_sensor_left_config), 27 | std::make_shared(rotary_sensor_right_config), imu, odometry_config 28 | }; 29 | 30 | stopwatch.reset_us(); 31 | 32 | proxy::Stopwatch::sleep_ms(1000); 33 | imu->calibrate(); 34 | 35 | TestCore::loop([&odometry, &imu, &stopwatch]() { 36 | const float elapsed_time = stopwatch.elapsed_time_us() / 1000000.0F; 37 | stopwatch.reset_us(); 38 | 39 | imu->update(); 40 | odometry.update(elapsed_time); 41 | 42 | const auto& state = odometry.get_state(); 43 | 44 | test_position_x = state.pose.position.x; 45 | test_position_y = state.pose.position.y; 46 | test_orientation = state.pose.orientation; 47 | test_linear_velocity = state.velocity.linear; 48 | test_angular_velocity = state.velocity.angular; 49 | 50 | while (stopwatch.elapsed_time_us() < 1000) { } 51 | }); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /micras_core/src/butterworth_filter.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | #include "micras/core/butterworth_filter.hpp" 9 | 10 | namespace micras::core { 11 | ButterworthFilter::ButterworthFilter(float cutoff_frequency, float sampling_frequency) { 12 | const float relative_frequency = cutoff_frequency / sampling_frequency; 13 | const float relative_frequency_2 = relative_frequency * relative_frequency; 14 | 15 | const float b0 = 1; 16 | const float b1 = 2; 17 | const float b2 = 1; 18 | 19 | // Butterworth filter coefficients 20 | const float a0 = 1 + 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; 21 | const float a1 = 2 - 8 / relative_frequency_2; 22 | const float a2 = 1 - 2 * std::numbers::sqrt2_v / relative_frequency + 4 / relative_frequency_2; 23 | 24 | this->a_array[0] = a2 / a0; 25 | this->a_array[1] = a1 / a0; 26 | 27 | this->b_array[0] = b2 / a0; 28 | this->b_array[1] = b1 / a0; 29 | this->b_array[2] = b0 / a0; 30 | } 31 | 32 | float ButterworthFilter::update(float x0) { 33 | this->x_array[0] = this->x_array[1]; 34 | this->x_array[1] = this->x_array[2]; 35 | this->x_array[2] = x0; 36 | 37 | float x_b_dot = 0; 38 | 39 | for (uint8_t i = 0; i < filter_order + 1; i++) { 40 | x_b_dot += this->x_array.at(i) * this->b_array.at(i); 41 | } 42 | 43 | float y_a_dot = 0; 44 | 45 | for (uint8_t i = 0; i < filter_order; i++) { 46 | y_a_dot += this->y_array.at(i) * this->a_array.at(i); 47 | } 48 | 49 | const float y0 = x_b_dot - y_a_dot; 50 | 51 | this->y_array[0] = this->y_array[1]; 52 | this->y_array[1] = y0; 53 | 54 | return y0; 55 | } 56 | 57 | float ButterworthFilter::get_last() const { 58 | return this->y_array[1]; 59 | } 60 | } // namespace micras::core 61 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/pwm.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_PWM_HPP 6 | #define MICRAS_HAL_PWM_HPP 7 | 8 | #include 9 | #include 10 | 11 | namespace micras::hal { 12 | /** 13 | * @brief Class to handle PWM peripheral on STM32 microcontrollers. 14 | */ 15 | class Pwm { 16 | public: 17 | /** 18 | * @brief PWM configuration struct. 19 | */ 20 | struct Config { 21 | void (*init_function)(); 22 | TIM_HandleTypeDef* handle; 23 | uint32_t timer_channel; 24 | }; 25 | 26 | /** 27 | * @brief Construct a new Pwm object. 28 | * 29 | * @param config Configuration for the PWM. 30 | */ 31 | explicit Pwm(const Config& config); 32 | 33 | /** 34 | * @brief Set the PWM duty cycle. 35 | * 36 | * @param duty_cycle Duty cycle value. 37 | */ 38 | void set_duty_cycle(float duty_cycle); 39 | 40 | /** 41 | * @brief Set the PWM frequency. 42 | * 43 | * @note Changing the pwm frequency will modify the autoreload and reset the counter, 44 | * but the compare value will stay the same, so the duty cycle will be different from the previously set value. 45 | * There is a minimum and maximum frequency that can be set by changing the autoreload register. 46 | * The minimum frequency is base_freq / (2^timer_resolution * (prescaler + 1)). 47 | * The maximum frequency is base_freq / (prescaler + 1). 48 | * 49 | * @param frequency Frequency value in Hz. 50 | */ 51 | void set_frequency(uint32_t frequency); 52 | 53 | private: 54 | /** 55 | * @brief Timer handle. 56 | */ 57 | TIM_HandleTypeDef* handle; 58 | 59 | /** 60 | * @brief Channel number of the timer. 61 | */ 62 | uint32_t channel; 63 | }; 64 | } // namespace micras::hal 65 | 66 | #endif // MICRAS_HAL_PWM_HPP 67 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/buzzer.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_BUZZER_HPP 6 | #define MICRAS_PROXY_BUZZER_HPP 7 | 8 | #include 9 | 10 | #include "micras/hal/pwm.hpp" 11 | #include "micras/proxy/stopwatch.hpp" 12 | 13 | namespace micras::proxy { 14 | /** 15 | * @brief Class for controlling a buzzer. 16 | */ 17 | class Buzzer { 18 | public: 19 | /** 20 | * @brief Configuration struct for the buzzer. 21 | */ 22 | struct Config { 23 | hal::Pwm::Config pwm; 24 | }; 25 | 26 | /** 27 | * @brief Construct a new Buzzer object. 28 | * 29 | * @param config Configuration for the buzzer. 30 | */ 31 | explicit Buzzer(const Config& config); 32 | 33 | /** 34 | * @brief Play a tone for a duration. 35 | * 36 | * @param frequency Buzzer sound frequency in Hz. 37 | * @param duration Duration of the sound in ms. 38 | */ 39 | void play(uint32_t frequency, uint32_t duration = 0); 40 | 41 | /** 42 | * @brief Update the buzzer state. 43 | */ 44 | void update(); 45 | 46 | /** 47 | * @brief Stop the buzzer sound. 48 | */ 49 | void stop(); 50 | 51 | /** 52 | * @brief Wait for a time interval updating the buzzer. 53 | * 54 | * @param interval Time to wait in ms. 55 | */ 56 | void wait(uint32_t interval); 57 | 58 | private: 59 | /** 60 | * @brief PWM object. 61 | */ 62 | hal::Pwm pwm; 63 | 64 | /** 65 | * @brief Stopwatch to play the sound. 66 | */ 67 | proxy::Stopwatch stopwatch; 68 | 69 | /** 70 | * @brief Flag to check if the buzzer is playing. 71 | */ 72 | bool is_playing{}; 73 | 74 | /** 75 | * @brief Duration of the sound. 76 | */ 77 | uint32_t duration{}; 78 | }; 79 | } // namespace micras::proxy 80 | 81 | #endif // MICRAS_PROXY_BUZZER_HPP 82 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/spi.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_SPI_HPP 6 | #define MICRAS_HAL_SPI_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "micras/hal/gpio.hpp" 13 | 14 | namespace micras::hal { 15 | /** 16 | * @brief Class to handle SPI peripheral on STM32 microcontrollers. 17 | */ 18 | class Spi { 19 | public: 20 | /** 21 | * @brief SPI configuration struct. 22 | */ 23 | struct Config { 24 | void (*init_function)(); 25 | SPI_HandleTypeDef* handle; 26 | hal::Gpio::Config cs_gpio; 27 | uint32_t timeout; 28 | }; 29 | 30 | /** 31 | * @brief Construct a new Spi object. 32 | * 33 | * @param config Configuration for the SPI. 34 | */ 35 | explicit Spi(const Config& config); 36 | 37 | /** 38 | * @brief Activate the chip select. 39 | * 40 | * @return True if the device was successfully selected, false otherwise. 41 | */ 42 | bool select_device(); 43 | 44 | /** 45 | * @brief Deactivate the chip select. 46 | */ 47 | void unselect_device(); 48 | 49 | /** 50 | * @brief Transmit data over SPI. 51 | * 52 | * @param data Data to transmit. 53 | */ 54 | void transmit(std::span data); 55 | 56 | /** 57 | * @brief Receive data over SPI. 58 | * 59 | * @param data Data to receive data. 60 | */ 61 | void receive(std::span data); 62 | 63 | private: 64 | /** 65 | * @brief Handle for the SPI. 66 | */ 67 | SPI_HandleTypeDef* handle; 68 | 69 | /** 70 | * @brief GPIO for the chip select pin. 71 | */ 72 | hal::Gpio cs_gpio; 73 | 74 | /** 75 | * @brief Timeout for the SPI operations in ms. 76 | */ 77 | uint32_t timeout; 78 | }; 79 | } // namespace micras::hal 80 | 81 | #endif // MICRAS_HAL_SPI_HPP 82 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/pwm_dma.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_PWM_DMA_HPP 6 | #define MICRAS_HAL_PWM_DMA_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace micras::hal { 13 | /** 14 | * @brief Class to handle PWM peripheral on STM32 microcontrollers using DMA. 15 | */ 16 | class PwmDma { 17 | public: 18 | /** 19 | * @brief PWM configuration struct. 20 | */ 21 | struct Config { 22 | void (*init_function)(); 23 | TIM_HandleTypeDef* handle; 24 | uint32_t timer_channel; 25 | }; 26 | 27 | /** 28 | * @brief Construct a new PwmDma object. 29 | * 30 | * @param config Configuration for the PWM. 31 | */ 32 | explicit PwmDma(const Config& config); 33 | 34 | /** 35 | * @brief Start PWM and DMA transfer. 36 | * 37 | * @param buffer Buffer to transfer. 38 | */ 39 | void start_dma(std::span buffer); 40 | 41 | /** 42 | * @brief Start PWM and DMA transfer. 43 | * 44 | * @param buffer Buffer to transfer. 45 | */ 46 | void start_dma(std::span buffer); 47 | 48 | /** 49 | * @brief Stop PWM and DMA transfer. 50 | */ 51 | void stop_dma(); 52 | 53 | /** 54 | * @brief Get the compare value for ad duty cycle. 55 | * 56 | * @param duty_cycle Duty cycle to get the compare value for. 57 | * @return Compare value for the duty cycle. 58 | */ 59 | uint32_t get_compare(float duty_cycle) const; 60 | 61 | /** 62 | * @brief Check if the PWM is busy. 63 | * 64 | * @return True If the PWM is busy, false otherwise. 65 | */ 66 | bool is_busy(); 67 | 68 | private: 69 | /** 70 | * @brief Timer handle. 71 | */ 72 | TIM_HandleTypeDef* handle; 73 | 74 | /** 75 | * @brief Channel number of the timer. 76 | */ 77 | uint32_t channel; 78 | }; 79 | } // namespace micras::hal 80 | 81 | #endif // MICRAS_HAL_PWM_DMA_HPP 82 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/stopwatch.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_STOPWATCH_HPP 6 | #define MICRAS_PROXY_STOPWATCH_HPP 7 | 8 | #include "micras/hal/timer.hpp" 9 | 10 | namespace micras::proxy { 11 | /** 12 | * @brief Class to measure the time elapsed between two events. 13 | */ 14 | class Stopwatch { 15 | public: 16 | /** 17 | * @brief Stopwatch configuration struct. 18 | */ 19 | struct Config { 20 | hal::Timer::Config timer; 21 | }; 22 | 23 | /** 24 | * @brief Construct a new Stopwatch object. 25 | */ 26 | Stopwatch(); 27 | 28 | /** 29 | * @brief Construct a new Stopwatch object. 30 | * 31 | * @param config Configuration for the timer. 32 | */ 33 | explicit Stopwatch(const Config& config); 34 | 35 | /** 36 | * @brief Reset the milliseconds timer counter. 37 | */ 38 | void reset_ms(); 39 | 40 | /** 41 | * @brief Reset the microseconds timer counter. 42 | */ 43 | void reset_us(); 44 | 45 | /** 46 | * @brief Get the time elapsed since the last reset. 47 | * 48 | * @return Time elapsed in miliseconds. 49 | */ 50 | uint32_t elapsed_time_ms() const; 51 | 52 | /** 53 | * @brief Get the time elapsed since the last reset. 54 | * 55 | * @return Time elapsed in microseconds. 56 | */ 57 | uint32_t elapsed_time_us() const; 58 | 59 | /** 60 | * @brief Sleep for a given amount of time. 61 | * 62 | * @param time Time to sleep in milliseconds. 63 | */ 64 | static void sleep_ms(uint32_t time); 65 | 66 | /** 67 | * @brief Sleep for a given amount of time. 68 | * 69 | * @param time Time to sleep in microseconds. 70 | */ 71 | void sleep_us(uint32_t time) const; 72 | 73 | private: 74 | hal::Timer timer; 75 | 76 | /** 77 | * @brief Stopwatch counter. 78 | */ 79 | uint32_t counter{}; 80 | }; 81 | } // namespace micras::proxy 82 | 83 | #endif // MICRAS_PROXY_STOPWATCH_HPP 84 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/battery.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_BATTERY_HPP 6 | #define MICRAS_PROXY_BATTERY_HPP 7 | 8 | #include 9 | 10 | #include "micras/core/butterworth_filter.hpp" 11 | #include "micras/hal/adc_dma.hpp" 12 | 13 | namespace micras::proxy { 14 | /** 15 | * @brief Class for getting the battery voltage. 16 | */ 17 | class Battery { 18 | public: 19 | /** 20 | * @brief Configuration struct for the battery. 21 | */ 22 | struct Config { 23 | hal::AdcDma::Config adc; 24 | float voltage_divider; 25 | float filter_cutoff; 26 | }; 27 | 28 | /** 29 | * @brief Construct a new Battery object. 30 | * 31 | * @param config Configuration for the battery. 32 | */ 33 | explicit Battery(const Config& config); 34 | 35 | /** 36 | * @brief Update the battery reading. 37 | */ 38 | void update(); 39 | 40 | /** 41 | * @brief Get the battery voltage. 42 | * 43 | * @return Battery voltage in volts. 44 | */ 45 | float get_voltage() const; 46 | 47 | /** 48 | * @brief Get the battery voltage in volts without the filter applied. 49 | * 50 | * @return Battery voltage in volts. 51 | */ 52 | float get_voltage_raw() const; 53 | 54 | /** 55 | * @brief Get the battery reading from the ADC. 56 | * 57 | * @return Battery reading from 0 to 1. 58 | */ 59 | float get_adc_reading() const; 60 | 61 | private: 62 | /** 63 | * @brief Adc object. 64 | */ 65 | hal::AdcDma adc; 66 | 67 | /** 68 | * @brief Raw reading from the battery. 69 | */ 70 | uint16_t raw_reading{}; 71 | 72 | /** 73 | * @brief Maximum voltage that can be read. 74 | */ 75 | float max_voltage; 76 | 77 | /** 78 | * @brief Butterworth filter for the battery reading. 79 | */ 80 | core::ButterworthFilter filter; 81 | }; 82 | } // namespace micras::proxy 83 | 84 | #endif // MICRAS_PROXY_BATTERY_HPP 85 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/adc_dma.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_ADC_DMA_HPP 6 | #define MICRAS_HAL_ADC_DMA_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace micras::hal { 13 | /** 14 | * @brief Class to handle ADC peripheral on STM32 microcontrollers using DMA. 15 | */ 16 | class AdcDma { 17 | public: 18 | /** 19 | * @brief Configuration struct for ADC DMA. 20 | */ 21 | struct Config { 22 | void (*init_function)(); 23 | ADC_HandleTypeDef* handle; 24 | uint16_t max_reading; 25 | }; 26 | 27 | /** 28 | * @brief Construct a new AdcDma object. 29 | * 30 | * @param config ADC DMA configuration struct. 31 | */ 32 | explicit AdcDma(const Config& config); 33 | 34 | /** 35 | * @brief Enable ADC, start conversion of regular group and transfer result through DMA. 36 | * 37 | * @param buffer 32 bit destination buffer address. 38 | */ 39 | void start_dma(std::span buffer); 40 | 41 | /** 42 | * @brief Enable ADC, start conversion of regular group and transfer result through DMA. 43 | * 44 | * @param buffer 16 bit destination buffer address. 45 | */ 46 | void start_dma(std::span buffer); 47 | 48 | /** 49 | * @brief Stop ADC conversion of regular group (and injected group in case of auto_injection mode). 50 | */ 51 | void stop_dma(); 52 | 53 | /** 54 | * @brief Get the maximum reading of the ADC. 55 | * 56 | * @return Maximum reading of the ADC. 57 | */ 58 | uint16_t get_max_reading() const; 59 | 60 | /** 61 | * @brief Reference voltage for the ADC measurement. 62 | */ 63 | static constexpr float reference_voltage{3.3F}; 64 | 65 | private: 66 | /** 67 | * @brief Maximum ADC reading. 68 | */ 69 | uint16_t max_reading; 70 | 71 | /** 72 | * @brief ADC handle. 73 | */ 74 | ADC_HandleTypeDef* handle; 75 | }; 76 | } // namespace micras::hal 77 | 78 | #endif // MICRAS_HAL_ADC_DMA_HPP 79 | -------------------------------------------------------------------------------- /micras_nav/include/micras/nav/actions/base.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_NAV_BASE_ACTION_HPP 6 | #define MICRAS_NAV_BASE_ACTION_HPP 7 | 8 | #include "micras/nav/state.hpp" 9 | 10 | namespace micras::nav { 11 | /** 12 | * @brief Base class for actions. 13 | */ 14 | class Action { 15 | public: 16 | /** 17 | * @brief Constructor for the Action class. 18 | * 19 | * @param action_id The ID of the action. 20 | */ 21 | explicit Action(uint8_t action_id) : id(action_id) { } 22 | 23 | /** 24 | * @brief Virtual destructor for the Action class. 25 | */ 26 | virtual ~Action() = default; 27 | 28 | /** 29 | * @brief Get the desired speeds for the robot. 30 | * 31 | * @param pose The current pose of the robot. 32 | * @return The desired speeds for the robot. 33 | */ 34 | virtual Twist get_speeds(const Pose& pose) const = 0; 35 | 36 | /** 37 | * @brief Check if the action is finished. 38 | * 39 | * @param pose The current pose of the robot. 40 | * @return True if the action is finished, false otherwise. 41 | */ 42 | virtual bool finished(const Pose& pose) const = 0; 43 | 44 | /** 45 | * @brief Check if the action allow the robot to follow walls. 46 | * 47 | * @return True if the action allows the robot to follow walls, false otherwise. 48 | */ 49 | virtual bool allow_follow_wall() const = 0; 50 | 51 | /** 52 | * @brief Get the ID of the action. 53 | * 54 | * @return The ID of the action. 55 | */ 56 | uint8_t get_id() const { return id; } 57 | 58 | protected: 59 | /** 60 | * @brief Special member functions declared as default. 61 | */ 62 | ///@{ 63 | Action(const Action&) = default; 64 | Action(Action&&) = default; 65 | Action& operator=(const Action&) = default; 66 | Action& operator=(Action&&) = default; 67 | ///@} 68 | 69 | private: 70 | /** 71 | * @brief The ID of the action. 72 | */ 73 | uint8_t id; 74 | }; 75 | } // namespace micras::nav 76 | 77 | #endif // MICRAS_NAV_BASE_ACTION_HPP 78 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/locomotion.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_LOCOMOTION_HPP 6 | #define MICRAS_PROXY_LOCOMOTION_HPP 7 | 8 | #include 9 | 10 | #include "micras/hal/gpio.hpp" 11 | #include "micras/proxy/motor.hpp" 12 | 13 | namespace micras::proxy { 14 | /** 15 | * @brief Class for controlling the locomotion driver. 16 | */ 17 | class Locomotion { 18 | public: 19 | /** 20 | * @brief Configuration struct for the locomotion. 21 | */ 22 | struct Config { 23 | Motor::Config left_motor; 24 | Motor::Config right_motor; 25 | hal::Gpio::Config enable_gpio; 26 | }; 27 | 28 | /** 29 | * @brief Construct a new locomotion object. 30 | * 31 | * @param config Configuration for the locomotion driver. 32 | */ 33 | explicit Locomotion(const Config& config); 34 | 35 | /** 36 | * @brief Enable the locomotion driver. 37 | */ 38 | void enable(); 39 | 40 | /** 41 | * @brief Disable the locomotion driver. 42 | */ 43 | void disable(); 44 | 45 | /** 46 | * @brief Set the command of the wheels. 47 | * 48 | * @param left_command Command of the left wheels. 49 | * @param right_command Command of the right wheels. 50 | */ 51 | void set_wheel_command(float left_command, float right_command); 52 | 53 | /** 54 | * @brief Set the linear and angular commands of the robot. 55 | * 56 | * @param linear Linear command of the robot. 57 | * @param angular Angular command of the robot. 58 | */ 59 | void set_command(float linear, float angular); 60 | 61 | /** 62 | * @brief Stop the motors. 63 | */ 64 | void stop(); 65 | 66 | private: 67 | /** 68 | * @brief Left motor of the robot. 69 | */ 70 | Motor left_motor; 71 | 72 | /** 73 | * @brief Right motor of the robot. 74 | */ 75 | Motor right_motor; 76 | 77 | /** 78 | * @brief GPIO handle for the motor driver enable pin. 79 | */ 80 | hal::Gpio enable_gpio; 81 | }; 82 | } // namespace micras::proxy 83 | 84 | #endif // MICRAS_PROXY_LOCOMOTION_HPP 85 | -------------------------------------------------------------------------------- /micras_proxy/src/argb.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_ARGB_CPP 6 | #define MICRAS_PROXY_ARGB_CPP 7 | 8 | #include "micras/proxy/argb.hpp" 9 | 10 | namespace micras::proxy { 11 | template 12 | TArgb::TArgb(const Config& config) : 13 | pwm{config.pwm}, 14 | low_bit{pwm.get_compare(config.low_duty_cycle)}, 15 | high_bit{pwm.get_compare(config.high_duty_cycle)}, 16 | brightness{config.max_brightness / 100.0F} { 17 | this->turn_off(); 18 | } 19 | 20 | template 21 | void TArgb::set_color(const Color& color, uint8_t index) { 22 | this->colors.at(index) = color; 23 | this->update(); 24 | } 25 | 26 | template 27 | void TArgb::set_color(const Color& color) { 28 | for (uint8_t i = 0; i < num_of_leds; i++) { 29 | this->colors.at(i) = color; 30 | } 31 | 32 | this->update(); 33 | } 34 | 35 | template 36 | void TArgb::set_colors(const std::array& colors) { 37 | this->colors = colors; 38 | this->update(); 39 | } 40 | 41 | template 42 | void TArgb::turn_off(uint8_t index) { 43 | this->set_color({0, 0, 0}, index); 44 | } 45 | 46 | template 47 | void TArgb::turn_off() { 48 | this->set_color({0, 0, 0}); 49 | } 50 | 51 | template 52 | void TArgb::update() { 53 | if (this->pwm.is_busy()) { 54 | return; 55 | } 56 | 57 | for (uint8_t i = 0; i < num_of_leds; i++) { 58 | this->encode_color(this->colors.at(i) * this->brightness, i); 59 | } 60 | 61 | this->pwm.start_dma(this->buffer); 62 | } 63 | 64 | template 65 | void TArgb::encode_color(const Color& color, uint8_t index) { 66 | uint32_t data = (color.green << (2 * bits_per_color)) | (color.red << bits_per_color) | color.blue; 67 | 68 | for (uint32_t i = bits_per_led * index, j = bits_per_led - 1; i < bits_per_led * (index + 1U); i++, j--) { 69 | this->buffer.at(i) = ((data >> j) & 1) == 1 ? this->high_bit : this->low_bit; 70 | } 71 | } 72 | } // namespace micras::proxy 73 | 74 | #endif // MICRAS_PROXY_ARGB_CPP 75 | -------------------------------------------------------------------------------- /micras_proxy/src/button.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/proxy/button.hpp" 6 | 7 | namespace micras::proxy { 8 | Button::Button(const Config& config) : 9 | debounce_delay{config.debounce_delay}, 10 | long_press_delay{config.long_press_delay}, 11 | extra_long_press_delay{config.extra_long_press_delay}, 12 | gpio{config.gpio}, 13 | pull_resistor{config.pull_resistor} { } 14 | 15 | bool Button::is_pressed() const { 16 | return this->current_state; 17 | } 18 | 19 | Button::Status Button::get_status() const { 20 | return this->current_status; 21 | } 22 | 23 | void Button::update() { 24 | this->previous_state = this->current_state; 25 | this->update_state(); 26 | 27 | if (this->is_rising_edge()) { 28 | this->status_stopwatch.reset_ms(); 29 | } else if (this->is_falling_edge()) { 30 | if (this->status_stopwatch.elapsed_time_ms() > extra_long_press_delay) { 31 | this->current_status = EXTRA_LONG_PRESS; 32 | return; 33 | } 34 | 35 | if (this->status_stopwatch.elapsed_time_ms() > long_press_delay) { 36 | this->current_status = LONG_PRESS; 37 | return; 38 | } 39 | 40 | this->current_status = SHORT_PRESS; 41 | return; 42 | } 43 | 44 | this->current_status = NO_PRESS; 45 | } 46 | 47 | bool Button::get_raw_reading() const { 48 | return this->gpio.read() == static_cast(this->pull_resistor); 49 | } 50 | 51 | void Button::update_state() { 52 | const bool raw_reading = this->get_raw_reading(); 53 | 54 | if ((raw_reading != this->current_state) and not this->is_debouncing) { 55 | this->is_debouncing = true; 56 | this->debounce_stopwatch.reset_ms(); 57 | } else if ((this->debounce_stopwatch.elapsed_time_ms() < debounce_delay) and this->is_debouncing) { 58 | if (this->current_state == raw_reading) { 59 | this->is_debouncing = false; 60 | } 61 | } else { 62 | this->is_debouncing = false; 63 | this->current_state = raw_reading; 64 | } 65 | } 66 | 67 | bool Button::is_rising_edge() const { 68 | return this->current_state and not this->previous_state; 69 | } 70 | 71 | bool Button::is_falling_edge() const { 72 | return not this->current_state and this->previous_state; 73 | } 74 | } // namespace micras::proxy 75 | -------------------------------------------------------------------------------- /micras_proxy/src/torque_sensors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_TORQUE_SENSORS_CPP 6 | #define MICRAS_PROXY_TORQUE_SENSORS_CPP 7 | 8 | #include "micras/core/utils.hpp" 9 | #include "micras/proxy/torque_sensors.hpp" 10 | 11 | namespace micras::proxy { 12 | template 13 | TTorqueSensors::TTorqueSensors(const Config& config) : 14 | adc{config.adc}, 15 | max_current{hal::AdcDma::reference_voltage / config.shunt_resistor}, 16 | max_torque{config.max_torque}, 17 | filters{core::make_array(config.filter_cutoff)} { 18 | this->adc.start_dma(this->buffer); 19 | } 20 | 21 | template 22 | void TTorqueSensors::calibrate() { 23 | for (uint8_t i = 0; i < num_of_sensors; i++) { 24 | this->base_reading[i] = this->filters[i].get_last(); 25 | } 26 | } 27 | 28 | template 29 | void TTorqueSensors::update() { 30 | for (uint8_t i = 0; i < num_of_sensors; i++) { 31 | this->filters[i].update(this->get_adc_reading(i)); 32 | } 33 | } 34 | 35 | template 36 | float TTorqueSensors::get_torque(uint8_t sensor_index) const { 37 | return this->filters.at(sensor_index).get_last() * this->max_torque; 38 | } 39 | 40 | template 41 | float TTorqueSensors::get_torque_raw(uint8_t sensor_index) const { 42 | return this->get_adc_reading(sensor_index) * this->max_torque; 43 | } 44 | 45 | template 46 | float TTorqueSensors::get_current(uint8_t sensor_index) const { 47 | return this->filters.at(sensor_index).get_last() * this->max_current; 48 | } 49 | 50 | template 51 | float TTorqueSensors::get_current_raw(uint8_t sensor_index) const { 52 | return this->get_adc_reading(sensor_index) * this->max_current; 53 | ; 54 | } 55 | 56 | template 57 | float TTorqueSensors::get_adc_reading(uint8_t sensor_index) const { 58 | return static_cast(this->buffer.at(sensor_index)) / this->adc.get_max_reading() - 59 | this->base_reading.at(sensor_index); 60 | } 61 | } // namespace micras::proxy 62 | 63 | #endif // MICRAS_PROXY_TORQUE_SENSORS_CPP 64 | -------------------------------------------------------------------------------- /micras_hal/src/flash.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/hal/flash.hpp" 8 | 9 | namespace micras::hal { 10 | /** 11 | * @brief Last address of the flash memory. 12 | * 13 | * @note Needs to be here because it is not defined at compile time. 14 | */ 15 | static const uint32_t base_address = FLASH_BASE + FLASH_SIZE - 8; 16 | 17 | // NOLINTNEXTLINE(*-avoid-c-arrays) 18 | void Flash::read(uint32_t address, uint64_t data[], uint32_t size) { 19 | const uint32_t end = base_address - 8 * address; 20 | 21 | for (address = end - 8 * (size - 1); address <= end; address += 8, data++) { 22 | (*data) = *(std::bit_cast(address)); 23 | } 24 | } 25 | 26 | // NOLINTNEXTLINE(*-avoid-c-arrays) 27 | void Flash::read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size) { 28 | read(page * double_words_per_page + page_address, data, size); 29 | } 30 | 31 | // NOLINTNEXTLINE(*-avoid-c-arrays) 32 | void Flash::write(uint32_t address, const uint64_t data[], uint32_t size) { 33 | HAL_FLASH_Unlock(); 34 | 35 | const uint32_t end = base_address - 8 * address; 36 | address = end - 8 * (size - 1); 37 | 38 | while (address <= end) { 39 | if (size >= double_words_per_row) { 40 | HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST, address, std::bit_cast(data)); 41 | address += bytes_per_row; 42 | data += double_words_per_row; 43 | size -= double_words_per_row; 44 | } else { 45 | HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, address, *data); 46 | address += 8; 47 | data++; 48 | size--; 49 | } 50 | } 51 | 52 | HAL_FLASH_Lock(); 53 | } 54 | 55 | // NOLINTNEXTLINE(*-avoid-c-arrays) 56 | void Flash::write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size) { 57 | write(page * double_words_per_page + page_address, data, size); 58 | } 59 | 60 | void Flash::erase_pages(uint16_t page, uint16_t number_of_pages) { 61 | FLASH_EraseInitTypeDef erase_struct = { 62 | .TypeErase = FLASH_TYPEERASE_PAGES, 63 | .Banks = FLASH_BANK_2, 64 | .Page = FLASH_PAGE_NB - page - number_of_pages, 65 | .NbPages = number_of_pages, 66 | }; 67 | 68 | uint32_t page_error{}; 69 | 70 | HAL_FLASH_Unlock(); 71 | HAL_FLASHEx_Erase(&erase_struct, &page_error); 72 | HAL_FLASH_Lock(); 73 | } 74 | } // namespace micras::hal 75 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: Build lint and check formatting 2 | 3 | on: 4 | pull_request: 5 | types: [opened, synchronize, reopened, ready_for_review] 6 | branches: 7 | - main 8 | push: 9 | branches: 10 | - main 11 | 12 | jobs: 13 | build: 14 | runs-on: ubuntu-latest 15 | if: github.event_name != 'pull_request' || github.event.pull_request.draft == false 16 | steps: 17 | - name: 🔀 Checkout 18 | uses: actions/checkout@v4 19 | with: 20 | submodules: recursive 21 | 22 | - name: 🗃️ Check cache 23 | id: cache 24 | uses: actions/cache/restore@v4 25 | with: 26 | path: .docker/Dockerfile 27 | key: ${{ hashFiles('.github/*', 'cmake/*', '**/*.cpp', '**/*.hpp', '**/*.ioc', '.clang*', 'CMakeLists.txt', '.docker/*') }} 28 | lookup-only: true 29 | 30 | - name: 🔧 Set up Docker Buildx 31 | if: steps.cache.outputs.cache-hit != 'true' 32 | uses: docker/setup-buildx-action@v3 33 | 34 | - name: 🐋 Build Docker image 35 | if: steps.cache.outputs.cache-hit != 'true' 36 | timeout-minutes: 10 37 | uses: docker/build-push-action@v6 38 | with: 39 | context: . 40 | file: .docker/Dockerfile 41 | target: build 42 | load: true 43 | cache-from: type=gha 44 | cache-to: type=gha,mode=max 45 | no-cache-filters: build 46 | tags: project:build 47 | 48 | - name: 🎨 Check code formatting 49 | if: steps.cache.outputs.cache-hit != 'true' 50 | run: docker run --rm project:build /bin/bash /project/.docker/scripts/check_format.sh 51 | 52 | - name: 🔨 Build project 53 | if: steps.cache.outputs.cache-hit != 'true' 54 | run: docker run --rm project:build make -C build --no-print-directory -j 55 | 56 | - name: 🧪 Build tests 57 | if: steps.cache.outputs.cache-hit != 'true' 58 | run: docker run --rm project:build make -C build --no-print-directory -j test_all 59 | 60 | - name: 🚨 Lint project 61 | if: steps.cache.outputs.cache-hit != 'true' 62 | run: docker run --rm project:build make -C build --no-print-directory -j lint 63 | 64 | - name: 🗃️ Save cache 65 | if: steps.cache.outputs.cache-hit != 'true' 66 | uses: actions/cache/save@v4 67 | with: 68 | path: .docker/Dockerfile 69 | key: ${{ steps.cache.outputs.cache-primary-key }} 70 | -------------------------------------------------------------------------------- /tests/src/proxy/test_locomotion.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | static constexpr float test_speed{50.0F}; 10 | static constexpr uint32_t time_interval{1000}; 11 | 12 | int main(int argc, char* argv[]) { 13 | TestCore::init(argc, argv); 14 | proxy::Button button{button_config}; 15 | proxy::Locomotion locomotion{locomotion_config}; 16 | 17 | TestCore::loop([&button, &locomotion]() { 18 | button.update(); 19 | 20 | switch (button.get_status()) { 21 | case proxy::Button::Status::SHORT_PRESS: 22 | locomotion.enable(); 23 | locomotion.set_command(test_speed, 0.0F); 24 | proxy::Stopwatch::sleep_ms(time_interval); 25 | locomotion.stop(); 26 | proxy::Stopwatch::sleep_ms(time_interval); 27 | locomotion.set_command(-test_speed, 0.0F); 28 | proxy::Stopwatch::sleep_ms(time_interval); 29 | locomotion.stop(); 30 | proxy::Stopwatch::sleep_ms(time_interval); 31 | 32 | locomotion.set_command(0.0F, test_speed); 33 | proxy::Stopwatch::sleep_ms(time_interval); 34 | locomotion.stop(); 35 | proxy::Stopwatch::sleep_ms(time_interval); 36 | locomotion.set_command(0.0F, -test_speed); 37 | proxy::Stopwatch::sleep_ms(time_interval); 38 | locomotion.stop(); 39 | locomotion.disable(); 40 | break; 41 | 42 | case proxy::Button::Status::LONG_PRESS: 43 | locomotion.enable(); 44 | 45 | for (int8_t i = 1; i < 100; i++) { 46 | locomotion.set_wheel_command(i, i); 47 | proxy::Stopwatch::sleep_ms(20); 48 | } 49 | 50 | for (int8_t i = 100; i > -100; i--) { 51 | locomotion.set_wheel_command(i, i); 52 | proxy::Stopwatch::sleep_ms(20); 53 | } 54 | 55 | for (int8_t i = -100; i <= 0; i++) { 56 | locomotion.set_wheel_command(i, i); 57 | proxy::Stopwatch::sleep_ms(20); 58 | } 59 | 60 | locomotion.disable(); 61 | break; 62 | 63 | default: 64 | break; 65 | } 66 | }); 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /tests/src/proxy/test_rotary_sensors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "test_core.hpp" 6 | 7 | using namespace micras; // NOLINT(google-build-using-namespace) 8 | 9 | static constexpr float linear_speed{50.0F}; 10 | 11 | // NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) 12 | static volatile float test_left_position{}; 13 | static volatile float test_right_position{}; 14 | 15 | // NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) 16 | 17 | int main(int argc, char* argv[]) { 18 | TestCore::init(argc, argv); 19 | proxy::RotarySensor rotary_sensor_left{rotary_sensor_left_config}; 20 | proxy::RotarySensor rotary_sensor_right{rotary_sensor_right_config}; 21 | proxy::Locomotion locomotion{locomotion_config}; 22 | proxy::Button button{button_config}; 23 | bool running{}; 24 | 25 | locomotion.enable(); 26 | 27 | // proxy::RotarySensor::CommandFrame command_frame{}; 28 | // proxy::RotarySensor::DataFrame data_frame{}; 29 | 30 | // command_frame.fields.address = proxy::RotarySensor::Registers::settings2_addr; 31 | // data_frame.fields.data = rotary_sensor_right_config.registers.settings2.raw; 32 | // rotary_sensor_right.write_register(command_frame, data_frame); 33 | 34 | // // command_frame.fields.address = proxy::RotarySensor::Registers::settings3_addr; 35 | // // data_frame.fields.data = rotary_sensor_right_config.registers.settings3.raw; 36 | // rotary_sensor_right.write_register(command_frame, data_frame); 37 | 38 | // command_frame.fields.address = proxy::RotarySensor::Registers::disable_addr; 39 | // data_frame.fields.data = rotary_sensor_right_config.registers.disable.raw; 40 | // rotary_sensor_right.write_register(command_frame, data_frame); 41 | 42 | TestCore::loop([&rotary_sensor_left, &rotary_sensor_right, &button, &locomotion, &running]() { 43 | test_left_position = rotary_sensor_left.get_position(); 44 | test_right_position = rotary_sensor_right.get_position(); 45 | 46 | button.update(); 47 | 48 | if (button.get_status() != proxy::Button::Status::NO_PRESS) { 49 | running = not running; 50 | 51 | if (running) { 52 | locomotion.enable(); 53 | locomotion.set_command(linear_speed, 0.0F); 54 | } else { 55 | locomotion.disable(); 56 | locomotion.stop(); 57 | } 58 | } 59 | }); 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /micras_nav/src/grid_pose.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/nav/grid_pose.hpp" 8 | 9 | namespace micras::nav { 10 | Side angle_to_grid(float angle) { 11 | const int32_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); 12 | return static_cast(grid_angle < 0 ? 4 + (grid_angle % 4) : grid_angle % 4); 13 | } 14 | 15 | Side GridPoint::direction(const GridPoint& next) const { 16 | if (next.x > this->x) { 17 | return Side::RIGHT; 18 | } 19 | 20 | if (next.y > this->y) { 21 | return Side::UP; 22 | } 23 | 24 | if (next.x < this->x) { 25 | return Side::LEFT; 26 | } 27 | 28 | if (next.y < this->y) { 29 | return Side::DOWN; 30 | } 31 | 32 | return Side::UP; 33 | } 34 | 35 | GridPoint GridPoint::from_vector(const core::Vector& point, float cell_size) { 36 | return {static_cast(point.x / cell_size), static_cast(point.y / cell_size)}; 37 | } 38 | 39 | core::Vector GridPoint::to_vector(float cell_size) const { 40 | return {cell_size * (this->x + 0.5F), cell_size * (this->y + 0.5F)}; 41 | } 42 | 43 | GridPoint GridPoint::operator+(const Side& side) const { 44 | switch (side) { 45 | case Side::RIGHT: 46 | return {static_cast(this->x + 1), this->y}; 47 | case Side::UP: 48 | return {this->x, static_cast(this->y + 1)}; 49 | case Side::LEFT: 50 | return {static_cast(this->x - 1), this->y}; 51 | case Side::DOWN: 52 | return {this->x, static_cast(this->y - 1)}; 53 | } 54 | 55 | return *this; 56 | } 57 | 58 | bool GridPoint::operator==(const GridPoint& other) const { 59 | return this->x == other.x and this->y == other.y; 60 | } 61 | 62 | GridPose GridPose::front() const { 63 | return {this->position + this->orientation, this->orientation}; 64 | } 65 | 66 | GridPose GridPose::turned_back() const { 67 | return {this->position, static_cast((this->orientation + 2) % 4)}; 68 | } 69 | 70 | GridPose GridPose::turned_left() const { 71 | return {this->position, static_cast((this->orientation + 1) % 4)}; 72 | } 73 | 74 | GridPose GridPose::turned_right() const { 75 | return {this->position, static_cast((this->orientation + 3) % 4)}; 76 | } 77 | 78 | bool GridPose::operator==(const GridPose& other) const { 79 | return this->position == other.position and this->orientation == other.orientation; 80 | } 81 | } // namespace micras::nav 82 | -------------------------------------------------------------------------------- /cmake/templates/helpme.in: -------------------------------------------------------------------------------- 1 | 2 | ------------------------------ Thunder CMake ------------------------------ 3 | Welcome to ThundeRatz's CMakeLists, check the current configurations 4 | and modify the file if necessary 5 | 6 | Options: 7 | all: build main project target, can also be called using only make 8 | helpme: show this help 9 | cube: generate cube files 10 | info: show information about the connected uC (microcontroller) 11 | [test_name]: compile the project with the test file as executable 12 | test_all: compile all test files and generate tests executables 13 | flash: load the compiled files of the main program into the microcontroller via st-link 14 | flash_[test_name]: load the compiled files of a test into the microcontroller via st-link 15 | jflash: load the compiled files of the main program into the microcontroller via j-link 16 | jflash_[test_name]: load the compiled files of a test into the microcontroller via j-link 17 | debug generate the VS Code files for debugging the main program 18 | debug_[test_name]: generate the VS Code files for debugging a test 19 | format: format .c/.h files 20 | lint: run clang-tidy checks on all source files 21 | lint_fix: run clang-tidy checks on all source files and fix when possible 22 | clear: delete all files in the /build folder 23 | clear_cube: clean the files generated by Cube 24 | clear_all: delete all files in the /build folder and the files generated by Cube 25 | reset: reset the microcontroller 26 | rebuild: recompile the files in the /build folder 27 | rebuild_all: recompile the files in the /build folder and the files generated by Cube 28 | docs: generate the Doxygen documentation 29 | 30 | Project Variables: 31 | When running the cmake command, you can set the following variables 32 | by adding `-D{VARIABLE}={VALUE}` after `cmake ..` in the terminal: 33 | BUILD_TYPE: Set the project build type 34 | Possible values: Debug, Release, RelWithDebInfo, MinSizeRel (default: RelWithDebInfo) 35 | 36 | Current configuration: 37 | PROJECT NAME = ${CMAKE_PROJECT_NAME} 38 | IOC FILE = ${PROJECT_RELEASE}.ioc 39 | DEVICE = ${DEVICE} 40 | BUILD TYPE = ${BUILD_TYPE} 41 | 42 | --------------------------------------------------------------------------- 43 | -------------------------------------------------------------------------------- /micras_core/include/micras/core/vector.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_CORE_VECTOR_HPP 6 | #define MICRAS_CORE_VECTOR_HPP 7 | 8 | namespace micras::core { 9 | /** 10 | * @brief Type to store a point in 2D space. 11 | */ 12 | struct Vector { 13 | /** 14 | * @brief Calculate the distance to another point. 15 | * 16 | * @param other The other point. 17 | * @return The distance to the other point. 18 | */ 19 | float distance(const Vector& other) const; 20 | 21 | /** 22 | * @brief Calculate the distance of the point to the origin. 23 | * 24 | * @return The distance of the point to the origin. 25 | */ 26 | float magnitude() const; 27 | 28 | /** 29 | * @brief Calculate the angle between two points. 30 | * 31 | * @param other The other point. 32 | * @return The angle between the two points. 33 | */ 34 | float angle_between(const Vector& other) const; 35 | 36 | /** 37 | * @brief Move the point towards another point by a given distance. 38 | * 39 | * @param other The point to move towards. 40 | * @param distance The distance to move. 41 | * @return The point after moving towards the other point. 42 | */ 43 | Vector move_towards(const Vector& other, float distance) const; 44 | 45 | /** 46 | * @brief Add a point to another. 47 | * 48 | * @param other The point to add. 49 | * @return The result of the addition. 50 | */ 51 | Vector operator+(const Vector& other) const; 52 | 53 | /** 54 | * @brief Subtract a point from another. 55 | * 56 | * @param other The point to subtract. 57 | * @return The result of the subtraction. 58 | */ 59 | Vector operator-(const Vector& other) const; 60 | 61 | /** 62 | * @brief Compare two points for equality. 63 | * 64 | * @param other The other point to compare. 65 | * @return True if the points are equal, false otherwise. 66 | */ 67 | bool operator==(const Vector& other) const; 68 | 69 | /** 70 | * @brief Calculate the remainder of the division of the point by a value. 71 | * 72 | * @param value The value to divide the point by. 73 | * @return The remainder of the division. 74 | */ 75 | Vector operator%(float value) const; 76 | 77 | /** 78 | * @brief The x coordinate of the point in 2D space. 79 | */ 80 | float x; 81 | 82 | /** 83 | * @brief The y coordinate of the point in 2D space. 84 | */ 85 | float y; 86 | }; 87 | } // namespace micras::core 88 | 89 | #endif // MICRAS_CORE_VECTOR_HPP 90 | -------------------------------------------------------------------------------- /micras_core/include/micras/core/butterworth_filter.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_CORE_BUTTERWORTH_FILTER_HPP 6 | #define MICRAS_CORE_BUTTERWORTH_FILTER_HPP 7 | 8 | #include 9 | #include 10 | 11 | namespace micras::core { 12 | /** 13 | * @brief Implementation of Butterworth second order low-pass filter. 14 | * 15 | * @details 16 | * A generic digital filter follows the relation: 17 | * a0 * y[k] = sum(bi * x[k - i]) - sum(aj * y[k - j]) 18 | * Where: 19 | * x[k] - measurement at instant k 20 | * y[k] - filtered signal at instant k 21 | * The Butterworth filter have the special property of being a 22 | * maximally flat magnitude filter, in other words, is the best 23 | * filter that doesn't present distortions around the cutoff 24 | * frequency. 25 | * 26 | * @see 27 | * The formula for the continuos coefficients of the Butterworth 28 | * filter is available here: 29 | * https://en.wikipedia.org/wiki/Butterworth_filter 30 | * The discrete version were computed with the Tustin method: 31 | * https://en.wikipedia.org/wiki/Bilinear_transform 32 | */ 33 | class ButterworthFilter { 34 | public: 35 | /** 36 | * @brief Construct a new Butterworth Second Order filter object. 37 | * 38 | * @param cutoff_frequency Low-pass cutoff frequency in Hz. 39 | * @param sampling_frequency Sampling frequency in Hz. 40 | */ 41 | explicit ButterworthFilter(float cutoff_frequency, float sampling_frequency = 100.0F); 42 | 43 | /** 44 | * @brief Produce a new value from measured data. 45 | * 46 | * @param x0 Last measure. 47 | * @return Filtered value. 48 | */ 49 | float update(float x0); 50 | 51 | /** 52 | * @brief Get the last filtered value. 53 | * 54 | * @return Last filtered value. 55 | */ 56 | float get_last() const; 57 | 58 | private: 59 | /** 60 | * @brief Order of the filter. 61 | */ 62 | static constexpr uint8_t filter_order{2}; 63 | 64 | /** 65 | * @brief Last input values of the filter. 66 | */ 67 | std::array x_array{}; 68 | 69 | /** 70 | * @brief Last output values of the filter. 71 | */ 72 | std::array y_array{}; 73 | 74 | /** 75 | * @brief Coefficients of the filter related to the output value. 76 | */ 77 | std::array a_array{}; 78 | 79 | /** 80 | * @brief Coefficients of the filter related to the input value. 81 | */ 82 | std::array b_array{}; 83 | }; 84 | } // namespace micras::core 85 | 86 | #endif // MICRAS_CORE_BUTTERWORTH_FILTER_HPP 87 | -------------------------------------------------------------------------------- /micras_nav/include/micras/nav/state.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_NAV_POSE_HPP 6 | #define MICRAS_NAV_POSE_HPP 7 | 8 | #include "micras/core/vector.hpp" 9 | #include "micras/nav/grid_pose.hpp" 10 | 11 | namespace micras::nav { 12 | /** 13 | * @brief Type to store a pose in 2D space. 14 | */ 15 | struct Pose { 16 | /** 17 | * @brief Convert the pose to a grid pose. 18 | * 19 | * @param cell_size The size of the grid cells. 20 | * @return The grid pose corresponding to the pose. 21 | */ 22 | GridPose to_grid(float cell_size) const; 23 | 24 | /** 25 | * @brief Convert the pose to a cell reference. 26 | * 27 | * @param cell_size The size of the grid cells. 28 | * @return The point inside the cell reference. 29 | */ 30 | core::Vector to_cell(float cell_size) const; 31 | 32 | /** 33 | * @brief The position of the pose. 34 | */ 35 | core::Vector position; 36 | 37 | /** 38 | * @brief The orientation of the pose. 39 | */ 40 | float orientation; 41 | }; 42 | 43 | /** 44 | * @brief Type to store a twist in 2D space. 45 | */ 46 | struct Twist { 47 | /** 48 | * @brief The linear velocity of the twist. 49 | */ 50 | float linear; 51 | 52 | /** 53 | * @brief The angular velocity of the twist. 54 | */ 55 | float angular; 56 | }; 57 | 58 | /** 59 | * @brief Type to store the state of the robot. 60 | */ 61 | struct State { 62 | /** 63 | * @brief The pose of the robot. 64 | */ 65 | Pose pose; 66 | 67 | /** 68 | * @brief The velocity of the robot. 69 | */ 70 | Twist velocity; 71 | }; 72 | 73 | class RelativePose { 74 | public: 75 | /** 76 | * @brief Construct a new Relative Pose object. 77 | * 78 | * @param absolute_pose The absolute pose to be used as a reference. 79 | */ 80 | explicit RelativePose(const Pose& absolute_pose); 81 | 82 | /** 83 | * @brief Get the relative pose. 84 | * 85 | * @return The pose relative to the reference. 86 | */ 87 | Pose get() const; 88 | 89 | /** 90 | * @brief Reset the reference to the current absolute pose. 91 | */ 92 | void reset_reference(); 93 | 94 | private: 95 | /** 96 | * @brief A reference to the absolute pose. 97 | */ 98 | const Pose* absolute_pose; 99 | 100 | /** 101 | * @brief The reference pose to be used for calculations. 102 | */ 103 | Pose reference_pose{}; 104 | }; 105 | } // namespace micras::nav 106 | 107 | #endif // MICRAS_NAV_POSE_HPP 108 | -------------------------------------------------------------------------------- /micras_proxy/src/wall_sensors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_WALL_SENSORS_CPP 6 | #define MICRAS_PROXY_WALL_SENSORS_CPP 7 | 8 | #include "micras/core/utils.hpp" 9 | #include "micras/proxy/wall_sensors.hpp" 10 | 11 | namespace micras::proxy { 12 | template 13 | TWallSensors::TWallSensors(const Config& config) : 14 | adc{config.adc}, 15 | led_0_pwm{config.led_0_pwm}, 16 | led_1_pwm{config.led_1_pwm}, 17 | filters{core::make_array(config.filter_cutoff)}, 18 | base_readings{config.base_readings}, 19 | uncertainty{config.uncertainty} { 20 | this->adc.start_dma(this->buffer); 21 | this->turn_off(); 22 | } 23 | 24 | template 25 | void TWallSensors::turn_on() { 26 | this->led_0_pwm.set_duty_cycle(50.0F); 27 | this->led_1_pwm.set_duty_cycle(50.0F); 28 | } 29 | 30 | template 31 | void TWallSensors::turn_off() { 32 | this->led_0_pwm.set_duty_cycle(0.0F); 33 | this->led_1_pwm.set_duty_cycle(0.0F); 34 | } 35 | 36 | template 37 | void TWallSensors::update() { 38 | for (uint8_t i = 0; i < num_of_sensors; i++) { 39 | this->filters[i].update(this->get_adc_reading(i)); 40 | } 41 | } 42 | 43 | template 44 | bool TWallSensors::get_wall(uint8_t sensor_index, bool disturbed) const { 45 | return this->filters.at(sensor_index).get_last() > 46 | this->base_readings.at(sensor_index) * this->uncertainty * (disturbed ? 1.2F : 1.0F); 47 | } 48 | 49 | template 50 | float TWallSensors::get_reading(uint8_t sensor_index) const { 51 | return this->filters.at(sensor_index).get_last(); 52 | } 53 | 54 | template 55 | float TWallSensors::get_adc_reading(uint8_t sensor_index) const { 56 | return static_cast(std::abs(this->buffer.at(sensor_index) - this->buffer.at(sensor_index + num_of_sensors)) 57 | ) / 58 | this->adc.get_max_reading(); 59 | } 60 | 61 | template 62 | float TWallSensors::get_sensor_error(uint8_t sensor_index) const { 63 | return this->get_reading(sensor_index) - this->base_readings.at(sensor_index); 64 | } 65 | 66 | template 67 | void TWallSensors::calibrate_sensor(uint8_t sensor_index) { 68 | this->base_readings.at(sensor_index) = this->get_reading(sensor_index); 69 | } 70 | } // namespace micras::proxy 71 | 72 | #endif // MICRAS_PROXY_WALL_SENSORS_CPP 73 | -------------------------------------------------------------------------------- /micras_nav/src/speed_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | #include "micras/nav/speed_controller.hpp" 9 | 10 | namespace micras::nav { 11 | SpeedController::SpeedController(const Config& config) : 12 | max_linear_acceleration{config.max_linear_acceleration}, 13 | max_angular_acceleration{config.max_angular_acceleration}, 14 | linear_pid(config.linear_pid), 15 | angular_pid(config.angular_pid), 16 | left_feed_forward{config.left_feed_forward}, 17 | right_feed_forward{config.right_feed_forward} { } 18 | 19 | std::pair SpeedController::compute_control_commands( 20 | const Twist& current_twist, const Twist& desired_twist, float elapsed_time 21 | ) { 22 | this->linear_pid.set_setpoint(desired_twist.linear); 23 | this->angular_pid.set_setpoint(desired_twist.angular); 24 | 25 | const float linear_command = this->linear_pid.compute_response(current_twist.linear, elapsed_time); 26 | const float angular_command = this->angular_pid.compute_response(current_twist.angular, elapsed_time); 27 | 28 | return {linear_command - angular_command, linear_command + angular_command}; 29 | } 30 | 31 | std::pair SpeedController::compute_feed_forward_commands(const Twist& desired_twist, float elapsed_time) { 32 | const float linear_acceleration = (desired_twist.linear - this->last_linear_speed) / elapsed_time; 33 | const float angular_acceleration = (desired_twist.angular - this->last_angular_speed) / elapsed_time; 34 | const Twist acceleration_twist{ 35 | .linear = std::clamp(linear_acceleration, -this->max_linear_acceleration, this->max_linear_acceleration), 36 | .angular = std::clamp(angular_acceleration, -this->max_angular_acceleration, this->max_angular_acceleration), 37 | }; 38 | 39 | this->last_linear_speed = desired_twist.linear; 40 | this->last_angular_speed = desired_twist.angular; 41 | 42 | const float left_feed_forward = feed_forward(desired_twist, acceleration_twist, this->left_feed_forward); 43 | const float right_feed_forward = feed_forward(desired_twist, acceleration_twist, this->right_feed_forward); 44 | 45 | return {left_feed_forward, right_feed_forward}; 46 | } 47 | 48 | float SpeedController::feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config) { 49 | return config.linear_speed * speed.linear + config.linear_acceleration * acceleration.linear + 50 | config.angular_speed * speed.angular + config.angular_acceleration * acceleration.angular; 51 | } 52 | 53 | void SpeedController::reset() { 54 | this->linear_pid.reset(); 55 | this->angular_pid.reset(); 56 | } 57 | } // namespace micras::nav 58 | -------------------------------------------------------------------------------- /micras_core/include/micras/core/fsm.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_CORE_FSM_HPP 6 | #define MICRAS_CORE_FSM_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace micras::core { 13 | class FSM { 14 | public: 15 | class State { 16 | public: 17 | /** 18 | * @brief Destroy the State object. 19 | */ 20 | virtual ~State() = default; 21 | 22 | /** 23 | * @brief Execute the entry function of this state. 24 | */ 25 | virtual void on_entry() = 0; 26 | 27 | /** 28 | * @brief Execute this state. 29 | * 30 | * @return The id of the next state. 31 | */ 32 | virtual uint8_t execute() = 0; 33 | 34 | /** 35 | * @brief Get the id object of the state. 36 | * 37 | * @return The id of the state. 38 | */ 39 | uint8_t get_id() const; 40 | 41 | /** 42 | * @brief The id of the state that is not valid. 43 | */ 44 | static constexpr uint8_t invalid_id{0xFF}; 45 | 46 | protected: 47 | /** 48 | * @brief Special member functions declared as default. 49 | */ 50 | ///@{ 51 | explicit State(uint8_t id); 52 | State(const State&) = default; 53 | State(State&&) = default; 54 | State& operator=(const State&) = default; 55 | State& operator=(State&&) = default; 56 | ///@} 57 | 58 | private: 59 | /** 60 | * @brief Fixed id of the state. 61 | */ 62 | uint8_t id; 63 | }; 64 | 65 | /** 66 | * @brief Construct a new FSM object. 67 | * 68 | * @param initial_state_id The id of the initial state. 69 | */ 70 | explicit FSM(uint8_t initial_state_id); 71 | 72 | /** 73 | * @brief Add a state to the FSM. 74 | * 75 | * @param state The state to be added. 76 | */ 77 | void add_state(std::unique_ptr state); 78 | 79 | /** 80 | * @brief Run the FSM current state to compute the next state. 81 | */ 82 | void update(); 83 | 84 | private: 85 | /** 86 | * @brief Map of ids to states. 87 | */ 88 | std::unordered_map> states; 89 | 90 | /** 91 | * @brief Id of the state currently running. 92 | */ 93 | uint8_t current_state_id{0}; 94 | 95 | /** 96 | * @brief Id of the last executed state. 97 | */ 98 | uint8_t previous_state_id{State::invalid_id}; 99 | }; 100 | } // namespace micras::core 101 | 102 | #endif // MICRAS_CORE_FSM_HPP 103 | -------------------------------------------------------------------------------- /micras_core/src/pid_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | #include "micras/core/pid_controller.hpp" 9 | 10 | // NOLINTBEGIN(cppcoreguidelines-avoid-non-const-global-variables) 11 | static volatile float test_error; 12 | static volatile float test_proportional; 13 | static volatile float test_integrative; 14 | static volatile float test_derivative; 15 | static volatile float test_response; 16 | 17 | // NOLINTEND(cppcoreguidelines-avoid-non-const-global-variables) 18 | 19 | namespace micras::core { 20 | PidController::PidController(Config config) : 21 | kp{config.kp}, 22 | ki{config.ki}, 23 | kd{config.kd}, 24 | setpoint{config.setpoint}, 25 | saturation{config.saturation}, 26 | max_integral{config.max_integral} { } 27 | 28 | void PidController::set_setpoint(float setpoint) { 29 | this->setpoint = setpoint; 30 | } 31 | 32 | void PidController::reset() { 33 | this->error_acc = 0; 34 | this->prev_state = 0; 35 | this->last_response = 0; 36 | } 37 | 38 | float PidController::compute_response(float state, float elapsed_time, bool save) { 39 | const float state_change = (state - this->prev_state) / elapsed_time; 40 | return this->compute_response(state, elapsed_time, state_change, save); 41 | } 42 | 43 | float PidController::compute_response(float state, float elapsed_time, float state_change, bool save) { 44 | const float error = this->setpoint - state; 45 | this->prev_state = state; 46 | 47 | float response = this->kp * (error + this->ki * this->error_acc - this->kd * state_change); 48 | 49 | if (this->saturation < 0 or std::abs(response) < this->saturation or 50 | (this->error_acc != 0 and std::signbit(this->error_acc) != std::signbit(error))) { 51 | this->error_acc += error * elapsed_time; 52 | } 53 | 54 | if (this->max_integral >= 0 and this->ki > 0) { 55 | this->error_acc = std::clamp( 56 | this->error_acc, -this->max_integral / (this->kp * this->ki), this->max_integral / (this->kp * this->ki) 57 | ); 58 | } 59 | 60 | response = this->kp * (error + this->ki * this->error_acc - this->kd * state_change); 61 | 62 | if (this->saturation >= 0 and std::abs(response) >= this->saturation) { 63 | response = std::clamp(response, -this->saturation, this->saturation); 64 | } 65 | 66 | this->last_response = response; 67 | 68 | if (save) { 69 | test_error = error; 70 | test_proportional = this->kp * error; 71 | test_integrative = this->kp * this->ki * this->error_acc; 72 | test_derivative = -this->kp * this->kd * state_change; 73 | test_response = response; 74 | } 75 | 76 | return response; 77 | } 78 | } // namespace micras::core 79 | -------------------------------------------------------------------------------- /micras_nav/src/odometry.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/nav/odometry.hpp" 8 | 9 | namespace micras::nav { 10 | Odometry::Odometry( 11 | const std::shared_ptr& left_rotary_sensor, 12 | const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, 13 | Config config 14 | ) : 15 | left_rotary_sensor{left_rotary_sensor}, 16 | right_rotary_sensor{right_rotary_sensor}, 17 | imu{imu}, 18 | wheel_radius{config.wheel_radius}, 19 | left_last_position{left_rotary_sensor->get_position()}, 20 | right_last_position{right_rotary_sensor->get_position()}, 21 | linear_filter{config.linear_cutoff_frequency}, 22 | state{config.initial_pose, {0.0F, 0.0F}} { } 23 | 24 | void Odometry::update(float elapsed_time) { 25 | if (this->imu.use_count() == 1) { 26 | this->imu->update(); 27 | } 28 | 29 | const float left_position = this->left_rotary_sensor->get_position(); 30 | const float right_position = this->right_rotary_sensor->get_position(); 31 | 32 | const float left_distance = this->wheel_radius * (left_position - this->left_last_position); 33 | const float right_distance = this->wheel_radius * (right_position - this->right_last_position); 34 | 35 | this->left_last_position = left_position; 36 | this->right_last_position = right_position; 37 | 38 | const float linear_distance = (left_distance + right_distance) / 2; 39 | 40 | this->state.velocity.linear = this->linear_filter.update(linear_distance / elapsed_time); 41 | this->state.velocity.angular = this->imu->get_angular_velocity(proxy::Imu::Axis::Z); 42 | 43 | const float angular_distance = this->state.velocity.angular * elapsed_time; 44 | 45 | const float half_angle = angular_distance / 2; 46 | const float linear_diagonal = 47 | angular_distance < 0.05F ? linear_distance : std::abs(std::sin(half_angle) * linear_distance / half_angle); 48 | 49 | this->state.pose.position.x += linear_diagonal * std::cos(this->state.pose.orientation + half_angle); 50 | this->state.pose.position.y += linear_diagonal * std::sin(this->state.pose.orientation + half_angle); 51 | 52 | this->state.pose.orientation += angular_distance; 53 | } 54 | 55 | void Odometry::reset() { 56 | this->left_last_position = this->left_rotary_sensor->get_position(); 57 | this->right_last_position = this->right_rotary_sensor->get_position(); 58 | this->state = {{{0.0F, 0.0F}, 0.0F}, {0.0F, 0.0F}}; 59 | } 60 | 61 | const nav::State& Odometry::get_state() const { 62 | return this->state; 63 | } 64 | 65 | void Odometry::set_state(const nav::State& new_state) { 66 | this->state = new_state; 67 | } 68 | } // namespace micras::nav 69 | -------------------------------------------------------------------------------- /cmake/utilities.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ## Utilities Functions 3 | ############################################################################### 4 | 5 | # This function adds a target with name '${TARGET}_always_display_size'. The new 6 | # target builds a TARGET and then calls the program defined in CMAKE_SIZE to 7 | # display the size of the final ELF. 8 | function(print_size_of_target TARGET) 9 | add_custom_command( 10 | TARGET ${TARGET} 11 | POST_BUILD 12 | COMMAND ${CMAKE_SIZE} "$" 13 | COMMENT "Target Sizes: " 14 | ) 15 | endfunction() 16 | 17 | # This function calls the objcopy program defined in CMAKE_OBJCOPY to generate 18 | # file with object format specified in OBJCOPY_BFD_OUTPUT. 19 | # The generated file has the name of the target output but with extension 20 | # corresponding to the OUTPUT_EXTENSION argument value. 21 | # The generated file will be placed in the same directory as the target output file. 22 | function(_generate_file TARGET OUTPUT_EXTENSION OBJCOPY_BFD_OUTPUT) 23 | set(OUTPUT_FILE_NAME "${TARGET}.${OUTPUT_EXTENSION}") 24 | 25 | add_custom_command( 26 | TARGET ${TARGET} 27 | POST_BUILD 28 | COMMAND ${CMAKE_OBJCOPY} -O ${OBJCOPY_BFD_OUTPUT} "$" ${OUTPUT_FILE_NAME} 29 | BYPRODUCTS ${OUTPUT_FILE_NAME} 30 | COMMENT "Generating ${OBJCOPY_BFD_OUTPUT} file ${OUTPUT_FILE_NAME}" 31 | ) 32 | endfunction() 33 | 34 | # This function adds post-build generation of the binary file from the target ELF. 35 | # The generated file will be placed in the same directory as the ELF file. 36 | function(generate_binary_file TARGET) 37 | _generate_file(${TARGET} "bin" "binary") 38 | endfunction() 39 | 40 | # This function adds post-build generation of the Motorola S-record file from the target ELF. 41 | # The generated file will be placed in the same directory as the ELF file. 42 | function(generate_srec_file TARGET) 43 | _generate_file(${TARGET} "srec" "srec") 44 | endfunction() 45 | 46 | # This function adds post-build generation of the Intel hex file from the target ELF. 47 | # The generated file will be placed in the same directory as the ELF file. 48 | function(generate_hex_file TARGET) 49 | _generate_file(${TARGET} "hex" "ihex") 50 | endfunction() 51 | 52 | function(generate_helpme_text) 53 | configure_file( 54 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/templates/helpme.in 55 | ${CMAKE_CURRENT_BINARY_DIR}/helpme 56 | ) 57 | endfunction() 58 | 59 | function(generate_vscode_tasks_json) 60 | configure_file( 61 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/templates/tasks.json.in 62 | ${CMAKE_CURRENT_SOURCE_DIR}/.vscode/tasks.json 63 | ) 64 | endfunction() 65 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/fan.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_FAN_HPP 6 | #define MICRAS_PROXY_FAN_HPP 7 | 8 | #include 9 | 10 | #include "micras/hal/gpio.hpp" 11 | #include "micras/hal/pwm.hpp" 12 | #include "micras/proxy/stopwatch.hpp" 13 | 14 | namespace micras::proxy { 15 | /** 16 | * @brief Class for controlling the fan driver. 17 | */ 18 | class Fan { 19 | public: 20 | /** 21 | * @brief Configuration struct for the fan. 22 | */ 23 | struct Config { 24 | hal::Pwm::Config pwm; 25 | hal::Gpio::Config direction_gpio; 26 | hal::Gpio::Config enable_gpio; 27 | float max_acceleration; 28 | }; 29 | 30 | /** 31 | * @brief Construct a new fan object. 32 | * 33 | * @param config Configuration for the fan driver. 34 | */ 35 | explicit Fan(const Config& config); 36 | 37 | /** 38 | * @brief Enable the fan. 39 | */ 40 | void enable(); 41 | 42 | /** 43 | * @brief Disable the fan. 44 | */ 45 | void disable(); 46 | 47 | /** 48 | * @brief Set the speed of the fans. 49 | * 50 | * @param speed Speed percentage of the fan. 51 | */ 52 | void set_speed(float speed); 53 | 54 | /** 55 | * @brief Update the speed of the fan. 56 | */ 57 | float update(); 58 | 59 | /** 60 | * @brief Stop the fan. 61 | */ 62 | void stop(); 63 | 64 | private: 65 | /** 66 | * @brief Enum for rotation direction. 67 | */ 68 | enum RotationDirection : uint8_t { 69 | FORWARD = 0, 70 | BACKWARDS = 1 71 | }; 72 | 73 | /** 74 | * @brief Set the rotation direction of the fan. 75 | * 76 | * @param direction Rotation direction. 77 | */ 78 | void set_direction(RotationDirection direction); 79 | 80 | /** 81 | * @brief PWM object for controlling the fan speed. 82 | */ 83 | hal::Pwm pwm; 84 | 85 | /** 86 | * @brief GPIO object for controlling the fan rotation direction. 87 | */ 88 | hal::Gpio direction_gpio; 89 | 90 | /** 91 | * @brief GPIO handle for the fan enable pin. 92 | */ 93 | hal::Gpio enable_gpio; 94 | 95 | /** 96 | * @brief Current speed of the fan. 97 | */ 98 | float current_speed{}; 99 | 100 | /** 101 | * @brief Target speed of the fan. 102 | */ 103 | float target_speed{}; 104 | 105 | /** 106 | * @brief Maximum acceleration of the fan in percentage per millisecond. 107 | */ 108 | float max_acceleration; 109 | 110 | /** 111 | * @brief Stopwatch for limiting the acceleration of the fan. 112 | */ 113 | proxy::Stopwatch acceleration_stopwatch; 114 | }; 115 | } // namespace micras::proxy 116 | 117 | #endif // MICRAS_PROXY_FAN_HPP 118 | -------------------------------------------------------------------------------- /micras_nav/include/micras/nav/odometry.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_NAV_ODOMETRY_HPP 6 | #define MICRAS_NAV_ODOMETRY_HPP 7 | 8 | #include 9 | 10 | #include "micras/core/butterworth_filter.hpp" 11 | #include "micras/nav/state.hpp" 12 | #include "micras/proxy/imu.hpp" 13 | #include "micras/proxy/rotary_sensor.hpp" 14 | 15 | namespace micras::nav { 16 | /** 17 | * @brief Class for computing the robot odometry. 18 | */ 19 | class Odometry { 20 | public: 21 | /** 22 | * @brief Configuration for the odometry. 23 | */ 24 | struct Config { 25 | float linear_cutoff_frequency; 26 | float wheel_radius; 27 | Pose initial_pose; 28 | }; 29 | 30 | /** 31 | * @brief Construct a new Odometry object. 32 | * 33 | * @param left_rotary_sensor Left rotary sensor. 34 | * @param right_rotary_sensor Right rotary sensor. 35 | * @param imu IMU sensor. 36 | * @param config Configuration for the odometry. 37 | */ 38 | Odometry( 39 | const std::shared_ptr& left_rotary_sensor, 40 | const std::shared_ptr& right_rotary_sensor, const std::shared_ptr& imu, 41 | Config config 42 | ); 43 | 44 | /** 45 | * @brief Update the odometry. 46 | * 47 | * @param elapsed_time Time since the last update. 48 | */ 49 | void update(float elapsed_time); 50 | 51 | /** 52 | * @brief Reset the odometry. 53 | */ 54 | void reset(); 55 | 56 | /** 57 | * @brief Get the state of the robot. 58 | * 59 | * @return Current state of the robot in space. 60 | */ 61 | const State& get_state() const; 62 | 63 | /** 64 | * @brief Set the state of the robot. 65 | * 66 | * @param state New state of the robot. 67 | */ 68 | void set_state(const State& new_state); 69 | 70 | private: 71 | /** 72 | * @brief Left rotary sensor. 73 | */ 74 | std::shared_ptr left_rotary_sensor; 75 | 76 | /** 77 | * @brief Right rotary sensor. 78 | */ 79 | std::shared_ptr right_rotary_sensor; 80 | 81 | /** 82 | * @brief IMU sensor. 83 | */ 84 | std::shared_ptr imu; 85 | 86 | /** 87 | * @brief Wheel radius. 88 | */ 89 | float wheel_radius; 90 | 91 | /** 92 | * @brief Last left rotary sensor position. 93 | */ 94 | float left_last_position{}; 95 | 96 | /** 97 | * @brief Last right rotary sensor position. 98 | */ 99 | float right_last_position{}; 100 | 101 | /** 102 | * @brief Linear velocity filter. 103 | */ 104 | core::ButterworthFilter linear_filter; 105 | 106 | /** 107 | * @brief Current state of the robot in space. 108 | */ 109 | State state; 110 | }; 111 | } // namespace micras::nav 112 | 113 | #endif // MICRAS_NAV_ODOMETRY_HPP 114 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/rotary_sensor.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_ROTARY_SENSOR_HPP 6 | #define MICRAS_PROXY_ROTARY_SENSOR_HPP 7 | 8 | #include 9 | 10 | #include "micras/hal/crc.hpp" 11 | #include "micras/hal/encoder.hpp" 12 | #include "micras/hal/spi.hpp" 13 | 14 | namespace micras::proxy { 15 | /** 16 | * @brief Class for acquiring rotary sensor data. 17 | */ 18 | class RotarySensor { 19 | public: 20 | #include "micras/proxy/rotary_sensor_reg.hpp" 21 | 22 | /** 23 | * @brief Rotary sensor configuration struct. 24 | */ 25 | struct Config { 26 | hal::Spi::Config spi; 27 | hal::Encoder::Config encoder; 28 | hal::Crc::Config crc; 29 | uint32_t resolution; 30 | Registers registers; 31 | }; 32 | 33 | union CommandFrame { 34 | struct __attribute__((__packed__)) Fields { 35 | uint8_t crc : 8; 36 | uint16_t address : 14; 37 | uint8_t rw : 1; 38 | uint8_t do_not_care : 1; 39 | }; 40 | 41 | Fields fields; 42 | uint32_t raw; 43 | }; 44 | 45 | union DataFrame { 46 | struct __attribute__((__packed__)) Fields { 47 | uint8_t crc : 8; 48 | uint16_t data : 14; 49 | uint8_t error : 1; 50 | uint8_t warning : 1; 51 | }; 52 | 53 | Fields fields; 54 | uint32_t raw; 55 | }; 56 | 57 | /** 58 | * @brief Construct a new RotarySensor object. 59 | * 60 | * @param config Configuration for the rotary sensor. 61 | */ 62 | explicit RotarySensor(const Config& config); 63 | 64 | /** 65 | * @brief Get the rotary sensor position over an axis. 66 | * 67 | * @return Current angular position of the sensor in radians. 68 | */ 69 | float get_position() const; 70 | 71 | /** 72 | * @brief Read a register to the rotary sensor. 73 | * 74 | * @param command_frame Command frame to send trough SPI. 75 | */ 76 | uint16_t read_register(uint16_t address); 77 | 78 | /** 79 | * @brief Write a register to the rotary sensor. 80 | * 81 | * @param command_frame Command frame to send trough SPI. 82 | * @param data_frame Data frame to send trough SPI. 83 | */ 84 | void write_register(CommandFrame& command_frame, DataFrame& data_frame); 85 | 86 | private: 87 | /** 88 | * @brief SPI for the rotary sensor configuration. 89 | */ 90 | hal::Spi spi; 91 | 92 | /** 93 | * @brief Encoder for getting the rotary sensor data. 94 | */ 95 | hal::Encoder encoder; 96 | 97 | /** 98 | * @brief CRC for the rotary sensor configuration. 99 | */ 100 | hal::Crc crc; 101 | 102 | /** 103 | * @brief Resolution of the rotary sensor. 104 | */ 105 | uint32_t resolution; 106 | }; 107 | } // namespace micras::proxy 108 | 109 | #endif // MICRAS_PROXY_ROTARY_SENSOR_HPP 110 | -------------------------------------------------------------------------------- /include/micras/interface.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_INTERFACE_HPP 6 | #define MICRAS_INTERFACE_HPP 7 | 8 | #include 9 | 10 | #include "micras/proxy/argb.hpp" 11 | #include "micras/proxy/button.hpp" 12 | #include "micras/proxy/buzzer.hpp" 13 | #include "micras/proxy/dip_switch.hpp" 14 | #include "micras/proxy/led.hpp" 15 | 16 | namespace micras { 17 | /** 18 | * @brief Class for controlling the robot interface with the external world. 19 | */ 20 | class Interface { 21 | public: 22 | /** 23 | * @brief Enum for the events that can be sent to the interface. 24 | */ 25 | enum Event : uint8_t { 26 | EXPLORE = 0, 27 | SOLVE = 1, 28 | CALIBRATE = 2, 29 | ERROR = 3, 30 | NUMBER_OF_EVENTS = 4, 31 | }; 32 | 33 | /** 34 | * @brief Construct a new Interface object. 35 | * 36 | * @param argb Shared pointer to the addressable RGB LED object. 37 | * @param button Shared pointer to the button object. 38 | * @param buzzer Shared pointer to the buzzer object. 39 | * @param dip_switch Shared pointer to the DIP switch object. 40 | * @param led Shared pointer to the LED object. 41 | */ 42 | Interface( 43 | const std::shared_ptr>& argb, const std::shared_ptr& button, 44 | const std::shared_ptr& buzzer, const std::shared_ptr>& dip_switch, 45 | const std::shared_ptr& led 46 | ); 47 | 48 | /** 49 | * @brief Update the interface. 50 | */ 51 | void update(); 52 | 53 | /** 54 | * @brief Send an event to the interface. 55 | * 56 | * @param event The event to send. 57 | */ 58 | void send_event(Event event); 59 | 60 | /** 61 | * @brief Get the value of an event and reset it. 62 | * 63 | * @param event The event to get. 64 | * @return True if the event happened, false otherwise. 65 | */ 66 | bool acknowledge_event(Event event); 67 | 68 | /** 69 | * @brief Get the value of an event without reseting it. 70 | * 71 | * @param event The event to get. 72 | * @return True if the event happened, false otherwise. 73 | */ 74 | bool peek_event(Event event) const; 75 | 76 | private: 77 | /** 78 | * @brief Addressable RGB LED object. 79 | */ 80 | std::shared_ptr> argb; 81 | 82 | /** 83 | * @brief Button object. 84 | */ 85 | std::shared_ptr button; 86 | 87 | /** 88 | * @brief Buzzer object. 89 | */ 90 | std::shared_ptr buzzer; 91 | 92 | /** 93 | * @brief Dip switch object. 94 | */ 95 | std::shared_ptr> dip_switch; 96 | 97 | /** 98 | * @brief LED object. 99 | */ 100 | std::shared_ptr led; 101 | 102 | /** 103 | * @brief Array of the listed events. 104 | */ 105 | std::array events{}; 106 | }; 107 | } // namespace micras 108 | 109 | #endif // MICRAS_INTERFACE_HPP 110 | -------------------------------------------------------------------------------- /micras_hal/include/micras/hal/flash.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_HAL_FLASH_HPP 6 | #define MICRAS_HAL_FLASH_HPP 7 | 8 | #include 9 | #include 10 | 11 | namespace micras::hal { 12 | /** 13 | * @brief Class to handle flash memory on STM32 microcontrollers. 14 | */ 15 | class Flash { 16 | public: 17 | /** 18 | * @brief Deleted constructor for static class. 19 | */ 20 | Flash() = delete; 21 | 22 | // NOLINTBEGIN(*-avoid-c-arrays) 23 | 24 | /** 25 | * @brief Read data from flash memory at an absolute address. 26 | * 27 | * @param address Address to read from (indexed by double words). 28 | * @param data Pointer to store the data read. 29 | * @param size Size in double words of the data to read. 30 | */ 31 | static void read(uint32_t address, uint64_t data[], uint32_t size = 1); 32 | 33 | /** 34 | * @brief Read data from flash memory at an address relative to a page. 35 | * 36 | * @param page Page to read from (counting from the last to the first). 37 | * @param page_address Address inside the page to read from (indexed by double words). 38 | * @param data Pointer to store the data read. 39 | * @param size Size in double words of the data to read. 40 | */ 41 | static void read(uint16_t page, uint16_t page_address, uint64_t data[], uint32_t size = 1); 42 | 43 | /** 44 | * @brief Write data to flash memory at an absolute address. 45 | * 46 | * @param address Address to write to (indexed by double words). 47 | * @param data Pointer to the data to write. 48 | * @param size Size in double words of the data to write. 49 | */ 50 | static void write(uint32_t address, const uint64_t data[], uint32_t size = 1); 51 | 52 | /** 53 | * @brief Write data to flash memory at an address relative to a page. 54 | * 55 | * @param page Page to write to (counting from the last to the first). 56 | * @param page_address Address inside the page to write to (indexed by double words). 57 | * @param data Pointer to the data to write. 58 | * @param size Size in double words of the data to write. 59 | */ 60 | static void write(uint16_t page, uint16_t page_address, const uint64_t data[], uint32_t size = 1); 61 | 62 | // NOLINTEND(*-avoid-c-arrays) 63 | 64 | /** 65 | * @brief Erase flash memory pages. 66 | * 67 | * @param page First page to erase (counting from the last to the first). 68 | * @param number_of_pages Number of pages to erase. 69 | */ 70 | static void erase_pages(uint16_t page, uint16_t number_of_pages = 1); 71 | 72 | private: 73 | /** 74 | * @brief Number of double words per row. 75 | */ 76 | static constexpr uint32_t double_words_per_row{32}; 77 | 78 | /** 79 | * @brief Number of bytes per row. 80 | */ 81 | static constexpr uint32_t bytes_per_row{8 * double_words_per_row}; 82 | 83 | /** 84 | * @brief Number of double words per page. 85 | */ 86 | static constexpr uint32_t double_words_per_page{FLASH_PAGE_SIZE / 8}; 87 | }; 88 | } // namespace micras::hal 89 | 90 | #endif // MICRAS_HAL_FLASH_HPP 91 | -------------------------------------------------------------------------------- /micras_proxy/src/rotary_sensor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | #include "micras/proxy/rotary_sensor.hpp" 9 | 10 | namespace micras::proxy { 11 | RotarySensor::RotarySensor(const Config& config) : 12 | spi{config.spi}, encoder{config.encoder}, crc{config.crc}, resolution{config.resolution} { 13 | CommandFrame command_frame{}; 14 | DataFrame data_frame{}; 15 | 16 | command_frame.fields.address = Registers::disable_addr; 17 | data_frame.fields.data = config.registers.disable.raw; 18 | this->write_register(command_frame, data_frame); 19 | 20 | command_frame.fields.address = Registers::zposm_addr; 21 | data_frame.fields.data = config.registers.zposm.raw; 22 | this->write_register(command_frame, data_frame); 23 | 24 | command_frame.fields.address = Registers::zposl_addr; 25 | data_frame.fields.data = config.registers.zposl.raw; 26 | this->write_register(command_frame, data_frame); 27 | 28 | command_frame.fields.address = Registers::settings1_addr; 29 | data_frame.fields.data = config.registers.settings1.raw; 30 | this->write_register(command_frame, data_frame); 31 | 32 | command_frame.fields.address = Registers::settings2_addr; 33 | data_frame.fields.data = config.registers.settings2.raw; 34 | this->write_register(command_frame, data_frame); 35 | 36 | command_frame.fields.address = Registers::settings3_addr; 37 | data_frame.fields.data = config.registers.settings3.raw; 38 | this->write_register(command_frame, data_frame); 39 | 40 | command_frame.fields.address = Registers::ecc_addr; 41 | data_frame.fields.data = config.registers.ecc.raw; 42 | this->write_register(command_frame, data_frame); 43 | } 44 | 45 | float RotarySensor::get_position() const { 46 | return encoder.get_counter() * 2.0F * std::numbers::pi_v / this->resolution; 47 | } 48 | 49 | uint16_t RotarySensor::read_register(uint16_t address) { 50 | CommandFrame command_frame = {{.crc = 0, .address = address, .rw = 1, .do_not_care = 0}}; 51 | const DataFrame data_frame{}; 52 | 53 | command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; 54 | 55 | while (not this->spi.select_device()) { } 56 | this->spi.transmit({std::bit_cast(&command_frame.raw), 3}); 57 | this->spi.unselect_device(); 58 | 59 | while (not this->spi.select_device()) { } 60 | this->spi.receive({std::bit_cast(&data_frame.raw), 3}); 61 | this->spi.unselect_device(); 62 | 63 | return data_frame.fields.data; 64 | } 65 | 66 | void RotarySensor::write_register(CommandFrame& command_frame, DataFrame& data_frame) { 67 | command_frame.fields.crc = this->crc.calculate(&command_frame.raw, 2) ^ 0xFF; 68 | data_frame.fields.crc = this->crc.calculate(&data_frame.raw, 2) ^ 0xFF; 69 | 70 | while (not this->spi.select_device()) { } 71 | this->spi.transmit({std::bit_cast(&command_frame.raw), 3}); 72 | this->spi.unselect_device(); 73 | 74 | while (not this->spi.select_device()) { } 75 | this->spi.transmit({std::bit_cast(&data_frame.raw), 3}); 76 | this->spi.unselect_device(); 77 | } 78 | } // namespace micras::proxy 79 | -------------------------------------------------------------------------------- /micras_core/include/micras/core/pid_controller.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_CORE_PID_CONTROLLER_HPP 6 | #define MICRAS_CORE_PID_CONTROLLER_HPP 7 | 8 | namespace micras::core { 9 | /** 10 | * @brief Implementation of simple PID controller. Response = kp * (error + ki * integral(error) kd * d/dt(error)) 11 | */ 12 | class PidController { 13 | public: 14 | /** 15 | * @brief Configuration struct for the PID controller. 16 | */ 17 | struct Config { 18 | float kp{}; 19 | float ki{}; 20 | float kd{}; 21 | float setpoint{}; 22 | float saturation{-1.0F}; 23 | float max_integral{-1.0F}; 24 | }; 25 | 26 | /** 27 | * @brief Construct a new Pid Controller object. 28 | * 29 | * @param config Controller parameters. 30 | */ 31 | explicit PidController(Config config); 32 | 33 | /** 34 | * @brief Set the desired setpoint. 35 | * 36 | * @param setpoint Desired state. 37 | */ 38 | void set_setpoint(float setpoint); 39 | 40 | /** 41 | * @brief Reset prev_error and error_acc objects. 42 | */ 43 | void reset(); 44 | 45 | /** 46 | * @brief Update PID with new state and return response. 47 | * 48 | * @param state Current value of the controlled variable. 49 | * @param elapsed_time Time since the last update. 50 | * @param save Whether to save the variables for calibration. 51 | * @return Response of the controller. 52 | */ 53 | float compute_response(float state, float elapsed_time, bool save = false); 54 | 55 | /** 56 | * @brief Update PID with new state and return response. 57 | * 58 | * @param state Current value of the controlled variable. 59 | * @param state_change Derivative of the controlled variable. 60 | * @param elapsed_time Time since the last update. 61 | * @param save Whether to save the variables for calibration. 62 | * @return Response of the controller. 63 | */ 64 | float compute_response(float state, float elapsed_time, float state_change, bool save = false); 65 | 66 | private: 67 | /** 68 | * @brief Proportional constant. 69 | */ 70 | float kp; 71 | 72 | /** 73 | * @brief Integrative constant. 74 | */ 75 | float ki; 76 | 77 | /** 78 | * @brief Derivative constant. 79 | */ 80 | float kd; 81 | 82 | /** 83 | * @brief Desired state. 84 | */ 85 | float setpoint; 86 | 87 | /** 88 | * @brief Maximum response returned by the controller. 89 | */ 90 | float saturation; 91 | 92 | /** 93 | * @brief Maximum integrative response. 94 | */ 95 | float max_integral; 96 | 97 | /** 98 | * @brief Accumulated error for integrative term. 99 | */ 100 | float error_acc = 0; 101 | 102 | /** 103 | * @brief Previous error for derivative term. 104 | */ 105 | float prev_state = 0; 106 | 107 | /** 108 | * @brief Last response returned by the controller. 109 | */ 110 | float last_response = 0; 111 | }; 112 | } // namespace micras::core 113 | 114 | #endif // MICRAS_CORE_PID_CONTROLLER_HPP 115 | -------------------------------------------------------------------------------- /micras_nav/include/micras/nav/action_queuer.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_NAV_ACTION_QUEUER_HPP 6 | #define MICRAS_NAV_ACTION_QUEUER_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "micras/nav/actions/move.hpp" 13 | #include "micras/nav/actions/turn.hpp" 14 | 15 | namespace micras::nav { 16 | /** 17 | * @brief Class to queue actions for the robot. 18 | */ 19 | class ActionQueuer { 20 | public: 21 | /** 22 | * @brief Enum for the exploration action types. 23 | */ 24 | enum ActionType : uint8_t { 25 | STOP = 0, 26 | START = 1, 27 | MOVE_FORWARD = 2, 28 | MOVE_HALF = 3, 29 | TURN_LEFT = 4, 30 | TURN_RIGHT = 5, 31 | TURN_BACK = 6, 32 | }; 33 | 34 | /** 35 | * @brief Configuration struct for the ActionQueuer class. 36 | */ 37 | struct Config { 38 | struct Dynamic { 39 | float max_linear_speed; 40 | float max_linear_acceleration; 41 | float max_linear_deceleration; 42 | float curve_radius; 43 | float max_centrifugal_acceleration; 44 | float max_angular_acceleration; 45 | }; 46 | 47 | float cell_size; 48 | float start_offset; 49 | Dynamic exploring; 50 | Dynamic solving; 51 | }; 52 | 53 | /** 54 | * @brief Construct a new ActionQueuer object. 55 | * 56 | * @param config Configuration for the ActionQueuer. 57 | */ 58 | explicit ActionQueuer(Config config); 59 | 60 | /** 61 | * @brief Push an action to the queue. 62 | * 63 | * @param current_pose Current pose of the robot. 64 | * @param target_position Target position to move to. 65 | */ 66 | void push(const GridPose& current_pose, const GridPoint& target_position); 67 | 68 | /** 69 | * @brief Pop an action from the queue. 70 | * 71 | * @return Shared pointer to the action. 72 | */ 73 | std::shared_ptr pop(); 74 | 75 | /** 76 | * @brief Check if the action queue is empty. 77 | * 78 | * @return True if the action queue is empty, false otherwise. 79 | */ 80 | bool empty() const; 81 | 82 | /** 83 | * @brief Fill the action queue with a sequence of actions to the end. 84 | */ 85 | void recompute(const std::list& best_route); 86 | 87 | private: 88 | /** 89 | * @brief Size of the cells in the grid. 90 | */ 91 | float cell_size; 92 | 93 | /** 94 | * @brief Dynamic exploring parameters. 95 | */ 96 | Config::Dynamic exploring_params; 97 | 98 | /** 99 | * @brief Dynamic solving parameters. 100 | */ 101 | // Config::Dynamic solving_params; 102 | 103 | /** 104 | * @brief Pre-built actions to use in the exploration. 105 | */ 106 | ///@{ 107 | std::shared_ptr stop; 108 | std::shared_ptr start; 109 | std::shared_ptr move_forward; 110 | std::shared_ptr move_half; 111 | std::shared_ptr turn_left; 112 | std::shared_ptr turn_right; 113 | std::shared_ptr turn_back; 114 | ///@} 115 | 116 | /** 117 | * @brief Queue of actions to be performed. 118 | */ 119 | std::queue> action_queue; 120 | }; 121 | } // namespace micras::nav 122 | 123 | #endif // MICRAS_NAV_ACTION_QUEUER_HPP 124 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/rotary_sensor_reg.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_ROTARY_SENSOR_REG_HPP 6 | #define MICRAS_PROXY_ROTARY_SENSOR_REG_HPP 7 | 8 | #include 9 | 10 | /** 11 | * @brief Registers struct for the rotary sensor configuration. 12 | */ 13 | struct Registers { 14 | /** 15 | * @brief Registers union types definition. 16 | */ 17 | ///@{ 18 | union Disable { 19 | struct __attribute__((__packed__)) Fields { 20 | uint8_t UVW_off : 1; 21 | uint8_t ABI_off : 1; 22 | uint8_t na : 4; 23 | uint8_t FILTER_disable : 1; 24 | }; 25 | 26 | Fields fields; 27 | uint8_t raw; 28 | }; 29 | 30 | union Zposm { 31 | struct __attribute__((__packed__)) Fields { 32 | uint8_t ZPOSM : 8; 33 | }; 34 | 35 | Fields fields; 36 | uint8_t raw; 37 | }; 38 | 39 | union Zposl { 40 | struct __attribute__((__packed__)) Fields { 41 | uint8_t ZPOSL : 6; 42 | uint8_t Dia1_en : 1; 43 | uint8_t Dia2_en : 1; 44 | }; 45 | 46 | Fields Fields; 47 | uint8_t raw; 48 | }; 49 | 50 | union Settings1 { 51 | struct __attribute__((__packed__)) Fields { 52 | uint8_t K_max : 3; 53 | uint8_t K_min : 3; 54 | uint8_t Dia3_en : 1; 55 | uint8_t Dia4_en : 1; 56 | }; 57 | 58 | Fields fields; 59 | uint8_t raw; 60 | }; 61 | 62 | union Settings2 { 63 | struct __attribute__((__packed__)) Fields { 64 | uint8_t IWIDTH : 1; 65 | uint8_t NOISESET : 1; 66 | uint8_t DIR : 1; 67 | uint8_t UVW_ABI : 1; 68 | uint8_t DAECDIS : 1; 69 | uint8_t ABI_DEC : 1; 70 | uint8_t Data_select : 1; 71 | uint8_t PWMon : 1; 72 | }; 73 | 74 | Fields fields; 75 | uint8_t raw; 76 | }; 77 | 78 | union Settings3 { 79 | struct __attribute__((__packed__)) Fields { 80 | uint8_t UVWPP : 3; 81 | uint8_t HYS : 2; 82 | uint8_t ABIRES : 3; 83 | }; 84 | 85 | Fields fields; 86 | uint8_t raw; 87 | }; 88 | 89 | union Ecc { 90 | struct __attribute__((__packed__)) Fields { 91 | uint8_t ECC_chsum : 7; 92 | uint8_t ECC_en : 1; 93 | }; 94 | 95 | Fields fields; 96 | uint8_t raw; 97 | }; 98 | 99 | ///@} 100 | 101 | /** 102 | * @brief Register addresses in the rotary sensor memory. 103 | */ 104 | ///@{ 105 | static constexpr uint16_t disable_addr{0x0015}; 106 | static constexpr uint16_t zposm_addr{0x0016}; 107 | static constexpr uint16_t zposl_addr{0x0017}; 108 | static constexpr uint16_t settings1_addr{0x0018}; 109 | static constexpr uint16_t settings2_addr{0x0019}; 110 | static constexpr uint16_t settings3_addr{0x001A}; 111 | static constexpr uint16_t ecc_addr{0x001B}; 112 | ///@} 113 | 114 | /** 115 | * @brief Member variables to be configured in the rotary sensor. 116 | */ 117 | ///@{ 118 | Disable disable; 119 | Zposm zposm; 120 | Zposl zposl; 121 | Settings1 settings1; 122 | Settings2 settings2; 123 | Settings3 settings3; 124 | Ecc ecc; 125 | ///@} 126 | }; 127 | 128 | #endif // MICRAS_PROXY_ROTARY_SENSOR_REG_HPP 129 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/torque_sensors.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_TORQUE_SENSORS_HPP 6 | #define MICRAS_PROXY_TORQUE_SENSORS_HPP 7 | 8 | #include 9 | #include 10 | 11 | #include "micras/core/butterworth_filter.hpp" 12 | #include "micras/hal/adc_dma.hpp" 13 | 14 | namespace micras::proxy { 15 | /** 16 | * @brief Class for acquiring torque sensors data. 17 | */ 18 | template 19 | class TTorqueSensors { 20 | public: 21 | /** 22 | * @brief Configuration struct for torque sensors. 23 | */ 24 | struct Config { 25 | hal::AdcDma::Config adc; 26 | float shunt_resistor; 27 | float max_torque; 28 | float filter_cutoff; 29 | }; 30 | 31 | /** 32 | * @brief Construct a new TorqueSensors object. 33 | * 34 | * @param config Configuration for the torque sensors. 35 | */ 36 | explicit TTorqueSensors(const Config& config); 37 | 38 | /** 39 | * @brief Calibrate the torque sensors. 40 | */ 41 | void calibrate(); 42 | 43 | /** 44 | * @brief Update the torque sensors readings. 45 | */ 46 | void update(); 47 | 48 | /** 49 | * @brief Get the torque from the sensor. 50 | * 51 | * @param sensor_index Index of the sensor. 52 | * @return Torque reading from the sensor in N*m. 53 | */ 54 | float get_torque(uint8_t sensor_index) const; 55 | 56 | /** 57 | * @brief Get the raw torque from the sensor without filtering. 58 | * 59 | * @param sensor_index Index of the sensor. 60 | * @return Raw torque reading from the sensor. 61 | */ 62 | float get_torque_raw(uint8_t sensor_index) const; 63 | 64 | /** 65 | * @brief Get the electric current through the sensor. 66 | * 67 | * @param sensor_index Index of the sensor. 68 | * @return Current reading from the sensor in amps. 69 | */ 70 | float get_current(uint8_t sensor_index) const; 71 | 72 | /** 73 | * @brief Get the raw electric current through the sensor without filtering. 74 | * 75 | * @param sensor_index Index of the sensor. 76 | * @return Raw current reading from the sensor in amps. 77 | */ 78 | float get_current_raw(uint8_t sensor_index) const; 79 | 80 | /** 81 | * @brief Get the ADC reading from the sensor. 82 | * 83 | * @param sensor_index Index of the sensor. 84 | * @return Adc reading from the sensor from 0 to 1. 85 | */ 86 | float get_adc_reading(uint8_t sensor_index) const; 87 | 88 | private: 89 | /** 90 | * @brief ADC DMA handle. 91 | */ 92 | hal::AdcDma adc; 93 | 94 | /** 95 | * @brief Buffer to store the ADC values. 96 | */ 97 | std::array buffer; 98 | 99 | /** 100 | * @brief Reading of each sensor when no current is flowing. 101 | */ 102 | std::array base_reading{}; 103 | 104 | /** 105 | * @brief Value of the maximum current that can be measured by the sensor. 106 | */ 107 | float max_current; 108 | 109 | /** 110 | * @brief Maximum torque that can be measured by the sensor. 111 | */ 112 | float max_torque; 113 | 114 | /** 115 | * @brief Butterworth filters for the torque reading. 116 | */ 117 | std::array filters; 118 | }; 119 | } // namespace micras::proxy 120 | 121 | #include "../src/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) 122 | 123 | #endif // MICRAS_PROXY_TORQUE_SENSORS_HPP 124 | -------------------------------------------------------------------------------- /micras_nav/src/action_queuer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include 6 | 7 | #include "micras/nav/action_queuer.hpp" 8 | 9 | namespace micras::nav { 10 | ActionQueuer::ActionQueuer(Config config) : 11 | cell_size{config.cell_size}, 12 | exploring_params{config.exploring}, 13 | // solving_params{config.solving}, 14 | stop{std::make_shared( 15 | ActionType::STOP, cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, 16 | exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false 17 | )}, 18 | start{std::make_shared( 19 | ActionType::START, cell_size - config.start_offset, 0.001F * exploring_params.max_linear_acceleration, 20 | exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, 21 | exploring_params.max_linear_deceleration 22 | )}, 23 | move_forward{std::make_shared( 24 | ActionType::MOVE_FORWARD, cell_size, exploring_params.max_linear_speed, exploring_params.max_linear_speed, 25 | exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, 26 | exploring_params.max_linear_deceleration 27 | )}, 28 | move_half{std::make_shared( 29 | ActionType::MOVE_HALF, cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, 30 | exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, 31 | exploring_params.max_linear_deceleration, false 32 | )}, 33 | turn_left{std::make_shared( 34 | ActionType::TURN_LEFT, std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, 35 | exploring_params.max_angular_acceleration 36 | )}, 37 | turn_right{std::make_shared( 38 | ActionType::TURN_RIGHT, -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, 39 | exploring_params.max_angular_acceleration 40 | )}, 41 | turn_back{std::make_shared( 42 | ActionType::TURN_BACK, std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration 43 | )} { } 44 | 45 | void ActionQueuer::push(const GridPose& current_pose, const GridPoint& target_position) { 46 | if (current_pose.front().position == target_position) { 47 | this->action_queue.emplace(move_forward); 48 | return; 49 | } 50 | 51 | if (current_pose.turned_left().front().position == target_position) { 52 | this->action_queue.emplace(turn_left); 53 | return; 54 | } 55 | 56 | if (current_pose.turned_right().front().position == target_position) { 57 | this->action_queue.emplace(turn_right); 58 | return; 59 | } 60 | 61 | if (current_pose.turned_back().front().position == target_position) { 62 | this->action_queue.emplace(stop); 63 | this->action_queue.emplace(turn_back); 64 | this->action_queue.emplace(move_half); 65 | return; 66 | } 67 | } 68 | 69 | std::shared_ptr ActionQueuer::pop() { 70 | const auto& action = this->action_queue.front(); 71 | this->action_queue.pop(); 72 | return action; 73 | } 74 | 75 | bool ActionQueuer::empty() const { 76 | return this->action_queue.empty(); 77 | } 78 | 79 | void ActionQueuer::recompute(const std::list& best_route) { 80 | this->action_queue = {}; 81 | this->action_queue.emplace(start); 82 | 83 | if (best_route.empty()) { 84 | return; 85 | } 86 | 87 | for (auto it = std::next(best_route.begin()); std::next(it) != best_route.end(); it++) { 88 | this->push(*it, std::next(it)->position); 89 | } 90 | } 91 | } // namespace micras::nav 92 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/button.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_BUTTON_HPP 6 | #define MICRAS_PROXY_BUTTON_HPP 7 | 8 | #include 9 | 10 | #include "micras/hal/gpio.hpp" 11 | #include "micras/proxy/stopwatch.hpp" 12 | 13 | namespace micras::proxy { 14 | /** 15 | * @brief Class for acquiring button data. 16 | */ 17 | class Button { 18 | public: 19 | /** 20 | * @brief Enum for button status. 21 | */ 22 | enum Status : uint8_t { 23 | NO_PRESS = 0, 24 | SHORT_PRESS = 1, 25 | LONG_PRESS = 2, 26 | EXTRA_LONG_PRESS = 3 27 | }; 28 | 29 | /** 30 | * @brief Enum for button pull resistor. 31 | */ 32 | enum PullResistor : uint8_t { 33 | PULL_UP = 0, 34 | PULL_DOWN = 1, 35 | }; 36 | 37 | /** 38 | * @brief Configuration struct for the button. 39 | */ 40 | struct Config { 41 | hal::Gpio::Config gpio{}; 42 | PullResistor pull_resistor{}; 43 | uint16_t debounce_delay{10}; 44 | uint16_t long_press_delay{500}; 45 | uint16_t extra_long_press_delay{2000}; 46 | }; 47 | 48 | /** 49 | * @brief Construct a new Button object. 50 | * 51 | * @param config Button configuration. 52 | */ 53 | explicit Button(const Config& config); 54 | 55 | /** 56 | * @brief Check if button is pressed. 57 | * 58 | * @return True if button is pressed, false otherwise. 59 | */ 60 | bool is_pressed() const; 61 | 62 | /** 63 | * @brief Get button status. 64 | * 65 | * @return Current button status. 66 | */ 67 | Status get_status() const; 68 | 69 | /** 70 | * @brief Update the status of the button. 71 | */ 72 | void update(); 73 | 74 | private: 75 | /** 76 | * @brief Get raw button reading. 77 | * 78 | * @return Button reading without debounce. 79 | */ 80 | bool get_raw_reading() const; 81 | 82 | /** 83 | * @brief Update button state. 84 | */ 85 | void update_state(); 86 | 87 | /** 88 | * @brief Check if button was just pressed. 89 | * 90 | * @return True if button was just pressed, false otherwise. 91 | */ 92 | bool is_rising_edge() const; 93 | 94 | /** 95 | * @brief Check if button was just released. 96 | * 97 | * @return True if button was just released, false otherwise. 98 | */ 99 | bool is_falling_edge() const; 100 | 101 | /** 102 | * @brief Button pressing delays in ms. 103 | */ 104 | ///@{ 105 | uint16_t debounce_delay; 106 | uint16_t long_press_delay; 107 | uint16_t extra_long_press_delay; 108 | ///@} 109 | 110 | /** 111 | * @brief Gpio object for button. 112 | */ 113 | hal::Gpio gpio; 114 | 115 | /** 116 | * @brief Pull resistor configuration. 117 | */ 118 | PullResistor pull_resistor; 119 | 120 | /** 121 | * @brief Stopwatch to check if button is debouncing. 122 | */ 123 | proxy::Stopwatch debounce_stopwatch; 124 | 125 | /** 126 | * @brief Stopwatch to determine type of button press. 127 | */ 128 | proxy::Stopwatch status_stopwatch; 129 | 130 | /** 131 | * @brief Flag to know when button is debouncing. 132 | */ 133 | bool is_debouncing{false}; 134 | 135 | /** 136 | * @brief Flag to know if button was being pressed. 137 | */ 138 | bool previous_state{false}; 139 | 140 | /** 141 | * @brief Flag to know if button is being pressed. 142 | */ 143 | bool current_state{false}; 144 | 145 | /** 146 | * @brief Current status of the button. 147 | */ 148 | Status current_status{NO_PRESS}; 149 | }; 150 | } // namespace micras::proxy 151 | 152 | #endif // MICRAS_PROXY_BUTTON_HPP 153 | -------------------------------------------------------------------------------- /micras_proxy/include/micras/proxy/wall_sensors.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_PROXY_WALL_SENSORS_HPP 6 | #define MICRAS_PROXY_WALL_SENSORS_HPP 7 | 8 | #include 9 | #include 10 | 11 | #include "micras/core/butterworth_filter.hpp" 12 | #include "micras/hal/adc_dma.hpp" 13 | #include "micras/hal/pwm.hpp" 14 | 15 | namespace micras::proxy { 16 | /** 17 | * @brief Class for controlling Wall Sensors. 18 | */ 19 | template 20 | class TWallSensors { 21 | public: 22 | /** 23 | * @brief Configuration struct for wall sensors. 24 | */ 25 | struct Config { 26 | hal::AdcDma::Config adc; 27 | hal::Pwm::Config led_0_pwm; 28 | hal::Pwm::Config led_1_pwm; 29 | float filter_cutoff; 30 | std::array base_readings; 31 | float uncertainty; 32 | }; 33 | 34 | /** 35 | * @brief Construct a new WallSensors object. 36 | * 37 | * @param config Configuration for the wall sensors. 38 | */ 39 | explicit TWallSensors(const Config& config); 40 | 41 | /** 42 | * @brief Turn on the wall sensors IR LED. 43 | */ 44 | void turn_on(); 45 | 46 | /** 47 | * @brief Turn off the wall sensors IR LED. 48 | */ 49 | void turn_off(); 50 | 51 | /** 52 | * @brief Update the wall sensors readings. 53 | */ 54 | void update(); 55 | 56 | /** 57 | * @brief Get the observation from a sensor. 58 | * 59 | * @param sensor_index Index of the sensor. 60 | * @param disturbed Whether or not there is another wall perpendicular to the one being measured. 61 | * @return True if the sensor detects a wall, false otherwise. 62 | */ 63 | bool get_wall(uint8_t sensor_index, bool disturbed = false) const; 64 | 65 | /** 66 | * @brief Get the reading from a sensor. 67 | * 68 | * @param sensor_index Index of the sensor. 69 | * @return Reading from the sensor. 70 | */ 71 | float get_reading(uint8_t sensor_index) const; 72 | 73 | /** 74 | * @brief Get the ADC reading from a sensor. 75 | * 76 | * @param sensor_index Index of the sensor. 77 | * @return ADC reading from the sensor from 0 to 1. 78 | */ 79 | float get_adc_reading(uint8_t sensor_index) const; 80 | 81 | /** 82 | * @brief Get the deviation of a wall sensor reading from its calibrated baseline. 83 | * 84 | * @param sensor_index Index of the sensor. 85 | * @return The reading error relative to the baseline; positive if above baseline. 86 | */ 87 | float get_sensor_error(uint8_t sensor_index) const; 88 | 89 | /** 90 | * @brief Calibrate a wall sensor base reading. 91 | */ 92 | void calibrate_sensor(uint8_t sensor_index); 93 | 94 | private: 95 | /** 96 | * @brief ADC DMA handle. 97 | */ 98 | hal::AdcDma adc; 99 | 100 | /** 101 | * @brief PWM handle for the even infrared LEDs. 102 | */ 103 | hal::Pwm led_0_pwm; 104 | 105 | /** 106 | * @brief PWM handle for the odd infrared LEDs. 107 | */ 108 | hal::Pwm led_1_pwm; 109 | 110 | /** 111 | * @brief Buffer to store the ADC values. 112 | */ 113 | std::array buffer; 114 | 115 | /** 116 | * @brief Butterworth filter for the ADC readings. 117 | */ 118 | std::array filters; 119 | 120 | /** 121 | * @brief Measured wall values during calibration. 122 | */ 123 | std::array base_readings; 124 | 125 | /** 126 | * @brief Ratio of the base reading to still consider as seeing a wall. 127 | */ 128 | float uncertainty; 129 | }; 130 | } // namespace micras::proxy 131 | 132 | #include "../src/wall_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) 133 | 134 | #endif // MICRAS_PROXY_WALL_SENSORS_HPP 135 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | 3 | ############################################################################### 4 | ## CMake Configuration 5 | ############################################################################### 6 | 7 | set(CMAKE_C_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD 20) 9 | 10 | set(CMAKE_C_STANDARD_REQUIRED ON) 11 | set(CMAKE_C_EXTENSIONS ON) 12 | 13 | set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE) 14 | 15 | ############################################################################### 16 | ## Project Configuration 17 | ############################################################################### 18 | 19 | # The .ioc file used to generate the project will be PROJECT_RELEASE.ioc 20 | 21 | set(CMAKE_PROJECT_NAME micras) 22 | set(BOARD_VERSION "") 23 | 24 | if(BOARD_VERSION STREQUAL "") 25 | set(PROJECT_RELEASE ${CMAKE_PROJECT_NAME}) 26 | else() 27 | set(PROJECT_RELEASE "${CMAKE_PROJECT_NAME}_${BOARD_VERSION}") 28 | endif() 29 | 30 | ############################################################################### 31 | ## Toolchain Configuration 32 | ############################################################################### 33 | 34 | set(LAUNCH_JSON_PATH "${CMAKE_CURRENT_SOURCE_DIR}/.vscode/launch.json") 35 | set(DEBUG_FILE_NAME ${CMAKE_PROJECT_NAME}) 36 | 37 | include(cmake/config_validation.cmake) 38 | 39 | set(CMAKE_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/cube") 40 | include(cube/cmake/gcc-arm-none-eabi.cmake) 41 | set(CMAKE_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}) 42 | 43 | project(${CMAKE_PROJECT_NAME} C CXX ASM) 44 | 45 | include(cmake/utilities.cmake) 46 | include(cmake/targets.cmake) 47 | 48 | add_subdirectory(cube/cmake/stm32cubemx) 49 | add_subdirectory(micras_core) 50 | add_subdirectory(micras_hal) 51 | add_subdirectory(micras_proxy) 52 | add_subdirectory(micras_nav) 53 | 54 | ############################################################################### 55 | ## Input files 56 | ############################################################################### 57 | 58 | file(GLOB_RECURSE FORMAT_SOURCES CONFIGURE_DEPENDS "src/*.c*" "tests/*.c*" "micras_*/src/*.c*") 59 | file(GLOB_RECURSE FORMAT_HEADERS CONFIGURE_DEPENDS "include/*.h*" "config/*.h*" "tests/*.h*" "micras_*/include/*.h*") 60 | 61 | generate_format_target(FORMAT_SOURCES FORMAT_HEADERS) 62 | generate_lint_target(FORMAT_SOURCES) 63 | 64 | file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") 65 | file(GLOB_RECURSE PROJECT_TESTS CONFIGURE_DEPENDS "tests/src/*/*.c*") 66 | 67 | ############################################################################### 68 | ## Main executable target 69 | ############################################################################### 70 | 71 | add_executable(${PROJECT_NAME} 72 | ${PROJECT_SOURCES} 73 | ) 74 | 75 | target_include_directories(${PROJECT_NAME} PUBLIC 76 | include 77 | config 78 | ) 79 | 80 | target_link_libraries(${PROJECT_NAME} PUBLIC 81 | micras::nav 82 | ) 83 | 84 | generate_hex_file(${PROJECT_NAME}) 85 | print_size_of_target(${PROJECT_NAME}) 86 | 87 | generate_helpme_text() 88 | generate_vscode_tasks_json() 89 | generate_debug_target(${PROJECT_NAME}) 90 | generate_flash_target(${PROJECT_NAME}) 91 | 92 | ############################################################################### 93 | ## Generate test executables 94 | ############################################################################### 95 | 96 | foreach(TEST_FILE ${PROJECT_TESTS}) 97 | get_filename_component(TEST_NAME ${TEST_FILE} NAME_WLE) 98 | 99 | add_executable(${TEST_NAME} EXCLUDE_FROM_ALL 100 | ${TEST_FILE} 101 | ) 102 | 103 | target_include_directories(${TEST_NAME} PUBLIC 104 | tests/include 105 | config 106 | ) 107 | 108 | target_link_libraries(${TEST_NAME} PUBLIC 109 | micras::nav 110 | ) 111 | 112 | generate_hex_file(${TEST_NAME}) 113 | print_size_of_target(${TEST_NAME}) 114 | 115 | generate_debug_target(${TEST_NAME}) 116 | generate_flash_target(${TEST_NAME}) 117 | endforeach() 118 | 119 | generate_test_all_target(${PROJECT_TESTS}) 120 | -------------------------------------------------------------------------------- /micras_nav/include/micras/nav/speed_controller.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_NAV_SPEED_CONTROLLER_HPP 6 | #define MICRAS_NAV_SPEED_CONTROLLER_HPP 7 | 8 | #include 9 | 10 | #include "micras/core/pid_controller.hpp" 11 | #include "micras/nav/state.hpp" 12 | 13 | namespace micras::nav { 14 | /** 15 | * @brief Class to compute a command to follow a pair of linear and angular speeds. 16 | */ 17 | class SpeedController { 18 | public: 19 | /** 20 | * @brief Configuration struct for the SpeedController class. 21 | */ 22 | struct Config { 23 | struct FeedForward { 24 | float linear_speed; 25 | float linear_acceleration; 26 | float angular_speed; 27 | float angular_acceleration; 28 | }; 29 | 30 | float max_linear_acceleration{}; 31 | float max_angular_acceleration{}; 32 | core::PidController::Config linear_pid; 33 | core::PidController::Config angular_pid; 34 | FeedForward left_feed_forward{}; 35 | FeedForward right_feed_forward{}; 36 | }; 37 | 38 | /** 39 | * @brief Construct a new SpeedController object. 40 | * 41 | * @param config The configuration for the SpeedController class. 42 | */ 43 | explicit SpeedController(const Config& config); 44 | 45 | /** 46 | * @brief Calculate the command to achieve the desired speeds. 47 | * 48 | * @param current_twist The current speeds of the robot. 49 | * @param desired_twist The desired speeds of the robot. 50 | * @param elapsed_time The time since the last update. 51 | * @return A pair of floats representing the left and right commands. 52 | */ 53 | std::pair 54 | compute_control_commands(const Twist& current_twist, const Twist& desired_twist, float elapsed_time); 55 | 56 | /** 57 | * @brief Calculate the feed-forward command to achieve the desired speeds and accelerations. 58 | * 59 | * @param desired_twist The desired speeds of the robot. 60 | * @param elapsed_time The time since the last update. 61 | * @return A pair of floats representing the left and right feed-forward commands. 62 | */ 63 | std::pair compute_feed_forward_commands(const Twist& desired_twist, float elapsed_time); 64 | 65 | /** 66 | * @brief Reset the PID controllers. 67 | */ 68 | void reset(); 69 | 70 | private: 71 | /** 72 | * @brief Calculate the feed-forward term for a motor. 73 | * 74 | * @param speed The current speeds of the robot. 75 | * @param acceleration The current accelerations of the robot. 76 | * @param config The configuration for the feed-forward term. 77 | * @return The feed-forward parameters for a motor. 78 | */ 79 | static float feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config); 80 | 81 | /** 82 | * @brief The maximum linear acceleration of the robot. 83 | */ 84 | float max_linear_acceleration; 85 | 86 | /** 87 | * @brief The maximum angular acceleration of the robot. 88 | */ 89 | float max_angular_acceleration; 90 | 91 | /** 92 | * @brief The last linear speed of the robot. 93 | */ 94 | float last_linear_speed{}; 95 | 96 | /** 97 | * @brief The last angular speed of the robot. 98 | */ 99 | float last_angular_speed{}; 100 | 101 | /** 102 | * @brief PID controller for stopping at the goal. 103 | */ 104 | core::PidController linear_pid; 105 | 106 | /** 107 | * @brief PID controller for the orientation. 108 | */ 109 | core::PidController angular_pid; 110 | 111 | /** 112 | * @brief Feed-forward parameters for the left motor. 113 | */ 114 | Config::FeedForward left_feed_forward; 115 | 116 | /** 117 | * @brief Feed-forward parameters for the right motor. 118 | */ 119 | Config::FeedForward right_feed_forward; 120 | }; 121 | } // namespace micras::nav 122 | 123 | #endif // MICRAS_NAV_SPEED_CONTROLLER_HPP 124 | -------------------------------------------------------------------------------- /micras_nav/include/micras/nav/grid_pose.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_NAV_GRID_POSE_HPP 6 | #define MICRAS_NAV_GRID_POSE_HPP 7 | 8 | #include 9 | #include 10 | 11 | #include "micras/core/vector.hpp" 12 | 13 | namespace micras::nav { 14 | /** 15 | * @brief Possible sides in the grid. 16 | */ 17 | enum Side : uint8_t { 18 | RIGHT = 0, 19 | UP = 1, 20 | LEFT = 2, 21 | DOWN = 3 22 | }; 23 | 24 | /** 25 | * @brief Convert an angle in radians to a Grid side. 26 | * 27 | * @param angle The angle in radians. 28 | * @return The corresponding Grid side. 29 | */ 30 | Side angle_to_grid(float angle); 31 | 32 | /** 33 | * @brief Type to store a point in the grid. 34 | */ 35 | struct GridPoint { 36 | /** 37 | * @brief Return the direction from this point to the next one. 38 | * 39 | * @param next The next point. 40 | * @return The direction from this point to the next one. 41 | */ 42 | Side direction(const GridPoint& next) const; 43 | 44 | /** 45 | * @brief Convert the point to a grid point. 46 | * 47 | * @param cell_size The size of the grid cells. 48 | * @return The grid point corresponding to the point. 49 | */ 50 | static GridPoint from_vector(const core::Vector& point, float cell_size); 51 | 52 | /** 53 | * @brief Convert a grid point to a point. 54 | * 55 | * @param grid_point The grid point to convert. 56 | * @param cell_size The size of the grid cells. 57 | * @return The point corresponding to the grid point. 58 | */ 59 | core::Vector to_vector(float cell_size) const; 60 | 61 | /** 62 | * @brief Move in the grid in the direction of the side. 63 | * 64 | * @param side The side to move to. 65 | * @return The new point after moving. 66 | */ 67 | GridPoint operator+(const Side& side) const; 68 | 69 | /** 70 | * @brief Compare two points for equality. 71 | * 72 | * @param other The other point to compare. 73 | * @return True if the points are equal, false otherwise. 74 | */ 75 | bool operator==(const GridPoint& other) const; 76 | 77 | /** 78 | * @brief The x coordinate of the point on the grid. 79 | */ 80 | uint8_t x; 81 | 82 | /** 83 | * @brief The y coordinate of the point on the grid. 84 | */ 85 | uint8_t y; 86 | }; 87 | 88 | struct GridPose { 89 | /** 90 | * @brief Return the pose after moving forward. 91 | * 92 | * @return The pose after moving forward. 93 | */ 94 | GridPose front() const; 95 | 96 | /** 97 | * @brief Return the pose after turning back. 98 | * 99 | * @return The pose after turning back. 100 | */ 101 | GridPose turned_back() const; 102 | 103 | /** 104 | * @brief Return the pose after turning left. 105 | * 106 | * @return The pose after turning left. 107 | */ 108 | GridPose turned_left() const; 109 | 110 | /** 111 | * @brief Return the pose after turning right. 112 | * 113 | * @return The pose after turning right. 114 | */ 115 | GridPose turned_right() const; 116 | 117 | /** 118 | * @brief Compare two poses for equality. 119 | * 120 | * @param other The other pose to compare. 121 | * @return True if the poses are equal, false otherwise. 122 | */ 123 | bool operator==(const GridPose& other) const; 124 | 125 | /** 126 | * @brief The position of the pose on the grid. 127 | */ 128 | GridPoint position; 129 | 130 | /** 131 | * @brief The orientation of the pose on the grid. 132 | */ 133 | Side orientation; 134 | }; 135 | } // namespace micras::nav 136 | 137 | namespace std { 138 | /** 139 | * @brief Hash specialization for the GridPoint type. 140 | * 141 | * @tparam T GridPoint type. 142 | */ 143 | template <> 144 | struct hash { 145 | size_t operator()(const micras::nav::GridPoint& point) const noexcept { 146 | const size_t h1 = hash{}(point.x); 147 | const size_t h2 = hash{}(point.y); 148 | return h1 ^ (h2 << 1); 149 | } 150 | }; 151 | } // namespace std 152 | 153 | #endif // MICRAS_NAV_GRID_POSE_HPP 154 | -------------------------------------------------------------------------------- /micras_nav/src/follow_wall.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #include "micras/nav/follow_wall.hpp" 6 | 7 | namespace micras::nav { 8 | FollowWall::FollowWall( 9 | const std::shared_ptr>& wall_sensors, const Pose& absolute_pose, const Config& config 10 | ) : 11 | wall_sensors{wall_sensors}, 12 | pid{config.pid}, 13 | sensor_index{config.wall_sensor_index}, 14 | max_linear_speed{config.max_linear_speed}, 15 | post_threshold{config.post_threshold}, 16 | blind_pose{absolute_pose}, 17 | cell_size{config.cell_size}, 18 | post_clearance{config.post_clearance} { } 19 | 20 | float FollowWall::compute_angular_correction(float elapsed_time, float linear_speed) { 21 | if (this->wall_sensors.use_count() == 1) { 22 | this->wall_sensors->update(); 23 | } 24 | 25 | if (this->check_posts()) { 26 | return 0.0F; 27 | } 28 | 29 | if ((not this->following_left or not this->following_right) and 30 | (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_clearance : 0.0F)))) { 31 | this->reset(); 32 | } 33 | 34 | float error{}; 35 | 36 | if (this->following_left and this->following_right) { 37 | error = this->wall_sensors->get_sensor_error(this->sensor_index.left) - 38 | this->wall_sensors->get_sensor_error(this->sensor_index.right); 39 | } else if (this->following_left) { 40 | error = 2.0F * this->wall_sensors->get_sensor_error(this->sensor_index.left); 41 | } else if (this->following_right) { 42 | error = -2.0F * this->wall_sensors->get_sensor_error(this->sensor_index.right); 43 | } else { 44 | return 0.0F; 45 | } 46 | 47 | const float response = this->pid.compute_response(error, elapsed_time); 48 | 49 | return response * linear_speed / this->max_linear_speed; 50 | } 51 | 52 | bool FollowWall::check_posts() { 53 | const float current_distance = this->blind_pose.get().position.magnitude(); 54 | const float delta_distance = current_distance - this->last_blind_distance; 55 | 56 | if (delta_distance <= 0.0F) { 57 | return false; 58 | } 59 | 60 | this->last_blind_distance = current_distance; 61 | bool found_posts = false; 62 | 63 | if (this->following_left and 64 | -(this->wall_sensors->get_sensor_error(this->sensor_index.left) - this->last_left_error) / delta_distance >= 65 | this->post_threshold) { 66 | this->following_left = false; 67 | 68 | if (this->following_right) { 69 | this->reset_displacement(true); 70 | } 71 | 72 | found_posts = true; 73 | } 74 | 75 | if (this->following_right and 76 | -(this->wall_sensors->get_sensor_error(this->sensor_index.right) - this->last_right_error) / delta_distance >= 77 | this->post_threshold) { 78 | this->following_right = false; 79 | 80 | if (this->following_left) { 81 | this->reset_displacement(true); 82 | } 83 | 84 | found_posts = true; 85 | } 86 | 87 | this->last_left_error = this->wall_sensors->get_sensor_error(this->sensor_index.left); 88 | this->last_right_error = this->wall_sensors->get_sensor_error(this->sensor_index.right); 89 | 90 | return found_posts; 91 | } 92 | 93 | core::Observation FollowWall::get_observation() const { 94 | const bool front_wall = this->wall_sensors->get_wall(this->sensor_index.left_front) and 95 | this->wall_sensors->get_wall(this->sensor_index.right_front); 96 | const bool disturbed = front_wall; 97 | 98 | return { 99 | .left = this->wall_sensors->get_wall(this->sensor_index.left, disturbed), 100 | .front = front_wall, 101 | .right = this->wall_sensors->get_wall(this->sensor_index.right, disturbed), 102 | }; 103 | } 104 | 105 | void FollowWall::reset() { 106 | this->pid.reset(); 107 | this->reset_displacement(); 108 | this->following_left = this->wall_sensors->get_wall(this->sensor_index.left); 109 | this->following_right = this->wall_sensors->get_wall(this->sensor_index.right); 110 | } 111 | 112 | void FollowWall::reset_displacement(bool reset_by_post) { 113 | this->blind_pose.reset_reference(); 114 | this->last_blind_distance = 0.0F; 115 | this->reset_by_post = reset_by_post; 116 | } 117 | } // namespace micras::nav 118 | -------------------------------------------------------------------------------- /micras_nav/include/micras/nav/costmap.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_NAV_COSTMAP_HPP 6 | #define MICRAS_NAV_COSTMAP_HPP 7 | 8 | #include 9 | #include 10 | 11 | #include "micras/nav/grid_pose.hpp" 12 | 13 | namespace micras::nav { 14 | constexpr int16_t max_cost{0x1FFF}; 15 | 16 | /** 17 | * @brief Class for managing a layered costmap. 18 | * 19 | * @tparam width The width of the maze. 20 | * @tparam height The height of the maze. 21 | * @tparam layers The number of layers in the costmap. 22 | */ 23 | template 24 | class Costmap { 25 | public: 26 | /** 27 | * @brief Type to store the state of a wall. 28 | */ 29 | enum WallState : uint8_t { 30 | UNKNOWN = 0, 31 | NO_WALL = 1, 32 | WALL = 2, 33 | VIRTUAL = 3, 34 | }; 35 | 36 | /** 37 | * @brief Type to store the information of a cell in the maze. 38 | */ 39 | struct Cell { 40 | Cell() { costs.fill(max_cost); } 41 | 42 | std::array walls{WallState::UNKNOWN}; 43 | std::array costs; 44 | }; 45 | 46 | /** 47 | * @brief Construct a new Costmap object. 48 | */ 49 | Costmap(); 50 | 51 | /** 52 | * @brief Compute the costmap of a given layer from a reference point using the flood fill algorithm. 53 | * 54 | * @param reference The reference point to start the flood fill algorithm. 55 | * @param layer The layer to compute the costmap for. 56 | */ 57 | void compute(const GridPoint& reference, uint8_t layer); 58 | 59 | /** 60 | * @brief Reset the costs and recompute the new values locally for a given layer. 61 | * 62 | * @param reference The reference point to start resetting the stored cost. 63 | * @param layer The layer to recompute. 64 | */ 65 | void recompute(const GridPoint& reference, uint8_t layer); 66 | 67 | /** 68 | * @brief Return the cell at the given position. 69 | * 70 | * @param position The position of the cell. 71 | * @return The cell at the given position. 72 | */ 73 | const Cell& get_cell(const GridPoint& position) const; 74 | 75 | /** 76 | * @brief Get the cost of a cell at a given position and layer. 77 | * 78 | * @param position The position of the cell. 79 | * @param layer The layer to get the cost for. 80 | * @return The cost of the cell at the given position and layer. 81 | */ 82 | int16_t get_cost(const GridPoint& position, uint8_t layer) const; 83 | 84 | /** 85 | * @brief Update the cost of a cell at a given position and layer. 86 | * 87 | * @param position The position of the cell. 88 | * @param layer The layer to update the cost for. 89 | * @param cost The new cost to set. 90 | */ 91 | void update_cost(const GridPoint& position, uint8_t layer, int16_t cost); 92 | 93 | /** 94 | * @brief Check whether there is a wall at the front of a given pose. 95 | * 96 | * @param pose The pose to check.\ 97 | * @param consider_virtual Whether to consider virtual walls. 98 | * @return True if there is a wall, false otherwise. 99 | */ 100 | bool has_wall(const GridPose& pose, bool consider_virtual = false) const; 101 | 102 | /** 103 | * @brief Update the existence of a wall in the maze. 104 | * 105 | * @param pose The pose of the robot. 106 | * @param wall Whether there is a wall at the front of a given pose. 107 | */ 108 | bool update_wall(const GridPose& pose, bool wall); 109 | 110 | /** 111 | * @brief Add a virtual wall to the costmap at a given position and side. 112 | * 113 | * @param pose The pose to add the virtual wall at. 114 | */ 115 | void add_virtual_wall(const GridPose& pose); 116 | 117 | private: 118 | /** 119 | * @brief Get the cell at the given position. 120 | * 121 | * @param position The position of the cell. 122 | * @return The cell at the given position. 123 | */ 124 | Cell& cell_on_position(const GridPoint& position); 125 | 126 | /** 127 | * @brief Cells matrix representing the maze. 128 | */ 129 | std::array, height> cells{}; 130 | }; 131 | } // namespace micras::nav 132 | 133 | #include "../src/costmap.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) 134 | 135 | #endif // MICRAS_NAV_COSTMAP_HPP 136 | -------------------------------------------------------------------------------- /micras_core/include/micras/core/utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_CORE_UTILS_HPP 6 | #define MICRAS_CORE_UTILS_HPP 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace micras::core { 14 | /** 15 | * @brief Remap a value from one range to another. 16 | * 17 | * @tparam T Type of the value. 18 | * @param value Value to be remapped. 19 | * @param in_min Minimum value of the input range. 20 | * @param in_max Maximum value of the input range. 21 | * @param out_min Minimum value of the output range. 22 | * @param out_max Maximum value of the output range. 23 | * @return Remapped value. 24 | */ 25 | template 26 | constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { 27 | return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 28 | } 29 | 30 | /** 31 | * @brief Move a value towards a target limiting by the step size. 32 | * 33 | * @tparam T Type of the value. 34 | * @param value Current value. 35 | * @param target Target value. 36 | * @param step Step size. 37 | * @return New value. 38 | */ 39 | template 40 | constexpr T move_towards(T value, T target, T step) { 41 | return std::max(std::min(value + step, target), value - step); 42 | } 43 | 44 | /** 45 | * @brief Vary a value from start to end smoothly. 46 | * 47 | * @tparam T Type of the value. 48 | * @param value Current value. 49 | * @param start Start value. 50 | * @param end End value. 51 | * @param resistance Value resistance to variation. 52 | * @return New value. 53 | */ 54 | template 55 | constexpr T transition(T value, T start, T end, T resistance) { 56 | return end - (end - start) * resistance / (resistance + value * value); 57 | } 58 | 59 | /** 60 | * @brief Create an array of objects calling their constructors from an array of parameters. 61 | * 62 | * @tparam T Type of the array. 63 | * @tparam N Size of the array. 64 | * @tparam C Type of the parameters. 65 | * @param parameters List of parameters. 66 | * @return Array with the objects created from the parameters. 67 | */ 68 | template 69 | constexpr std::array make_array(const std::array& parameters) { 70 | return [&](std::index_sequence) -> std::array { 71 | return {T{parameters[I]}...}; 72 | }(std::make_index_sequence()); 73 | } 74 | 75 | /** 76 | * @brief Create an array of objects calling their constructors from a single parameter. 77 | * 78 | * @tparam T Type of the array. 79 | * @tparam N Size of the array. 80 | * @tparam C Type of the parameter. 81 | * @param value Parameter. 82 | * @return Array with the objects created from the parameter. 83 | */ 84 | template 85 | constexpr std::array make_array(C value) { 86 | return [&](std::index_sequence) -> std::array { 87 | return {(static_cast(I), T{value})...}; 88 | }(std::make_index_sequence()); 89 | } 90 | 91 | /** 92 | * @brief Assert an angle to be in the range [-pi, pi]. 93 | * 94 | * @param angle Angle to be asserted. 95 | * @return Asserted angle. 96 | */ 97 | constexpr float assert_angle(float angle) { 98 | angle = std::fmod(angle, 2 * std::numbers::pi_v); 99 | 100 | if (angle > std::numbers::pi_v) { 101 | angle -= 2 * std::numbers::pi_v; 102 | } else if (angle < -std::numbers::pi_v) { 103 | angle += 2 * std::numbers::pi_v; 104 | } 105 | 106 | return angle; 107 | } 108 | 109 | /** 110 | * @brief Assert an angle to be in the range [-pi/2, pi/2]. 111 | * 112 | * @param angle Angle to be asserted. 113 | * @return Asserted angle. 114 | */ 115 | constexpr float assert_half_angle(float angle) { 116 | angle = std::fmod(angle, std::numbers::pi_v); 117 | 118 | if (angle > std::numbers::pi_v / 2.0F) { 119 | angle -= std::numbers::pi_v; 120 | } else if (angle < -std::numbers::pi_v / 2.0F) { 121 | angle += std::numbers::pi_v; 122 | } 123 | 124 | return angle; 125 | } 126 | 127 | /** 128 | * @brief Check if two floating point numbers are near each other. 129 | * 130 | * @param x First number. 131 | * @param y Second number. 132 | * @param tolerance Tolerance. 133 | * @return True if the numbers are near each other, false otherwise. 134 | */ 135 | constexpr bool is_near(float x, float y, float tolerance = 0.001) { 136 | return std::abs(x - y) <= tolerance; 137 | } 138 | } // namespace micras::core 139 | 140 | #endif // MICRAS_CORE_UTILS_HPP 141 | -------------------------------------------------------------------------------- /micras_nav/include/micras/nav/actions/move.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | */ 4 | 5 | #ifndef MICRAS_NAV_MOVE_ACTION_HPP 6 | #define MICRAS_NAV_MOVE_ACTION_HPP 7 | 8 | #include 9 | 10 | #include "micras/nav/actions/base.hpp" 11 | 12 | namespace micras::nav { 13 | /** 14 | * @brief Action to move the robot a certain distance forward. 15 | */ 16 | class MoveAction : public Action { 17 | public: 18 | /** 19 | * @brief Construct a new Move Action object. 20 | * 21 | * @param action_id The ID of the action. 22 | * @param distance Distance to move in meters. 23 | * @param start_speed Initial speed in m/s. 24 | * @param end_speed Final speed in m/s. 25 | * @param max_speed Maximum speed in m/s. 26 | * @param max_acceleration Maximum acceleration in m/s^2. 27 | * @param max_deceleration Maximum deceleration in m/s^2. 28 | * @param follow_wall Whether the robot can follow wall while executing this action. 29 | */ 30 | MoveAction( 31 | uint8_t action_id, float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, 32 | float max_deceleration, bool follow_wall = true 33 | ) : 34 | Action{action_id}, 35 | distance(distance), 36 | start_speed_2(start_speed * start_speed), 37 | end_speed_2(end_speed * end_speed), 38 | max_speed{max_speed}, 39 | max_acceleration_doubled{2.0F * max_acceleration}, 40 | max_deceleration_doubled{2.0F * max_deceleration}, 41 | follow_wall(follow_wall), 42 | decelerate_distance{ 43 | (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / 44 | (max_acceleration_doubled + max_deceleration_doubled) 45 | } { } 46 | 47 | /** 48 | * @brief Get the desired speeds for the robot to complete the action. 49 | * 50 | * @param pose The current pose of the robot. 51 | * @return The desired speeds for the robot to complete the action. 52 | * 53 | * @details The desired velocity is calculated from the linear displacement based on the Torricelli equation. 54 | */ 55 | Twist get_speeds(const Pose& pose) const override { 56 | const float current_distance = pose.position.magnitude(); 57 | Twist twist{}; 58 | 59 | if (current_distance < this->decelerate_distance) { 60 | twist = { 61 | .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance), 62 | .angular = 0.0F, 63 | }; 64 | } else { 65 | twist = { 66 | .linear = 67 | std::sqrt(this->end_speed_2 + this->max_deceleration_doubled * (this->distance - current_distance)), 68 | .angular = 0.0F, 69 | }; 70 | } 71 | 72 | twist.linear = std::fminf(twist.linear, this->max_speed); 73 | 74 | return twist; 75 | } 76 | 77 | /** 78 | * @brief Check if the action is finished. 79 | * 80 | * @param pose The current pose of the robot. 81 | * @return True if the action is finished, false otherwise. 82 | */ 83 | bool finished(const Pose& pose) const override { return pose.position.magnitude() >= this->distance; } 84 | 85 | /** 86 | * @brief Check if the action allows the robot to follow walls. 87 | * 88 | * @return True if the action allows the robot to follow walls, false otherwise. 89 | */ 90 | bool allow_follow_wall() const override { return this->follow_wall; } 91 | 92 | private: 93 | /** 94 | * @brief Distance to move in meters. 95 | */ 96 | float distance; 97 | 98 | /** 99 | * @brief Initial speed squared. 100 | */ 101 | float start_speed_2; 102 | 103 | /** 104 | * @brief Final speed squared. 105 | */ 106 | float end_speed_2; 107 | 108 | /** 109 | * @brief Maximum linear speed in m/s. 110 | */ 111 | float max_speed; 112 | 113 | /** 114 | * @brief Maximum linear acceleration multiplied by 2. 115 | */ 116 | float max_acceleration_doubled; 117 | 118 | /** 119 | * @brief Maximum linear deceleration multiplied by 2. 120 | */ 121 | float max_deceleration_doubled; 122 | 123 | /** 124 | * @brief Whether the robot can follow walls while executing this action. 125 | */ 126 | bool follow_wall; 127 | 128 | /** 129 | * @brief Distance from the start where the robot should start to decelerate in meters. 130 | */ 131 | float decelerate_distance; 132 | }; 133 | } // namespace micras::nav 134 | 135 | #endif // MICRAS_NAV_MOVE_ACTION_HPP 136 | --------------------------------------------------------------------------------