#ifndef FMW_CORE_H
#define FMW_CORE_H
-typedef struct {
+typedef struct FMW_Encoder {
TIM_HandleTypeDef *const timer;
uint32_t previous_millis;
uint32_t current_millis;
float wheel_circumference; // NOTE(lb): measured in meters
} FMW_Encoder;
-typedef struct {
+typedef struct FMW_Motor {
GPIO_TypeDef *const sleep_gpio_port;
GPIO_TypeDef *const dir_gpio_port;
TIM_HandleTypeDef *const pwm_timer;
uint32_t timer_channel;
} FMW_Buzzer;
-typedef void fmw_interrupt(void *user_data);
+typedef struct FMW_Hook {
+ void (*callback)(void *args);
+ void *args;
+} FMW_Hook;
-void fmw_motor_init(FMW_Motor motors[], int32_t count) __attribute__((nonnull));
-void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) __attribute__((nonnull));
-void fmw_motor_brake(FMW_Motor *motor) __attribute__((nonnull));
-void fmw_motor_enable(FMW_Motor * motor) __attribute__((nonnull));
-void fmw_motor_disable(FMW_Motor * motor) __attribute__((nonnull));
+void fmw_motor_init(FMW_Motor motors[], int32_t count) __attribute__((nonnull));
+void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) __attribute__((nonnull));
+void fmw_motor_brake(FMW_Motor motors[], int32_t count) __attribute__((nonnull));
+void fmw_motor_enable(FMW_Motor * motor) __attribute__((nonnull));
+void fmw_motor_disable(FMW_Motor * motor) __attribute__((nonnull));
void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull));
-FMW_Result fmw_encoder_update(FMW_Encoder *encoder) __attribute__((nonnull));
+void fmw_encoder_update(FMW_Encoder *encoder) __attribute__((nonnull));
float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) __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));
-int32_t fmw_pid_update(FMW_PidController *pid, float measure) __attribute__((warn_unused_result, nonnull));
-
-void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry,
- float linear, float angular) __attribute__((nonnull));
-
-void fmw_led_init(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__((warn_unused_result, nonnull));
-FMW_Result fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull));
+void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) __attribute__((nonnull));
-void fmw_result_log_uart(UART_HandleTypeDef *huart, FMW_Result result,
- const char *filename, int16_t filename_length,
- int32_t line) __attribute__((nonnull));
+void fmw_led_init(FMW_Led *led) __attribute__((nonnull));
+void fmw_led_update(FMW_Led *led) __attribute__((nonnull));
-FMW_Result fmw_message_receive_uart(UART_HandleTypeDef *huart,
- int32_t wait_ms, FMW_Message *msg) __attribute__((warn_unused_result, nonnull));
+void fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull));
-int32_t fmw_result_format(char buffer[], size_t buffer_size,
- const char *filename, int16_t filename_length,
- int32_t line, FMW_Result result) __attribute__((warn_unused_result, nonnull));
+FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) __attribute__((warn_unused_result, nonnull));
+void fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, FMW_Message *msg, int32_t wait_ms) __attribute__((nonnull));
#define FMW_LED_UPDATE_PERIOD 200
#define FMW_DEBOUNCE_DELAY 200
#define FMW_METERS_FROM_TICKS(Ticks, WheelCircumference, TicksPerRevolution) \
((Ticks * WheelCircumference) / TicksPerRevolution)
-#define FMW_RESULT_LOG_UART(HUART_PTR, FMW_RESULT) \
- fmw_result_log_uart((HUART_PTR), (FMW_RESULT), __FILE__, ARRLENGTH(__FILE__), __LINE__)
+#define FMW_ASSERT(Cond, ...) \
+ do { \
+ FMW_Hook hook = { __VA_ARGS__ }; \
+ if (!(Cond)) { \
+ if (hook.callback) { \
+ hook.callback(hook.args); \
+ } \
+ __builtin_trap(); \
+ } \
+ } while (0)
#endif
float values[3];
} FMW_PidConstants;
-typedef uint16_t FMW_MessageStatusCode;
-enum {
- FMW_MessageStatusCode_None,
-
- FMW_MessageStatusCode_Waiting4Config = 0,
- FMW_MessageStatusCode_Running = 1,
- FMW_MessageStatusCode_Error_Config = 2,
- FMW_MessageStatusCode_Error_Velocity = 3,
- FMW_MessageStatusCode_Fault_HBridge = 4,
-
- FMW_MessageStatusCode_COUNT,
-};
-
typedef uint8_t FMW_State;
enum {
FMW_State_None,
FMW_State_Init,
- FMW_State_Error,
FMW_State_Running,
FMW_State_COUNT,
FMW_MessageType_None,
FMW_MessageType_Error,
- FMW_MessageType_Run,
+ FMW_MessageType_Response,
+
+ FMW_MessageType_StateChange_Init,
+ FMW_MessageType_StateChange_Run,
+
FMW_MessageType_Config_Robot,
FMW_MessageType_Config_PID,
FMW_MessageType_Config_LED,
- FMW_MessageType_Status,
- FMW_MessageType_Velocity,
+
+ FMW_MessageType_Run_GetStatus,
+ FMW_MessageType_Run_GetStatus_Response,
+ FMW_MessageType_Run_SetVelocity,
+ FMW_MessageType_Run_SetVelocity_Response,
FMW_MessageType_COUNT,
};
union {
struct {
- const char *filename;
int32_t line;
- int32_t filename_size;
- FMW_Result reason;
+ int32_t file_size;
} error;
struct {
uint32_t update_period;
} config_led;
+ FMW_Result result;
struct {
- FMW_MessageStatusCode status_code;
+ FMW_Result result;
uint16_t delta_millis;
- int32_t left_ticks;
- int32_t right_ticks;
- } status;
+ int32_t ticks_left;
+ int32_t ticks_right;
+ } status_response;
struct {
float linear;
float angular;
#include <string.h>
-FMW_Result fmw_message_receive_uart(UART_HandleTypeDef *huart, int32_t wait_ms, FMW_Message *msg) {
- assert(wait_ms >= 0);
+static struct {
+ FMW_Motor *motors;
+ int32_t motors_count;
+} fmw_state;
+
+static void fmw_hook_assert_fail(void *_) {
+ if (fmw_state.motors != NULL) {
+ fmw_motor_brake(fmw_state.motors, fmw_state.motors_count);
+ }
+}
+
+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);
return FMW_Result_Ok;
}
-int32_t fmw_result_format(char buffer[], size_t buffer_size,
- const char *filename, int16_t filename_length,
- int32_t line, FMW_Result result) {
- static const char* const variant_strings[] = {
- #define X(Variant) #Variant,
- FMW_RESULT_VARIANTS(X)
- #undef X
- };
- return snprintf(buffer, buffer_size,
- "[%.*s:%ld] failed with result: %s\r\n",
- filename_length, filename, line,
- variant_strings[result]);
-}
-
-void fmw_result_log_uart(UART_HandleTypeDef *huart, FMW_Result result,
- const char *filename, int16_t filename_length,
- int32_t line) {
- char buff[512] = {0};
- int32_t length = fmw_result_format(buff, sizeof buff, filename, filename_length, line, result);
- (void)HAL_UART_Transmit(huart, (uint8_t*)buff, length, HAL_MAX_DELAY);
+void fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, FMW_Message *msg, int32_t wait_ms) {
+ msg->header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)msg, sizeof *msg);
+ HAL_StatusTypeDef res = HAL_UART_Transmit(huart, (uint8_t*)msg, sizeof *msg, wait_ms);
+ FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail);
}
// ============================================================
// Motor controller
void fmw_motor_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) {
- assert(motors[i].sleep_gpio_port);
- assert(motors[i].dir_gpio_port);
- assert(motors[i].pwm_timer);
- 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);
- 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);
- 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);
- assert(motors[i].dir_pin != motors[i].sleep_pin);
+ FMW_ASSERT(motors[i].sleep_gpio_port);
+ FMW_ASSERT(motors[i].dir_gpio_port);
+ FMW_ASSERT(motors[i].pwm_timer);
+ 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);
- assert(status == HAL_OK);
+ FMW_ASSERT(status == HAL_OK);
motors[i].max_dutycycle = motors[i].pwm_timer->Instance->ARR;
motors[i].active = true;
- fmw_motor_brake(&motors[i]);
}
+ fmw_motor_brake(motors, count);
}
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 >= 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_motor_brake(FMW_Motor *motor) {
- HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, FMW_MotorDirection_Backward);
- __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, 0);
+void fmw_motor_brake(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, FMW_MotorDirection_Backward);
+ __HAL_TIM_SET_COMPARE(motors[i].pwm_timer, motors[i].pwm_channel, 0);
+ }
}
void fmw_motor_enable(FMW_Motor *motor) {
- HAL_TIM_PWM_Start(motor->pwm_timer, motor->pwm_channel);
+ HAL_StatusTypeDef res = HAL_TIM_PWM_Start(motor->pwm_timer, motor->pwm_channel);
+ FMW_ASSERT(res == HAL_OK);
motor->active = true;
}
void fmw_motor_disable(FMW_Motor *motor) {
- HAL_TIM_PWM_Stop(motor->pwm_timer, motor->pwm_channel);
+ HAL_StatusTypeDef res = HAL_TIM_PWM_Stop(motor->pwm_timer, motor->pwm_channel);
+ FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail);
motor->active = false;
}
// Encoder
void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) {
for (int32_t i = 0; i < count; ++i) {
- assert(encoders[i].timer != NULL);
- assert(encoders[i].ticks_per_revolution > 0);
- assert(encoders[i].wheel_circumference > 0.f);
+ 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);
- assert(status == HAL_OK);
+ FMW_ASSERT(status == HAL_OK);
fmw_encoder_count_reset(&encoders[i]);
encoders[i].current_millis = HAL_GetTick();
}
}
-FMW_Result fmw_encoder_update(FMW_Encoder *encoder) {
+void fmw_encoder_update(FMW_Encoder *encoder) {
encoder->previous_millis = encoder->current_millis;
encoder->current_millis = HAL_GetTick();
encoder->ticks = fmw_encoder_count_get(encoder);
fmw_encoder_count_reset(encoder);
- if (!(__builtin_expect(encoder->current_millis >= encoder->previous_millis, true))) {
- return FMW_Result_Error_Encoder_GetTick;
- } else {
- return FMW_Result_Ok;
- }
+ FMW_ASSERT(encoder->current_millis >= encoder->previous_millis, .callback = fmw_hook_assert_fail);
}
float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) {
float deltatime = encoder->current_millis - encoder->previous_millis;
- if (!(__builtin_expect(deltatime > 0.f, true))) {
- return -1.f;
- } else {
- float meters = FMW_METERS_FROM_TICKS(encoder->ticks,
- encoder->wheel_circumference,
- encoder->ticks_per_revolution);
- assert(meters >= 0.f);
- float linear_velocity = meters / (deltatime / 1000.f);
- assert(linear_velocity >= 0.f);
- return linear_velocity;
- }
+ FMW_ASSERT(deltatime > 0.f, .callback = fmw_hook_assert_fail);
+ float meters = FMW_METERS_FROM_TICKS(encoder->ticks,
+ encoder->wheel_circumference,
+ encoder->ticks_per_revolution);
+ FMW_ASSERT(meters >= 0.f, .callback = fmw_hook_assert_fail);
+ float linear_velocity = meters / (deltatime / 1000.f);
+ FMW_ASSERT(linear_velocity >= 0.f, .callback = fmw_hook_assert_fail);
+ return linear_velocity;
}
void fmw_encoder_count_reset(FMW_Encoder *encoder) {
- assert(encoder->timer);
+ FMW_ASSERT(encoder->timer, .callback = fmw_hook_assert_fail);
__HAL_TIM_SET_COUNTER(encoder->timer, (encoder->timer->Init.Period / 2));
}
int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) {
- assert(encoder->timer);
+ FMW_ASSERT(encoder->timer, .callback = fmw_hook_assert_fail);
return (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2);
}
// ============================================================
// Odometry
void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) {
+ FMW_ASSERT(odometry->baseline > 0.f, .callback = fmw_hook_assert_fail);
odometry->setpoint_left = linear - (odometry->baseline * angular) / 2;
odometry->setpoint_right = linear + (odometry->baseline * angular) / 2;
}
// ============================================================
// PID
-int32_t fmw_pid_update(FMW_PidController *pid, float measure) {
- pid->error = pid->setpoint - measure;
-
+int32_t fmw_pid_update(FMW_PidController *pid, float velocity) {
+ pid->error = pid->setpoint - velocity;
float output = pid->error * pid->ks.proportional;
- //integral term without windup
+ // integral term without windup
pid->error_sum += pid->error;
output += pid->error_sum * pid->ks.integral;
// ============================================================
// LEDs
void fmw_led_init(FMW_Led *led) {
- assert(led->timer);
- assert(led->adc);
- 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);
- assert(led->voltage_red > 0.f);
- assert(led->voltage_orange > 0.f);
- assert(led->voltage_hysteresis >= 0.f);
- assert(led->state < FMW_LedState_COUNT);
+ FMW_ASSERT(led->timer);
+ FMW_ASSERT(led->adc);
+ 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);
HAL_StatusTypeDef status = HAL_TIM_PWM_Start(led->timer, led->timer_channel);
- assert(status == HAL_OK);
+ FMW_ASSERT(status == HAL_OK);
__HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, 0);
}
void fmw_led_update(FMW_Led *led) {
- HAL_ADC_Start(led->adc);
- HAL_ADC_PollForConversion(led->adc, HAL_MAX_DELAY);
+ FMW_ASSERT(led->timer && led->adc, .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);
+
+ HAL_StatusTypeDef adc_start_res = HAL_ADC_Start(led->adc);
+ FMW_ASSERT(adc_start_res == HAL_OK, .callback = fmw_hook_assert_fail);
+ 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);
+
uint32_t adc_val = HAL_ADC_GetValue(led->adc);
float v_adc = ((float)adc_val / FMW_ADC_RESOLUTION) * FMW_V_REF;
float vin = v_adc * FMW_VIN_SCALE_FACTOR;
// ============================================================
// Buzzers
// NOTE(lb): replace bool with uint8_t bitmask?
-FMW_Result fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) {
- if (count < 1) { return FMW_Result_Error_InvalidArguments; }
+void fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) {
+ FMW_ASSERT(count >= 0, .callback = fmw_hook_assert_fail);
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);
}
- if (res != HAL_OK) { return FMW_Result_Error_Buzzer_Timer; }
+ FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail);
}
- return FMW_Result_Ok;
}
\r
/* Private typedef -----------------------------------------------------------*/\r
/* USER CODE BEGIN PTD */\r
-FMW_Result message_handler(FMW_Message *msg)\r
+FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc)\r
__attribute__((warn_unused_result, nonnull));\r
/* USER CODE END PTD */\r
\r
/* USER CODE BEGIN 4 */\r
void start(void) {\r
for (FMW_Message msg = {0}; fmw_state == FMW_State_Init; ) {\r
- FMW_Result res = fmw_message_receive_uart(UART_MESSANGER_HANDLE, 180 * 1000, &msg);\r
- if (res != FMW_Result_Ok) {\r
- FMW_RESULT_LOG_UART(UART_MESSANGER_HANDLE, res);\r
- } else {\r
- res = message_handler(&msg);\r
- if (res != FMW_Result_Ok) { FMW_RESULT_LOG_UART(UART_MESSANGER_HANDLE, res); }\r
- }\r
+ FMW_Result res = fmw_message_uart_receive(UART_MESSANGER_HANDLE, &msg, 180 * 1000);\r
+ FMW_Message response = {0};\r
+ response.header.type = FMW_MessageType_Response;\r
+ response.result = (res == FMW_Result_Ok) ? message_handler(&msg, &hcrc) : res;\r
+ fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY);\r
}\r
\r
fmw_encoder_init(encoders.values, ENCODER_COUNT);\r
// Right and left motors have the same parameters\r
pid_max = (int32_t)htim4.Instance->ARR;\r
pid_min = -pid_max;\r
- assert(pid_max > pid_min);\r
+ FMW_ASSERT(pid_max > pid_min);\r
\r
// Enables TIM6 interrupt (used for PID control)\r
HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6);\r
- assert(timer_status == HAL_OK);\r
+ FMW_ASSERT(timer_status == HAL_OK);\r
\r
// Enables UART RX interrupt\r
HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&run_msg, sizeof run_msg);\r
}\r
}\r
\r
-FMW_Result message_handler(FMW_Message *msg) {\r
+FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) {\r
// NOTE(lb): the `msg->header.crc != -1` checks are just because i haven't\r
// implemented CRC into the program that sends these messages.\r
// i also don't know if the code to calculate CRC is correct (probably isn't).\r
if (msg->header.crc != -1) {\r
uint32_t crc_received = msg->header.crc;\r
msg->header.crc = 0;\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof *msg);\r
+ uint32_t crc_computed = HAL_CRC_Calculate(hcrc, (uint32_t*)msg, sizeof *msg);\r
if (!(crc_computed == crc_received)) { return FMW_Result_Error_UART_Crc; }\r
}\r
\r
switch (fmw_state) {\r
case FMW_State_Init: {\r
switch (msg->header.type) {\r
- case FMW_MessageType_Run: {\r
+ case FMW_MessageType_StateChange_Run: {\r
fmw_state = FMW_State_Running;\r
} break;\r
case FMW_MessageType_Config_Robot: {\r
pled.voltage_hysteresis = msg->config_led.voltage_hysteresis;\r
led_update_period = msg->config_led.update_period;\r
} break;\r
- case FMW_MessageType_Status: // NOTE(lb): allow status messages in init mode?\r
- case FMW_MessageType_Velocity: {\r
+ case FMW_MessageType_Run_GetStatus: // NOTE(lb): allow status messages in init mode?\r
+ case FMW_MessageType_Run_SetVelocity: {\r
return FMW_Result_Error_Command_NotAvailable;\r
} break;\r
default: {\r
} break;\r
case FMW_State_Running: {\r
switch (msg->header.type) {\r
- case FMW_MessageType_Status: { // TODO(lb): this should be `GetStatus` or something like that.\r
+ case FMW_MessageType_Run_GetStatus: {\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
ticks_left = ticks_right = 0;\r
float time_millis_delta = time_millis_current - time_millis_previous;\r
time_millis_previous = time_millis_current;\r
\r
- // NOTE(lb): Does a status response need to be its own message or\r
- // is just logging fine? Is the workstation program interactive\r
- // (is there a user choosing which messages to send)\r
- // or is automated? And if it is automated wouldn't\r
- // it want to know if the board actually received its message?\r
- // If the build process wasn't so overcomplicated i could just add\r
- // a compilation flag and switch between both at comptime.\r
-\r
- char buffer[128] = {0};\r
- int32_t buffer_size = snprintf(buffer, ARRLENGTH(buffer), "time_millis_delta : %f\n"\r
- "ticks_left : %ld\n"\r
- "ticks_right : %ld\n",\r
- time_millis_delta, current_ticks_left, current_ticks_right);\r
- (void)HAL_UART_Transmit(UART_MESSANGER_HANDLE, (uint8_t*)buffer, buffer_size, HAL_MAX_DELAY);\r
+ FMW_Message msg = {0};\r
+ msg.header.type = FMW_MessageType_Run_GetStatus_Response;\r
+ msg.status_response.delta_millis = time_millis_delta;\r
+ msg.status_response.ticks_left = current_ticks_left;\r
+ msg.status_response.ticks_right = current_ticks_right;\r
+ msg.header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)&msg, sizeof msg);\r
+\r
+ (void)HAL_UART_Transmit(UART_MESSANGER_HANDLE, (uint8_t*)&msg, sizeof msg, HAL_MAX_DELAY);\r
} break;\r
- case FMW_MessageType_Velocity: {\r
+ case FMW_MessageType_Run_SetVelocity: {\r
fmw_odometry_setpoint_from_velocities(&odometry, msg->velocity.linear, msg->velocity.angular);\r
pid_left.setpoint = odometry.setpoint_left;\r
pid_right.setpoint = odometry.setpoint_right;\r
pid_cross.setpoint = odometry.setpoint_left - odometry.setpoint_right;\r
} break;\r
- case FMW_MessageType_Run:\r
+ case FMW_MessageType_StateChange_Run:\r
case FMW_MessageType_Config_Robot:\r
case FMW_MessageType_Config_PID:\r
case FMW_MessageType_Config_LED: {\r
{ // PID control\r
fmw_encoder_update(&encoders.left);\r
float velocity_left = fmw_encoder_get_linear_velocity(&encoders.left);\r
- int dutycycle_left = fmw_pid_update(&pid_left, velocity_left);\r
+ int32_t dutycycle_left = fmw_pid_update(&pid_left, velocity_left);\r
\r
fmw_encoder_update(&encoders.right);\r
float velocity_right = fmw_encoder_get_linear_velocity(&encoders.right);\r
- int dutycycle_right = fmw_pid_update(&pid_right, velocity_right);\r
- int dutycycle_cross = fmw_pid_update(&pid_cross, velocity_left - velocity_right);\r
+ int32_t dutycycle_right = fmw_pid_update(&pid_right, velocity_right);\r
+ int32_t dutycycle_cross = fmw_pid_update(&pid_cross, velocity_left - velocity_right);\r
\r
dutycycle_left += dutycycle_cross;\r
dutycycle_right -= dutycycle_cross;\r
\r
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {\r
if (huart == UART_MESSANGER_HANDLE) {\r
- // NOTE(lb): i don't think another interrupt of this kind can occur\r
- // while i'm still handling the previous one so casting away\r
- // volatile should be fine.\r
- FMW_Result res = message_handler((FMW_Message*)&run_msg);\r
- if (res != FMW_Result_Ok) { FMW_RESULT_LOG_UART(huart, res); }\r
+ FMW_Message response = {0};\r
+ response.header.type = FMW_MessageType_Response;\r
+ response.result = message_handler((FMW_Message*)&run_msg, &hcrc);\r
+ fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY);\r
\r
// NOTE(lb): listen for the next message.\r
HAL_UART_Receive_DMA(huart, (uint8_t*)&run_msg, sizeof run_msg);\r
/* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */\r
// NOTE(lb): lol strlen\r
} else {\r
- // NOTE(lb): is it safe to assert here since the motors aren't running?\r
- assert(!motors.left.active);\r
- assert(!motors.right.active);\r
+ // NOTE(lb): is it safe to FMW_ASSERT here since the motors aren't running?\r
+ FMW_ASSERT(!motors.left.active);\r
+ FMW_ASSERT(!motors.right.active);\r
\r
fmw_motor_enable(&motors.left);\r
fmw_motor_enable(&motors.right);\r
} break;\r
case fault1_Pin:\r
case fault2_Pin: {\r
- fmw_motor_brake(&motors.left);\r
- fmw_motor_brake(&motors.right);\r
+ fmw_motor_brake(motors.values, ARRLENGTH(motors.values));\r
// stop TIM6 interrupt (used for PID control)\r
HAL_TIM_Base_Stop_IT(&htim6);\r
/* otto_status = MessageStatusCode_Fault_HBridge; */\r
{\r
/* USER CODE BEGIN Error_Handler_Debug */\r
/* User can add his own implementation to report the HAL error return state */\r
-\r
/* USER CODE END Error_Handler_Debug */\r
}\r
#ifdef USE_FULL_ASSERT\r
// TODO(lb): implement CRC
static int serial_open(char *portname) {
- int fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC);
+ int fd = open(portname, O_RDWR | O_NOCTTY);
assert(fd >= 0);
- tcflush(fd, TCIOFLUSH);
struct termios tty;
- assert(!tcgetattr(fd, &tty));
- cfsetospeed(&tty, B115200); // output baud rate
- cfsetispeed(&tty, B115200); // input baud rate
- tty.c_cflag = (tty.c_cflag & (tcflag_t)~CSIZE) | CS8; // 8-bit chars
- tty.c_iflag &= (tcflag_t)~IGNBRK; // disable break processing
- tty.c_cc[VMIN] = 0; // read doesn't block
- tty.c_cc[VTIME] = 20; // 2.0 seconds read timeout
- tty.c_cflag &= (tcflag_t)~(PARENB | PARODD); // no parity
- tty.c_cflag &= (tcflag_t)~CSTOPB; // 1 stop bit
- assert(!tcsetattr(fd, TCSANOW, &tty));
+ tcgetattr(fd, &tty);
+
+ cfsetispeed(&tty, B115200);
+ cfsetospeed(&tty, B115200);
+
+ tty.c_cflag &= (tcflag_t)~PARENB;
+ tty.c_cflag &= (tcflag_t)~CSTOPB;
+ tty.c_cflag &= (tcflag_t)~CRTSCTS;
+ tty.c_cflag |= CS8 | CLOCAL | CREAD;
+
+ tty.c_lflag = 0;
+ tty.c_iflag = 0;
+ tty.c_oflag = 0;
+
+ tty.c_cc[VMIN] = 1;
+ tty.c_cc[VTIME] = 0;
+
+ tcflush(fd, TCIFLUSH);
+ tcsetattr(fd, TCSANOW, &tty);
+
return fd;
}
int fd = serial_open("/dev/ttyACM0");
assert(fd);
-#if 1
+ FMW_Message response = {0};
+
FMW_Message msg_config_robot = {
.header = {
.type = FMW_MessageType_Config_Robot,
},
.config_robot = {
.baseline = 2.3f,
- .left_wheel_circumference = 4.f,
- .left_ticks_per_revolution = 250,
- .right_wheel_circumference = 3.2f,
- .right_ticks_per_revolution = 350,
+ .wheel_circumference_left = 4.f,
+ .wheel_circumference_right = 3.2f,
+ .ticks_per_revolution_left = 250,
+ .ticks_per_revolution_right = 350,
},
};
write(fd, &msg_config_robot, sizeof(FMW_Message));
+ for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) {
+ ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read);
+ if (n >= 0) { bytes_read += (uint32_t)n; }
+ }
+ assert(response.header.type == FMW_MessageType_Response);
+ assert(response.result == FMW_Result_Ok);
FMW_Message msg_config_pid = {
.header = {
},
};
write(fd, &msg_config_pid, sizeof(FMW_Message));
+ for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) {
+ ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read);
+ if (n >= 0) { bytes_read += (uint32_t)n; }
+ }
+ assert(response.header.type == FMW_MessageType_Response);
+ assert(response.result == FMW_Result_Ok);
FMW_Message msg_run = {
.header = {
- .type = FMW_MessageType_Run,
+ .type = FMW_MessageType_StateChange_Run,
.crc = (uint32_t)-1,
},
};
write(fd, &msg_run, sizeof(FMW_Message));
-#else
- FMW_Message msgs[3] = {
- {
- .header = {
- .type = FMW_MessageType_Config_Robot,
- .crc = (uint32_t)-1,
- },
- .config_robot = {
- .baseline = 2.3f,
- .left_wheel_circumference = 4.f,
- .left_ticks_per_revolution = 250,
- .right_wheel_circumference = 3.2f,
- .right_ticks_per_revolution = 350,
- },
- },
- {
- .header = {
- .type = FMW_MessageType_Config_PID,
- .crc = (uint32_t)-1,
- },
- .config_pid = {
- .left = {
- .proportional = 0.2f,
- .integral = 0.2f,
- .derivative = 0.2f,
- },
- .right = {
- .proportional = 0.3f,
- .integral = 0.3f,
- .derivative = 0.3f,
- },
- .cross = {
- .proportional = 0.4f,
- .integral = 0.4f,
- .derivative = 0.4f,
- },
- },
- },
- {
- .header = {
- .type = FMW_MessageType_Run,
- .crc = (uint32_t)-1,
- },
- },
- };
- write(fd, &msgs, 3 * sizeof(FMW_Message));
-#endif
+ for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) {
+ ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read);
+ if (n >= 0) { bytes_read += (uint32_t)n; }
+ }
+ assert(response.header.type == FMW_MessageType_Response);
+ assert(response.result == FMW_Result_Ok);
close(fd);
}
projects / pioneer-stm32 / commitdiff