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;
} 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));
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
#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
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;
.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;
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) {
// 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};
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();
}
}
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(); }
}
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);
}
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;
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;
}
// ============================================================
// ============================================================
// 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;
}
__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) {
} 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;
}
/* USER CODE BEGIN PTD */\r
void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) __attribute__((nonnull));\r
\r
+static __attribute__((always_inline)) inline Vec2Float setpoint_from_velocities(float baseline, float linear, float angular) {\r
+ Vec2Float res = {\r
+ .left = linear - (baseline * angular) / 2.f,\r
+ .right = linear + (baseline * angular) / 2.f,\r
+ };\r
+ return res;\r
+}\r
+\r
void emergency_mode_begin(void);\r
void emergency_mode_end(void);\r
/* USER CODE END PTD */\r
.handler = message_handler,\r
},\r
};\r
- fmw_init(&fmw_info);\r
+ FMW_ASSERT(fmw_init(&fmw_info) == FMW_Result_Ok);\r
\r
for (;;) {\r
switch (fmw_mode_current()) {\r
uint32_t time_now = HAL_GetTick();\r
if (time_now - time_last_led_update >= led_update_period) {\r
time_last_led_update = time_now;\r
- fmw_led_update(&pled);\r
+ FMW_ASSERT(fmw_led_update(&pled) == FMW_Result_Ok);\r
}\r
} break;\r
}\r
case FMW_Mode_Config: {\r
switch (msg->header.type) {\r
case FMW_MessageType_ModeChange_Run: {\r
- fmw_encoders_init(encoders.values, ARRLENGTH(encoders.values));\r
- fmw_motors_init(motors.values, ARRLENGTH(motors.values));\r
+ fmw_encoders_init();\r
+ fmw_motors_init();\r
fmw_led_init(&pled);\r
\r
// Right and left motors have the same parameters\r
case FMW_Mode_Run: {\r
switch (msg->header.type) {\r
case FMW_MessageType_Run_SetVelocity: {\r
- Vec2Float setpoint = fmw_setpoint_from_velocities(&odometry, msg->run_set_velocity.linear, msg->run_set_velocity.angular);\r
+ Vec2Float setpoint = setpoint_from_velocities(odometry.baseline, msg->run_set_velocity.linear, msg->run_set_velocity.angular);\r
pid_left.setpoint = setpoint.left;\r
pid_right.setpoint = setpoint.right;\r
pid_cross.setpoint = setpoint.left - setpoint.right;\r
// NOTE(lb): SetVelocity continues here as well because the previous case doesn't end with a `break`.\r
// order matters.\r
\r
- int32_t current_ticks_left = ticks_left + fmw_encoder_count_get(&encoders.left);\r
- int32_t current_ticks_right = ticks_right + fmw_encoder_count_get(&encoders.right);\r
+ int32_t ticks_measured_left = 0;\r
+ int32_t ticks_measured_right = 0;\r
+ fmw_encoder_count_get(&encoders.left, &ticks_measured_left);\r
+ fmw_encoder_count_get(&encoders.right, &ticks_measured_right);\r
+\r
+ int32_t current_ticks_left = ticks_left + ticks_measured_left;\r
+ int32_t current_ticks_right = ticks_right + ticks_measured_right;\r
ticks_left = ticks_right = 0;\r
\r
static float time_millis_previous = 0.f;\r
// so they don't continue down to `msg_contains_error`.\r
} break;\r
case FMW_MessageType_ModeChange_Config: {\r
- fmw_motors_stop(motors.values, ARRLENGTH(motors.values));\r
+ fmw_motors_stop();\r
fmw_encoder_count_reset(&encoders.left);\r
fmw_encoder_count_reset(&encoders.right);\r
\r
- fmw_encoders_deinit(encoders.values, ARRLENGTH(encoders.values));\r
- fmw_motors_deinit(motors.values, ARRLENGTH(motors.values));\r
+ fmw_encoders_deinit();\r
+ fmw_motors_deinit();\r
fmw_led_deinit(&pled);\r
\r
HAL_StatusTypeDef timer_status = HAL_TIM_Base_Stop_IT(&htim6);\r
if (htim == &htim7) { // TIMER 1Hz no message exchange check\r
fmw_emergency_timer_update();\r
} else if (htim == &htim6) { // TIMER 100Hz PID control\r
- fmw_encoders_update(encoders.values, ARRLENGTH(encoders.values));\r
+ fmw_encoders_update();\r
ticks_left += encoders.left.ticks;\r
ticks_right += encoders.right.ticks;\r
\r
\r
fmw_odometry_pose_update(&odometry, meters_traveled_left, meters_traveled_right);\r
\r
- float velocity_left = fmw_encoder_get_linear_velocity(&encoders.left, meters_traveled_left);\r
- float velocity_right = fmw_encoder_get_linear_velocity(&encoders.right, meters_traveled_right);\r
+ float velocity_left = 0.f;\r
+ float velocity_right = 0.f;\r
+ fmw_encoder_get_linear_velocity(&encoders.left, meters_traveled_left, &velocity_left);\r
+ fmw_encoder_get_linear_velocity(&encoders.right, meters_traveled_right, &velocity_right);\r
+\r
odometry.velocity_linear.left = velocity_left;\r
odometry.velocity_linear.right = velocity_right;\r
odometry.velocity_angular = (velocity_right - velocity_left) / odometry.baseline;\r
if (time_now - time_last_motors > FMW_DEBOUNCE_DELAY) {\r
time_last_motors = time_now;\r
if (motors.left.active && motors.right.active) {\r
- fmw_motors_disable(motors.values, ARRLENGTH(motors.values));\r
+ fmw_motors_deinit();\r
HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_RESET);\r
fmw_buzzers_set(&buzzer, 1, false);\r
} else {\r
FMW_ASSERT(!motors.left.active);\r
FMW_ASSERT(!motors.right.active);\r
- fmw_motors_enable(motors.values, ARRLENGTH(motors.values));\r
+ fmw_motors_init();\r
HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_SET);\r
fmw_buzzers_set(&buzzer, 1, false);\r
}\r
} break;\r
case fault1_Pin:\r
case fault2_Pin: {\r
- fmw_motors_stop(motors.values, ARRLENGTH(motors.values));\r
+ fmw_motors_stop();\r
FMW_Message response = {0};\r
response.header.type = FMW_MessageType_Response;\r
response.response.result = FMW_Result_Error_FaultPinTriggered;\r
projects / pioneer-stm32 / commitdiff