From 3e7289929c16f70aced653c83954a3abd9ef1f5d Mon Sep 17 00:00:00 2001 From: LeonardoBizzoni Date: Sat, 28 Mar 2026 09:38:13 +0100 Subject: [PATCH] Removed FMW_ASSERT from fmw layer & forgot emergency service msg --- .../Core/Inc/firmware/fmw_core.h | 74 ++-- .../Core/Inc/firmware/fmw_messages.h | 43 +- .../Core/Src/firmware/fwm_core.c | 393 ++++++++---------- pioneer_controller/Core/Src/main.c | 48 ++- .../messages/EmergencyMode.msg | 1 + .../services/EmergencyMode.srv | 3 + 6 files changed, 256 insertions(+), 306 deletions(-) create mode 100644 pioneer_workstation_ws/src/pioneer3dx_controller/messages/EmergencyMode.msg create mode 100644 pioneer_workstation_ws/src/pioneer3dx_controller/services/EmergencyMode.srv diff --git a/pioneer_controller/Core/Inc/firmware/fmw_core.h b/pioneer_controller/Core/Inc/firmware/fmw_core.h index 00c4fcd..8573c08 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_core.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_core.h @@ -75,11 +75,6 @@ typedef struct FMW_Buzzer { uint32_t timer_channel; } FMW_Buzzer; -typedef struct FMW_Hook { - void (*callback)(void *args); - void *args; -} FMW_Hook; - typedef struct FMW_InitInfo { struct { UART_HandleTypeDef *huart; @@ -95,10 +90,12 @@ typedef struct FMW_InitInfo { } emergency; FMW_Motor *motors; + FMW_Encoder *encoders; int32_t motors_count; + int32_t encoders_count; } FMW_InitInfo; -void fmw_init(const FMW_InitInfo *info) __attribute__((nonnull)); +FMW_Result fmw_init(const FMW_InitInfo *info) __attribute__((nonnull, warn_unused_result)); void fmw_uart_message_dispatch(void); void fmw_uart_message_send(FMW_Message *msg) __attribute__((nonnull)); @@ -108,36 +105,34 @@ void fmw_emergency_begin(void); void fmw_emergency_end(void); void fmw_emergency_timer_update(void); -FMW_Mode fmw_mode_current(void); -FMW_Mode fmw_mode_transition(FMW_Mode mode); +FMW_Mode fmw_mode_current(void) __attribute__((warn_unused_result)); +FMW_Mode fmw_mode_transition(FMW_Mode mode) __attribute__((warn_unused_result)); -FMW_Result fmw_result_from_uart_error(void) __attribute__((nonnull, warn_unused_result)); +FMW_Result fmw_result_from_uart_error(void) __attribute__((warn_unused_result)); -void fmw_motors_init(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_motors_deinit(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_motors_stop(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_motors_enable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_motors_disable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) __attribute__((nonnull)); +FMW_Result fmw_motors_init(void) __attribute__((warn_unused_result)); +FMW_Result fmw_motors_deinit(void) __attribute__((warn_unused_result)); +FMW_Result fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) __attribute__((nonnull, warn_unused_result)); +void fmw_motors_stop(void); -void fmw_encoders_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); -void fmw_encoders_deinit(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); -void fmw_encoders_update(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); -float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled) __attribute__((warn_unused_result, nonnull)); -void fmw_encoder_count_reset(FMW_Encoder *encoder) __attribute__((nonnull)); -int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) __attribute__((warn_unused_result, nonnull)); +FMW_Result fmw_encoders_init(void) __attribute__((nonnull, warn_unused_result)); +FMW_Result fmw_encoders_deinit(void) __attribute__((nonnull, warn_unused_result)); +FMW_Result fmw_encoders_update(void) __attribute__((nonnull, warn_unused_result)); +FMW_Result fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, + float meters_traveled, + float *linear_velocity) __attribute__((nonnull, warn_unused_result)); +FMW_Result fmw_encoder_count_reset(FMW_Encoder *encoder) __attribute__((nonnull, warn_unused_result)); +FMW_Result fmw_encoder_count_get(const FMW_Encoder *encoder, int32_t *ticks) __attribute__((nonnull, warn_unused_result)); void fmw_odometry_pose_update(FMW_Odometry *odometry, float meters_traveled_left, float meters_traveled_right) __attribute__((nonnull)); -int32_t fmw_pid_update(FMW_PidController *pid, float velocity) __attribute__((warn_unused_result, nonnull)); - -void fmw_led_init(FMW_Led *led) __attribute__((nonnull)); -void fmw_led_deinit(FMW_Led *led) __attribute__((nonnull)); -void fmw_led_update(FMW_Led *led) __attribute__((nonnull)); +int32_t fmw_pid_update(FMW_PidController *pid, float velocity) __attribute__((nonnull)); -void fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull)); +FMW_Result fmw_led_init(FMW_Led *led) __attribute__((nonnull, warn_unused_result)); +FMW_Result fmw_led_deinit(FMW_Led *led) __attribute__((nonnull, warn_unused_result)); +FMW_Result fmw_led_update(FMW_Led *led) __attribute__((nonnull, warn_unused_result)); -Vec2Float fmw_setpoint_from_velocities(const FMW_Odometry *odometry, float linear, float angular) __attribute__((nonnull)); +FMW_Result fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull, warn_unused_result)); #define FMW_LED_UPDATE_PERIOD 200 #define FMW_DEBOUNCE_DELAY 200 @@ -147,24 +142,9 @@ Vec2Float fmw_setpoint_from_velocities(const FMW_Odometry *odometry, float linea #define FMW_ADC_RESOLUTION 4095.0f #define FMW_VIN_SCALE_FACTOR 3.733f -#define FMW_METERS_FROM_TICKS(Ticks, WheelCircumference, TicksPerRevolution) \ - ((Ticks * WheelCircumference) / TicksPerRevolution) - -// NOTE(lb): The variadic arguments are the fields of FMW_Hook, so you can just type -// `.callback = your_function_pointer, .args = your_pointer_to_args`. -// Calling this macro with the condition argument is GCC extension. -// I don't use `assert` because i never figured out how to make it trigger -// a debug breakpoint inside the STM32 ide debugger (and also because having a -// on-failure callback is handy). -#define FMW_ASSERT(Cond, ...) \ - do { \ - FMW_Hook hook = { __VA_ARGS__ }; \ - if (!(Cond)) { \ - if (hook.callback) { \ - hook.callback(hook.args); \ - } \ - __builtin_trap(); \ - } \ - } while (0) +#define FMW_METERS_FROM_TICKS(Ticks, WheelCircumference, TicksPerRevolution) (((Ticks) * (WheelCircumference)) / (TicksPerRevolution)) + +// NOTE(lb): I don't use `assert` because i never figured out how to make it trigger a debug breakpoint inside the STM32 ide debugger. +#define FMW_ASSERT(Cond) do { if (!(Cond)) { __builtin_trap(); } } while (0) #endif diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index e17ea98..0167b3b 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -48,27 +48,28 @@ enum { #define FMW_RESULT_VARIANTS() \ X(FMW_Result_Ok, 0) \ X(FMW_Result_Error_InvalidArguments, 1) \ - X(FMW_Result_Error_FaultPinTriggered, 2) \ - X(FMW_Result_Error_UART_Crc, 3) \ - X(FMW_Result_Error_UART_NegativeTimeout, 4) \ - X(FMW_Result_Error_UART_ReceiveTimeoutElapsed, 5) \ - X(FMW_Result_Error_UART_Parity, 6) \ - X(FMW_Result_Error_UART_Frame, 7) \ - X(FMW_Result_Error_UART_Noise, 8) \ - X(FMW_Result_Error_UART_Overrun, 9) \ - X(FMW_Result_Error_Encoder_InvalidTimer, 10) \ - X(FMW_Result_Error_Encoder_NonPositiveTicksPerRevolution, 11) \ - X(FMW_Result_Error_Encoder_NonPositiveWheelCircumference, 12) \ - X(FMW_Result_Error_Encoder_GetTick, 13) \ - X(FMW_Result_Error_Buzzer_Timer, 14) \ - X(FMW_Result_Error_MessageHandler_InvalidState, 15) \ - X(FMW_Result_Error_MessageHandler_Config_NonPositiveBaseline, 16) \ - X(FMW_Result_Error_MessageHandler_Config_NonPositiveWheelCircumference, 17) \ - X(FMW_Result_Error_MessageHandler_Config_NonPositiveTicksPerRevolution, 18) \ - X(FMW_Result_Error_MessageHandler_Config_NonPositiveLEDUpdatePeriod, 19) \ - X(FMW_Result_Error_Command_NotRecognized, 20) \ - X(FMW_Result_Error_Command_NotAvailable, 21) \ - X(FMW_Result_COUNT, 22) + X(FMW_Result_Error_Hal, 2) \ + X(FMW_Result_Error_FaultPinTriggered, 3) \ + X(FMW_Result_Error_UART_Crc, 4) \ + X(FMW_Result_Error_UART_NegativeTimeout, 5) \ + X(FMW_Result_Error_UART_ReceiveTimeoutElapsed, 6) \ + X(FMW_Result_Error_UART_Parity, 7) \ + X(FMW_Result_Error_UART_Frame, 8) \ + X(FMW_Result_Error_UART_Noise, 9) \ + X(FMW_Result_Error_UART_Overrun, 10) \ + X(FMW_Result_Error_Encoder_InvalidTimer, 11) \ + X(FMW_Result_Error_Encoder_NonPositiveTicksPerRevolution, 12) \ + X(FMW_Result_Error_Encoder_NonPositiveWheelCircumference, 13) \ + X(FMW_Result_Error_Encoder_GetTick, 14) \ + X(FMW_Result_Error_Buzzer_Timer, 15) \ + X(FMW_Result_Error_MessageHandler_InvalidState, 16) \ + X(FMW_Result_Error_MessageHandler_Config_NonPositiveBaseline, 17) \ + X(FMW_Result_Error_MessageHandler_Config_NonPositiveWheelCircumference, 18) \ + X(FMW_Result_Error_MessageHandler_Config_NonPositiveTicksPerRevolution, 19) \ + X(FMW_Result_Error_MessageHandler_Config_NonPositiveLEDUpdatePeriod, 20) \ + X(FMW_Result_Error_Command_NotRecognized, 21) \ + X(FMW_Result_Error_Command_NotAvailable, 22) \ + X(FMW_Result_COUNT, 23) typedef uint8_t FMW_Result; enum { diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c index 0720bee..0863b6a 100644 --- a/pioneer_controller/Core/Src/firmware/fwm_core.c +++ b/pioneer_controller/Core/Src/firmware/fwm_core.c @@ -14,7 +14,9 @@ static struct { volatile uint32_t emergency_last_message_received_time; FMW_Motor *motors; + FMW_Encoder *encoders; int32_t motors_count; + int32_t encoders_count; bool motors_active; bool motors_active_before_emergency; @@ -30,25 +32,55 @@ static struct { .mode_previous = FMW_Mode_None, }; -static void fmw_hook_assert_fail(void *_) { - if (fmw_state.motors != NULL) { - fmw_motors_stop(fmw_state.motors, fmw_state.motors_count); - } -} - // ============================================================ // Firmware initialization -void fmw_init(const FMW_InitInfo *info) { - FMW_ASSERT(info->message_exchange.huart != NULL); - FMW_ASSERT(info->message_exchange.hcrc != NULL); - FMW_ASSERT(info->message_exchange.handler != NULL); - FMW_ASSERT(info->emergency.on_begin != NULL); - FMW_ASSERT(info->emergency.on_end != NULL); - FMW_ASSERT(info->motors_count >= 0); - if (info->motors_count > 0) { FMW_ASSERT(info->motors != NULL); } +FMW_Result fmw_init(const FMW_InitInfo *info) { + if ((info->message_exchange.huart == NULL) || (info->message_exchange.hcrc == NULL) || + (info->message_exchange.handler == NULL) || (info->emergency.on_begin == NULL) || + (info->emergency.on_end == NULL) || + (info->motors_count < 0) || (info->motors_count > 0 && info->motors == NULL) || + (info->encoders_count < 0) || (info->encoders_count > 0 && info->encoders == NULL)) { + return FMW_Result_Error_InvalidArguments; + } + + for (int32_t i = 0; i < info->motors_count; ++i) { + if ((info->motors[i].sleep_gpio_port == NULL) || (info->motors[i].dir_gpio_port == NULL) || + (info->motors[i].pwm_timer == NULL) || (info->motors[i].dir_pin == info->motors[i].sleep_pin) || + (info->motors[i].pwm_channel != TIM_CHANNEL_1 && info->motors[i].pwm_channel != TIM_CHANNEL_2 && + info->motors[i].pwm_channel != TIM_CHANNEL_3 && info->motors[i].pwm_channel != TIM_CHANNEL_4 && + info->motors[i].pwm_channel != TIM_CHANNEL_5 && info->motors[i].pwm_channel != TIM_CHANNEL_6 && + info->motors[i].pwm_channel != TIM_CHANNEL_ALL) || + (info->motors[i].sleep_pin != GPIO_PIN_1 && info->motors[i].sleep_pin != GPIO_PIN_2 && + info->motors[i].sleep_pin != GPIO_PIN_3 && info->motors[i].sleep_pin != GPIO_PIN_4 && + info->motors[i].sleep_pin != GPIO_PIN_5 && info->motors[i].sleep_pin != GPIO_PIN_6 && + info->motors[i].sleep_pin != GPIO_PIN_7 && info->motors[i].sleep_pin != GPIO_PIN_8 && + info->motors[i].sleep_pin != GPIO_PIN_9 && info->motors[i].sleep_pin != GPIO_PIN_10 && + info->motors[i].sleep_pin != GPIO_PIN_11 && info->motors[i].sleep_pin != GPIO_PIN_12 && + info->motors[i].sleep_pin != GPIO_PIN_13 && info->motors[i].sleep_pin != GPIO_PIN_14 && + info->motors[i].sleep_pin != GPIO_PIN_15 && info->motors[i].sleep_pin != GPIO_PIN_All) || + (info->motors[i].dir_pin != GPIO_PIN_1 && info->motors[i].dir_pin != GPIO_PIN_2 && + info->motors[i].dir_pin != GPIO_PIN_3 && info->motors[i].dir_pin != GPIO_PIN_4 && + info->motors[i].dir_pin != GPIO_PIN_5 && info->motors[i].dir_pin != GPIO_PIN_6 && + info->motors[i].dir_pin != GPIO_PIN_7 && info->motors[i].dir_pin != GPIO_PIN_8 && + info->motors[i].dir_pin != GPIO_PIN_9 && info->motors[i].dir_pin != GPIO_PIN_10 && + info->motors[i].dir_pin != GPIO_PIN_11 && info->motors[i].dir_pin != GPIO_PIN_12 && + info->motors[i].dir_pin != GPIO_PIN_13 && info->motors[i].dir_pin != GPIO_PIN_14 && + info->motors[i].dir_pin != GPIO_PIN_15 && info->motors[i].dir_pin != GPIO_PIN_All)) { + return FMW_Result_Error_InvalidArguments; + } + } + + for (int32_t i = 0; i < info->encoders_count; ++i) { + if ((info->encoders[i].timer == NULL) || (info->encoders[i].ticks_per_revolution <= 0) || + (info->encoders[i].wheel_circumference <= 0.f)) { + return FMW_Result_Error_InvalidArguments; + } + } fmw_state.motors = info->motors; fmw_state.motors_count = info->motors_count; + fmw_state.encoders = info->encoders; + fmw_state.encoders_count = info->encoders_count; fmw_state.huart = info->message_exchange.huart; fmw_state.hcrc = info->message_exchange.hcrc; fmw_state.emergency_mode_grace_period_ms = info->emergency.wait_at_most_ms_before_emergency; @@ -58,10 +90,12 @@ void fmw_init(const FMW_InitInfo *info) { fmw_state.emergency_timer = info->emergency.timer; HAL_StatusTypeDef timer_init_res = HAL_TIM_Base_Start_IT(info->emergency.timer); - FMW_ASSERT(timer_init_res == HAL_OK); + if (timer_init_res != HAL_OK) { return FMW_Result_Error_Hal; } HAL_StatusTypeDef dma_init_res = HAL_UART_Receive_DMA(fmw_state.huart, (uint8_t*)&fmw_state.uart_buffer, sizeof fmw_state.uart_buffer); - FMW_ASSERT(dma_init_res == HAL_OK); + if (dma_init_res != HAL_OK) { return FMW_Result_Error_Hal; } + + return FMW_Result_Ok; } void fmw_uart_message_dispatch(void) { @@ -75,7 +109,7 @@ void fmw_uart_error(void) { // NOTE(lb): i don't know how to determine if the error that cause the jump here // was during a receive or a send of a message over UART, so i'm just // going to stop the motors and abort the receive just in case. - fmw_motors_stop(fmw_state.motors, fmw_state.motors_count); + fmw_motors_stop(); HAL_UART_AbortReceive(fmw_state.huart); FMW_Message response = {0}; @@ -89,8 +123,8 @@ void fmw_emergency_begin(void) { if (fmw_state.motors_count > 0) { fmw_state.motors_active_before_emergency = fmw_state.motors[0].active; if (fmw_state.motors[0].active) { - fmw_motors_stop(fmw_state.motors, fmw_state.motors_count); - fmw_motors_disable(fmw_state.motors, fmw_state.motors_count); + fmw_motors_stop(); + fmw_motors_deinit(); } } @@ -105,7 +139,7 @@ void fmw_emergency_end(void) { fmw_state.mode_current = fmw_state.mode_previous; fmw_state.mode_previous = FMW_Mode_None; if (fmw_state.motors_count > 0 && fmw_state.motors_active_before_emergency) { - fmw_motors_enable(fmw_state.motors, fmw_state.motors_count); + fmw_motors_init(); } if (fmw_state.callback_emergency_end) { fmw_state.callback_emergency_end(); } } @@ -122,29 +156,15 @@ FMW_Mode fmw_mode_current(void) { return fmw_state.mode_current; } -FMW_Mode fmw_mode_transition(FMW_Mode mode) { - FMW_ASSERT(mode > FMW_Mode_None); - FMW_ASSERT(mode < FMW_Mode_COUNT); - FMW_Mode old = fmw_state.mode_previous; +FMW_Result fmw_mode_transition(FMW_Mode mode) { + if (mode <= FMW_Mode_None || mode >= FMW_Mode_COUNT) { return FMW_Result_Error_InvalidArguments; } fmw_state.mode_previous = fmw_state.mode_current; fmw_state.mode_current = mode; - return old; + return FMW_Result_Ok; } // ============================================================ // Misc -FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) { - FMW_ASSERT(wait_ms >= 0, .callback = fmw_hook_assert_fail); - memset(msg, 0, sizeof *msg); - - HAL_StatusTypeDef uart_packet_status = HAL_UART_Receive(huart, (uint8_t*)msg, sizeof(*msg), wait_ms); - if (!(uart_packet_status == HAL_OK)) { return FMW_Result_Error_UART_ReceiveTimeoutElapsed; } - if (!(msg->header.type > FMW_MessageType_None && msg->header.type < FMW_MessageType_COUNT)) { - return FMW_Result_Error_Command_NotRecognized; - } - return FMW_Result_Ok; -} - void fmw_uart_message_send(FMW_Message *msg) { msg->header.crc = HAL_CRC_Calculate(fmw_state.hcrc, (uint32_t*)msg, sizeof *msg); HAL_StatusTypeDef res = HAL_UART_Transmit(fmw_state.huart, (uint8_t*)msg, sizeof *msg, fmw_state.emergency_mode_grace_period_ms); @@ -152,7 +172,6 @@ void fmw_uart_message_send(FMW_Message *msg) { } FMW_Result fmw_result_from_uart_error(void) { - FMW_ASSERT(fmw_state.huart->ErrorCode != HAL_UART_ERROR_NONE, .callback = fmw_hook_assert_fail); switch (fmw_state.huart->ErrorCode) { case HAL_UART_ERROR_PE: { return FMW_Result_Error_UART_Parity; @@ -169,195 +188,118 @@ FMW_Result fmw_result_from_uart_error(void) { case HAL_UART_ERROR_RTO: { return FMW_Result_Error_UART_ReceiveTimeoutElapsed; } break; - default: { // NOTE(lb): unreachable - FMW_ASSERT(false, .callback = fmw_hook_assert_fail); + default: { + return FMW_Result_Ok; } break; } - return FMW_Result_Ok; -} - -Vec2Float fmw_setpoint_from_velocities(const FMW_Odometry *odometry, float linear, float angular) { - FMW_ASSERT(odometry->baseline > 0.f, .callback = fmw_hook_assert_fail); - Vec2Float res = { - .left = linear - (odometry->baseline * angular) / 2.f, - .right = linear + (odometry->baseline * angular) / 2.f, - }; - return res; } // ============================================================ // Motor controller -void fmw_motors_init(FMW_Motor motors[], int32_t count) { - FMW_ASSERT(count > 0); - fmw_state.motors = motors; - fmw_state.motors_count = count; - - for (int32_t i = 0; i < count; ++i) { - FMW_ASSERT(motors[i].sleep_gpio_port != NULL); - FMW_ASSERT(motors[i].dir_gpio_port != NULL); - FMW_ASSERT(motors[i].pwm_timer != NULL); - FMW_ASSERT(motors[i].pwm_channel == TIM_CHANNEL_1 || motors[i].pwm_channel == TIM_CHANNEL_2 || - motors[i].pwm_channel == TIM_CHANNEL_3 || motors[i].pwm_channel == TIM_CHANNEL_4 || - motors[i].pwm_channel == TIM_CHANNEL_5 || motors[i].pwm_channel == TIM_CHANNEL_6 || - motors[i].pwm_channel == TIM_CHANNEL_ALL); - FMW_ASSERT(motors[i].sleep_pin == GPIO_PIN_1 || motors[i].sleep_pin == GPIO_PIN_2 || - motors[i].sleep_pin == GPIO_PIN_3 || motors[i].sleep_pin == GPIO_PIN_4 || - motors[i].sleep_pin == GPIO_PIN_5 || motors[i].sleep_pin == GPIO_PIN_6 || - motors[i].sleep_pin == GPIO_PIN_7 || motors[i].sleep_pin == GPIO_PIN_8 || - motors[i].sleep_pin == GPIO_PIN_9 || motors[i].sleep_pin == GPIO_PIN_10 || - motors[i].sleep_pin == GPIO_PIN_11 || motors[i].sleep_pin == GPIO_PIN_12 || - motors[i].sleep_pin == GPIO_PIN_13 || motors[i].sleep_pin == GPIO_PIN_14 || - motors[i].sleep_pin == GPIO_PIN_15 || motors[i].sleep_pin == GPIO_PIN_All); - FMW_ASSERT(motors[i].dir_pin == GPIO_PIN_1 || motors[i].dir_pin == GPIO_PIN_2 || - motors[i].dir_pin == GPIO_PIN_3 || motors[i].dir_pin == GPIO_PIN_4 || - motors[i].dir_pin == GPIO_PIN_5 || motors[i].dir_pin == GPIO_PIN_6 || - motors[i].dir_pin == GPIO_PIN_7 || motors[i].dir_pin == GPIO_PIN_8 || - motors[i].dir_pin == GPIO_PIN_9 || motors[i].dir_pin == GPIO_PIN_10 || - motors[i].dir_pin == GPIO_PIN_11 || motors[i].dir_pin == GPIO_PIN_12 || - motors[i].dir_pin == GPIO_PIN_13 || motors[i].dir_pin == GPIO_PIN_14 || - motors[i].dir_pin == GPIO_PIN_15 || motors[i].dir_pin == GPIO_PIN_All); - FMW_ASSERT(motors[i].dir_pin != motors[i].sleep_pin); - - HAL_StatusTypeDef status = HAL_TIM_PWM_Start(motors[i].pwm_timer, motors[i].pwm_channel); - FMW_ASSERT(status == HAL_OK); - motors[i].max_dutycycle = motors[i].pwm_timer->Instance->ARR; - motors[i].active = true; +FMW_Result fmw_motors_init(void) { + for (int32_t i = 0; i < fmw_state.motors_count; ++i) { + HAL_StatusTypeDef status = HAL_TIM_PWM_Start(fmw_state.motors[i].pwm_timer, fmw_state.motors[i].pwm_channel); + if (status != HAL_OK) { return FMW_Result_Error_Hal; } + fmw_state.motors[i].max_dutycycle = fmw_state.motors[i].pwm_timer->Instance->ARR; + fmw_state.motors[i].active = true; } - fmw_motors_stop(motors, count); + fmw_motors_stop(); + return FMW_Result_Ok; } -void fmw_motors_deinit(FMW_Motor motors[], int32_t count) { - for (int32_t i = 0; i < count; ++i) { - HAL_StatusTypeDef status = HAL_TIM_PWM_Stop(motors[i].pwm_timer, motors[i].pwm_channel); - FMW_ASSERT(status == HAL_OK); - motors[i].active = false; +FMW_Result fmw_motors_deinit(void) { + for (int32_t i = 0; i < fmw_state.motors_count; ++i) { + HAL_StatusTypeDef status = HAL_TIM_PWM_Stop(fmw_state.motors[i].pwm_timer, fmw_state.motors[i].pwm_channel); + if (status != HAL_OK) { return FMW_Result_Error_Hal; } + fmw_state.motors[i].active = false; } + return FMW_Result_Ok; } -void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) { - FMW_ASSERT(motor->dir_gpio_port != NULL, .callback = fmw_hook_assert_fail); - FMW_ASSERT(motor->dir_pin == GPIO_PIN_1 || motor->dir_pin == GPIO_PIN_2 || - motor->dir_pin == GPIO_PIN_3 || motor->dir_pin == GPIO_PIN_4 || - motor->dir_pin == GPIO_PIN_5 || motor->dir_pin == GPIO_PIN_6 || - motor->dir_pin == GPIO_PIN_7 || motor->dir_pin == GPIO_PIN_8 || - motor->dir_pin == GPIO_PIN_9 || motor->dir_pin == GPIO_PIN_10 || - motor->dir_pin == GPIO_PIN_11 || motor->dir_pin == GPIO_PIN_12 || - motor->dir_pin == GPIO_PIN_13 || motor->dir_pin == GPIO_PIN_14 || - motor->dir_pin == GPIO_PIN_15 || motor->dir_pin == GPIO_PIN_All, - .callback = fmw_hook_assert_fail); - HAL_GPIO_WritePin(motor->dir_gpio_port, motor->dir_pin, - (duty_cycle >= 0 ? FMW_MotorDirection_Forward : FMW_MotorDirection_Backward)); - duty_cycle = CLAMP_TOP(ABS(duty_cycle), motor->max_dutycycle); - __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, duty_cycle); - HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET); -} - -void fmw_motors_stop(FMW_Motor motors[], int32_t count) { - FMW_ASSERT(count > 0); - for (int32_t i = 0; i < count; ++i) { - FMW_ASSERT(motors[i].sleep_gpio_port != NULL); - FMW_ASSERT(motors[i].sleep_pin == GPIO_PIN_1 || motors[i].sleep_pin == GPIO_PIN_2 || - motors[i].sleep_pin == GPIO_PIN_3 || motors[i].sleep_pin == GPIO_PIN_4 || - motors[i].sleep_pin == GPIO_PIN_5 || motors[i].sleep_pin == GPIO_PIN_6 || - motors[i].sleep_pin == GPIO_PIN_7 || motors[i].sleep_pin == GPIO_PIN_8 || - motors[i].sleep_pin == GPIO_PIN_9 || motors[i].sleep_pin == GPIO_PIN_10 || - motors[i].sleep_pin == GPIO_PIN_11 || motors[i].sleep_pin == GPIO_PIN_12 || - motors[i].sleep_pin == GPIO_PIN_13 || motors[i].sleep_pin == GPIO_PIN_14 || - motors[i].sleep_pin == GPIO_PIN_15 || motors[i].sleep_pin == GPIO_PIN_All); - FMW_ASSERT(motors[i].pwm_timer != NULL); - FMW_ASSERT(motors[i].pwm_channel == TIM_CHANNEL_1 || motors[i].pwm_channel == TIM_CHANNEL_2 || - motors[i].pwm_channel == TIM_CHANNEL_3 || motors[i].pwm_channel == TIM_CHANNEL_4 || - motors[i].pwm_channel == TIM_CHANNEL_5 || motors[i].pwm_channel == TIM_CHANNEL_6 || - motors[i].pwm_channel == TIM_CHANNEL_ALL); - - HAL_GPIO_WritePin(motors[i].sleep_gpio_port, motors[i].sleep_pin, GPIO_PIN_RESET); - __HAL_TIM_SET_COMPARE(motors[i].pwm_timer, motors[i].pwm_channel, 0); +void fmw_motors_stop(void) { + for (int32_t i = 0; i < fmw_state.motors_count; ++i) { + HAL_GPIO_WritePin(fmw_state.motors[i].sleep_gpio_port, fmw_state.motors[i].sleep_pin, GPIO_PIN_RESET); + __HAL_TIM_SET_COMPARE(fmw_state.motors[i].pwm_timer, fmw_state.motors[i].pwm_channel, 0); } } -void fmw_motors_enable(FMW_Motor motors[], int32_t count) { - FMW_ASSERT(count > 0); - for (int32_t i = 0; i < count; ++i) { - FMW_ASSERT(motors[i].pwm_timer != NULL); - FMW_ASSERT(motors[i].pwm_channel == TIM_CHANNEL_1 || motors[i].pwm_channel == TIM_CHANNEL_2 || - motors[i].pwm_channel == TIM_CHANNEL_3 || motors[i].pwm_channel == TIM_CHANNEL_4 || - motors[i].pwm_channel == TIM_CHANNEL_5 || motors[i].pwm_channel == TIM_CHANNEL_6 || - motors[i].pwm_channel == TIM_CHANNEL_ALL); - FMW_ASSERT(!motors[i].active); - HAL_StatusTypeDef res = HAL_TIM_PWM_Start(motors[i].pwm_timer, motors[i].pwm_channel); - FMW_ASSERT(res == HAL_OK); - motors[i].active = true; +FMW_Result fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) { + if (motor < fmw_state.motors || motor >= (fmw_state.motors + fmw_state.motors_count)) { + return FMW_Result_Error_InvalidArguments; } -} -void fmw_motors_disable(FMW_Motor motors[], int32_t count) { - FMW_ASSERT(count > 0, .callback = fmw_hook_assert_fail); - for (int32_t i = 0; i < count; ++i) { - FMW_ASSERT(motors[i].pwm_timer != NULL, .callback = fmw_hook_assert_fail); - FMW_ASSERT(motors[i].pwm_channel == TIM_CHANNEL_1 || motors[i].pwm_channel == TIM_CHANNEL_2 || - motors[i].pwm_channel == TIM_CHANNEL_3 || motors[i].pwm_channel == TIM_CHANNEL_4 || - motors[i].pwm_channel == TIM_CHANNEL_5 || motors[i].pwm_channel == TIM_CHANNEL_6 || - motors[i].pwm_channel == TIM_CHANNEL_ALL, .callback = fmw_hook_assert_fail); - FMW_ASSERT(motors[i].active); - HAL_StatusTypeDef res = HAL_TIM_PWM_Stop(motors[i].pwm_timer, motors[i].pwm_channel); - FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail); - motors[i].active = false; - } + HAL_GPIO_WritePin(motor->dir_gpio_port, motor->dir_pin, (duty_cycle >= 0 ? FMW_MotorDirection_Forward : FMW_MotorDirection_Backward)); + duty_cycle = CLAMP_TOP(ABS(duty_cycle), motor->max_dutycycle); + __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, duty_cycle); + HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET); + + return FMW_Result_Ok; } // ============================================================ // Encoder -void fmw_encoders_init(FMW_Encoder encoders[], int32_t count) { - for (int32_t i = 0; i < count; ++i) { - FMW_ASSERT(encoders[i].timer != NULL); - FMW_ASSERT(encoders[i].ticks_per_revolution > 0); - FMW_ASSERT(encoders[i].wheel_circumference > 0.f); - - encoders[i].previous_millis = 0; - encoders[i].current_millis = 0; - encoders[i].ticks = 0; - - HAL_StatusTypeDef status = HAL_TIM_Encoder_Start(encoders[i].timer, TIM_CHANNEL_ALL); - FMW_ASSERT(status == HAL_OK); - fmw_encoder_count_reset(&encoders[i]); - encoders[i].current_millis = HAL_GetTick(); +FMW_Result fmw_encoders_init(void) { + for (int32_t i = 0; i < fmw_state.encoders_count; ++i) { + fmw_state.encoders[i].previous_millis = 0; + fmw_state.encoders[i].current_millis = 0; + fmw_state.encoders[i].ticks = 0; + + HAL_StatusTypeDef status = HAL_TIM_Encoder_Start(fmw_state.encoders[i].timer, TIM_CHANNEL_ALL); + if (status != HAL_OK) { return FMW_Result_Error_Hal; } + FMW_Result res = fmw_encoder_count_reset(&fmw_state.encoders[i]); + if (res != FMW_Result_Ok) { return res; } + fmw_state.encoders[i].current_millis = HAL_GetTick(); } + return FMW_Result_Ok; } -void fmw_encoders_deinit(FMW_Encoder encoders[], int32_t count) { - for (int32_t i = 0; i < count; ++i) { - FMW_ASSERT(encoders[i].timer != NULL); - HAL_StatusTypeDef status = HAL_TIM_Encoder_Stop(encoders[i].timer, TIM_CHANNEL_ALL); - FMW_ASSERT(status == HAL_OK); +FMW_Result fmw_encoders_deinit(void) { + for (int32_t i = 0; i < fmw_state.encoders_count; ++i) { + HAL_StatusTypeDef status = HAL_TIM_Encoder_Stop(fmw_state.encoders[i].timer, TIM_CHANNEL_ALL); + if (status != HAL_OK) { return FMW_Result_Error_Hal; } } + return FMW_Result_Ok; } -void fmw_encoders_update(FMW_Encoder encoders[], int32_t count) { - for (int32_t i = 0; i < count; ++i) { - encoders[i].previous_millis = encoders[i].current_millis; - encoders[i].current_millis = HAL_GetTick(); - encoders[i].ticks = fmw_encoder_count_get(&encoders[i]); - fmw_encoder_count_reset(&encoders[i]); - FMW_ASSERT(encoders[i].current_millis >= encoders[i].previous_millis, .callback = fmw_hook_assert_fail); +FMW_Result fmw_encoders_update(void) { + for (int32_t i = 0; i < fmw_state.encoders_count; ++i) { + fmw_state.encoders[i].previous_millis = fmw_state.encoders[i].current_millis; + fmw_state.encoders[i].current_millis = HAL_GetTick(); + FMW_Result res = fmw_encoder_count_get(&fmw_state.encoders[i], &fmw_state.encoders[i].ticks); + if (res != FMW_Result_Ok) { return res; } + res = fmw_encoder_count_reset(&fmw_state.encoders[i]); + if (res != FMW_Result_Ok) { return res; } } + return FMW_Result_Ok; } -float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled) { +FMW_Result fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled, float *linear_velocity) { + if (encoder < fmw_state.encoders || encoder >= (fmw_state.encoders + fmw_state.encoders_count)) { + return FMW_Result_Error_InvalidArguments; + } float deltatime = encoder->current_millis - encoder->previous_millis; - if (deltatime == 0.f) { return 0.f; } - float linear_velocity = meters_traveled / (deltatime / 1000.f); - return linear_velocity; + if (deltatime == 0.f) { + *linear_velocity = 0.f; + } else { + *linear_velocity = meters_traveled / (deltatime / 1000.f); + } + return FMW_Result_Ok; } -void fmw_encoder_count_reset(FMW_Encoder *encoder) { - FMW_ASSERT(encoder->timer != NULL, .callback = fmw_hook_assert_fail); +FMW_Result fmw_encoder_count_reset(FMW_Encoder *encoder) { + if (encoder < fmw_state.encoders || encoder >= (fmw_state.encoders + fmw_state.encoders_count)) { + return FMW_Result_Error_InvalidArguments; + } __HAL_TIM_SET_COUNTER(encoder->timer, (encoder->timer->Init.Period / 2)); + return FMW_Result_Ok; } -int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) { - FMW_ASSERT(encoder->timer != NULL, .callback = fmw_hook_assert_fail); - int32_t res = (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2); - return res; +FMW_Result fmw_encoder_count_get(const FMW_Encoder *encoder, int32_t *ticks) { + if (encoder < fmw_state.encoders || encoder >= (fmw_state.encoders + fmw_state.encoders_count)) { + return FMW_Result_Error_InvalidArguments; + } + *ticks = (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2); + return FMW_Result_Ok; } // ============================================================ @@ -429,44 +371,49 @@ int32_t fmw_pid_update(FMW_PidController *pid, float velocity) { // ============================================================ // LEDs -void fmw_led_init(FMW_Led *led) { - FMW_ASSERT(led->timer != NULL); - FMW_ASSERT(led->adc != NULL); - FMW_ASSERT(led->timer_channel == TIM_CHANNEL_1 || led->timer_channel == TIM_CHANNEL_2 || - led->timer_channel == TIM_CHANNEL_3 || led->timer_channel == TIM_CHANNEL_4 || - led->timer_channel == TIM_CHANNEL_5 || led->timer_channel == TIM_CHANNEL_6 || - led->timer_channel == TIM_CHANNEL_ALL); - FMW_ASSERT(led->voltage_red > 0.f); - FMW_ASSERT(led->voltage_orange > 0.f); - FMW_ASSERT(led->voltage_hysteresis >= 0.f); - FMW_ASSERT(led->state < FMW_LedState_COUNT); +FMW_Result fmw_led_init(FMW_Led *led) { + if ((led->timer == NULL) || (led->adc == NULL) || + (led->voltage_red <= 0.f) || (led->voltage_orange <= 0.f) || (led->voltage_hysteresis < 0.f) || + (led->timer_channel != TIM_CHANNEL_1 && led->timer_channel != TIM_CHANNEL_2 && + led->timer_channel != TIM_CHANNEL_3 && led->timer_channel != TIM_CHANNEL_4 && + led->timer_channel != TIM_CHANNEL_5 && led->timer_channel != TIM_CHANNEL_6 && + led->timer_channel != TIM_CHANNEL_ALL) || (led->state >= FMW_LedState_COUNT)) { + return FMW_Result_Error_InvalidArguments; + } HAL_StatusTypeDef status = HAL_TIM_PWM_Start(led->timer, led->timer_channel); - FMW_ASSERT(status == HAL_OK); + if (status != HAL_OK) { return FMW_Result_Error_Hal; } __HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, 0); + return FMW_Result_Ok; } -void fmw_led_deinit(FMW_Led *led) { +FMW_Result fmw_led_deinit(FMW_Led *led) { + if ((led->timer == NULL) || (led->adc == NULL) || + (led->voltage_red <= 0.f) || (led->voltage_orange <= 0.f) || (led->voltage_hysteresis < 0.f) || + (led->timer_channel != TIM_CHANNEL_1 && led->timer_channel != TIM_CHANNEL_2 && + led->timer_channel != TIM_CHANNEL_3 && led->timer_channel != TIM_CHANNEL_4 && + led->timer_channel != TIM_CHANNEL_5 && led->timer_channel != TIM_CHANNEL_6 && + led->timer_channel != TIM_CHANNEL_ALL) || (led->state >= FMW_LedState_COUNT)) { + return FMW_Result_Error_InvalidArguments; + } HAL_StatusTypeDef status = HAL_TIM_PWM_Stop(led->timer, led->timer_channel); - FMW_ASSERT(status == HAL_OK); + if (status == HAL_OK) { return FMW_Result_Error_Hal; } + return FMW_Result_Ok; } -void fmw_led_update(FMW_Led *led) { - FMW_ASSERT(led->timer != NULL, .callback = fmw_hook_assert_fail); - FMW_ASSERT(led->adc != NULL, .callback = fmw_hook_assert_fail); - FMW_ASSERT(led->timer_channel == TIM_CHANNEL_1 || led->timer_channel == TIM_CHANNEL_2 || - led->timer_channel == TIM_CHANNEL_3 || led->timer_channel == TIM_CHANNEL_4 || - led->timer_channel == TIM_CHANNEL_5 || led->timer_channel == TIM_CHANNEL_6 || - led->timer_channel == TIM_CHANNEL_ALL, .callback = fmw_hook_assert_fail); - FMW_ASSERT(led->voltage_red > 0.f, .callback = fmw_hook_assert_fail); - FMW_ASSERT(led->voltage_orange > 0.f, .callback = fmw_hook_assert_fail); - FMW_ASSERT(led->voltage_hysteresis >= 0.f, .callback = fmw_hook_assert_fail); - FMW_ASSERT(led->state < FMW_LedState_COUNT, .callback = fmw_hook_assert_fail); - +FMW_Result fmw_led_update(FMW_Led *led) { + if ((led->timer == NULL) || (led->adc == NULL) || + (led->voltage_red <= 0.f) || (led->voltage_orange <= 0.f) || (led->voltage_hysteresis < 0.f) || + (led->timer_channel != TIM_CHANNEL_1 && led->timer_channel != TIM_CHANNEL_2 && + led->timer_channel != TIM_CHANNEL_3 && led->timer_channel != TIM_CHANNEL_4 && + led->timer_channel != TIM_CHANNEL_5 && led->timer_channel != TIM_CHANNEL_6 && + led->timer_channel != TIM_CHANNEL_ALL) || (led->state >= FMW_LedState_COUNT)) { + return FMW_Result_Error_InvalidArguments; + } HAL_StatusTypeDef adc_start_res = HAL_ADC_Start(led->adc); - FMW_ASSERT(adc_start_res == HAL_OK, .callback = fmw_hook_assert_fail); + if (adc_start_res != HAL_OK) { return FMW_Result_Error_Hal; } HAL_StatusTypeDef adc_poll_res = HAL_ADC_PollForConversion(led->adc, HAL_MAX_DELAY); - FMW_ASSERT(adc_poll_res == HAL_OK, .callback = fmw_hook_assert_fail); + if (adc_poll_res != HAL_OK) { return FMW_Result_Error_Hal; } uint32_t adc_val = HAL_ADC_GetValue(led->adc); float v_adc = ((float)adc_val / FMW_ADC_RESOLUTION) * FMW_V_REF; @@ -513,13 +460,14 @@ void fmw_led_update(FMW_Led *led) { } __HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, duty); + return FMW_Result_Ok; } // ============================================================ // Buzzers // NOTE(lb): replace bool with uint8_t bitmask? -void fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) { - FMW_ASSERT(count >= 0, .callback = fmw_hook_assert_fail); +FMW_Result fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) { + if (count <= 0) { return FMW_Result_Error_InvalidArguments; } for (int32_t i = 0; i < count; ++i) { HAL_StatusTypeDef res; if (on) { @@ -529,6 +477,7 @@ void fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) { } else { res = HAL_TIM_PWM_Stop(buzzer[i].timer, buzzer[i].timer_channel); } - FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail); + if (res != HAL_OK) { return FMW_Result_Error_Hal; } } + return FMW_Result_Ok; } diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c index 53fddf0..483423d 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -34,6 +34,14 @@ /* USER CODE BEGIN PTD */ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) __attribute__((nonnull)); +static __attribute__((always_inline)) inline Vec2Float setpoint_from_velocities(float baseline, float linear, float angular) { + Vec2Float res = { + .left = linear - (baseline * angular) / 2.f, + .right = linear + (baseline * angular) / 2.f, + }; + return res; +} + void emergency_mode_begin(void); void emergency_mode_end(void); /* USER CODE END PTD */ @@ -851,7 +859,7 @@ void start(void) { .handler = message_handler, }, }; - fmw_init(&fmw_info); + FMW_ASSERT(fmw_init(&fmw_info) == FMW_Result_Ok); for (;;) { switch (fmw_mode_current()) { @@ -862,7 +870,7 @@ void start(void) { uint32_t time_now = HAL_GetTick(); if (time_now - time_last_led_update >= led_update_period) { time_last_led_update = time_now; - fmw_led_update(&pled); + FMW_ASSERT(fmw_led_update(&pled) == FMW_Result_Ok); } } break; } @@ -913,8 +921,8 @@ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { case FMW_Mode_Config: { switch (msg->header.type) { case FMW_MessageType_ModeChange_Run: { - fmw_encoders_init(encoders.values, ARRLENGTH(encoders.values)); - fmw_motors_init(motors.values, ARRLENGTH(motors.values)); + fmw_encoders_init(); + fmw_motors_init(); fmw_led_init(&pled); // Right and left motors have the same parameters @@ -985,7 +993,7 @@ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { case FMW_Mode_Run: { switch (msg->header.type) { case FMW_MessageType_Run_SetVelocity: { - Vec2Float setpoint = fmw_setpoint_from_velocities(&odometry, msg->run_set_velocity.linear, msg->run_set_velocity.angular); + Vec2Float setpoint = setpoint_from_velocities(odometry.baseline, msg->run_set_velocity.linear, msg->run_set_velocity.angular); pid_left.setpoint = setpoint.left; pid_right.setpoint = setpoint.right; pid_cross.setpoint = setpoint.left - setpoint.right; @@ -994,8 +1002,13 @@ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { // NOTE(lb): SetVelocity continues here as well because the previous case doesn't end with a `break`. // order matters. - int32_t current_ticks_left = ticks_left + fmw_encoder_count_get(&encoders.left); - int32_t current_ticks_right = ticks_right + fmw_encoder_count_get(&encoders.right); + int32_t ticks_measured_left = 0; + int32_t ticks_measured_right = 0; + fmw_encoder_count_get(&encoders.left, &ticks_measured_left); + fmw_encoder_count_get(&encoders.right, &ticks_measured_right); + + int32_t current_ticks_left = ticks_left + ticks_measured_left; + int32_t current_ticks_right = ticks_right + ticks_measured_right; ticks_left = ticks_right = 0; static float time_millis_previous = 0.f; @@ -1021,12 +1034,12 @@ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { // so they don't continue down to `msg_contains_error`. } break; case FMW_MessageType_ModeChange_Config: { - fmw_motors_stop(motors.values, ARRLENGTH(motors.values)); + fmw_motors_stop(); fmw_encoder_count_reset(&encoders.left); fmw_encoder_count_reset(&encoders.right); - fmw_encoders_deinit(encoders.values, ARRLENGTH(encoders.values)); - fmw_motors_deinit(motors.values, ARRLENGTH(motors.values)); + fmw_encoders_deinit(); + fmw_motors_deinit(); fmw_led_deinit(&pled); HAL_StatusTypeDef timer_status = HAL_TIM_Base_Stop_IT(&htim6); @@ -1076,7 +1089,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim == &htim7) { // TIMER 1Hz no message exchange check fmw_emergency_timer_update(); } else if (htim == &htim6) { // TIMER 100Hz PID control - fmw_encoders_update(encoders.values, ARRLENGTH(encoders.values)); + fmw_encoders_update(); ticks_left += encoders.left.ticks; ticks_right += encoders.right.ticks; @@ -1085,8 +1098,11 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { fmw_odometry_pose_update(&odometry, meters_traveled_left, meters_traveled_right); - float velocity_left = fmw_encoder_get_linear_velocity(&encoders.left, meters_traveled_left); - float velocity_right = fmw_encoder_get_linear_velocity(&encoders.right, meters_traveled_right); + float velocity_left = 0.f; + float velocity_right = 0.f; + fmw_encoder_get_linear_velocity(&encoders.left, meters_traveled_left, &velocity_left); + fmw_encoder_get_linear_velocity(&encoders.right, meters_traveled_right, &velocity_right); + odometry.velocity_linear.left = velocity_left; odometry.velocity_linear.right = velocity_right; odometry.velocity_angular = (velocity_right - velocity_left) / odometry.baseline; @@ -1143,13 +1159,13 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if (time_now - time_last_motors > FMW_DEBOUNCE_DELAY) { time_last_motors = time_now; if (motors.left.active && motors.right.active) { - fmw_motors_disable(motors.values, ARRLENGTH(motors.values)); + fmw_motors_deinit(); HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_RESET); fmw_buzzers_set(&buzzer, 1, false); } else { FMW_ASSERT(!motors.left.active); FMW_ASSERT(!motors.right.active); - fmw_motors_enable(motors.values, ARRLENGTH(motors.values)); + fmw_motors_init(); HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_SET); fmw_buzzers_set(&buzzer, 1, false); } @@ -1157,7 +1173,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { } break; case fault1_Pin: case fault2_Pin: { - fmw_motors_stop(motors.values, ARRLENGTH(motors.values)); + fmw_motors_stop(); FMW_Message response = {0}; response.header.type = FMW_MessageType_Response; response.response.result = FMW_Result_Error_FaultPinTriggered; diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/messages/EmergencyMode.msg b/pioneer_workstation_ws/src/pioneer3dx_controller/messages/EmergencyMode.msg new file mode 100644 index 0000000..8d9b079 --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/messages/EmergencyMode.msg @@ -0,0 +1 @@ +uint8 enable \ No newline at end of file diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/services/EmergencyMode.srv b/pioneer_workstation_ws/src/pioneer3dx_controller/services/EmergencyMode.srv new file mode 100644 index 0000000..f4c980e --- /dev/null +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/services/EmergencyMode.srv @@ -0,0 +1,3 @@ +uint8 enable +--- +uint8 status_response \ No newline at end of file -- 2.52.0