void *args;
} FMW_Hook;
-void fmw_motor_init(FMW_Motor motors[], int32_t count) __attribute__((nonnull));
+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));
-void fmw_motor_brake(FMW_Motor motors[], int32_t count) __attribute__((nonnull));
-void fmw_motor_enable(FMW_Motor motors[], int32_t count) __attribute__((nonnull));
-void fmw_motor_disable(FMW_Motor motors[], int32_t count) __attribute__((nonnull));
-void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull));
+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_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));
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_deinit(FMW_Led *led) __attribute__((nonnull));
void fmw_led_update(FMW_Led *led) __attribute__((nonnull));
-void fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull));
+void fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((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));
+FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) __attribute__((warn_unused_result, nonnull));
+HAL_StatusTypeDef fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc,
+ FMW_Message *msg, int32_t wait_ms) __attribute__((nonnull, warn_unused_result));
FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart);
#define FMW_MESSAGE_TYPE_VARIANTS(X) \
X(FMW_MessageType_None) \
X(FMW_MessageType_Response) \
- X(FMW_MessageType_StateChange_Config) \
- X(FMW_MessageType_StateChange_Run) \
+ X(FMW_MessageType_ModeChange_Config) \
+ X(FMW_MessageType_ModeChange_Run) \
X(FMW_MessageType_Config_Robot) \
X(FMW_MessageType_Config_PID) \
X(FMW_MessageType_Config_LED) \
#define FMW_RESULT_VARIANTS(X) \
X(FMW_Result_Ok) \
X(FMW_Result_Error_InvalidArguments) \
+ X(FMW_Result_Error_FaultPinTriggered) \
X(FMW_Result_Error_UART_Crc) \
X(FMW_Result_Error_UART_NegativeTimeout) \
X(FMW_Result_Error_UART_ReceiveTimeoutElapsed) \
};
#pragma pack(push, 1)
-typedef struct {
+typedef struct FMW_Message {
struct {
FMW_MessageType type;
uint32_t crc;
} FMW_Message;
#pragma pack(pop)
+static_assert(sizeof(uint8_t) == 1);
+static_assert(sizeof(uint16_t) == 2);
+static_assert(sizeof(uint32_t) == 4);
+static_assert(sizeof(float) == 4);
+
#endif /* INC_COMMUNICATION_OTTO_MESSAGES_H_ */
static void fmw_hook_assert_fail(void *_) {
if (fmw_state.motors != NULL) {
- fmw_motor_brake(fmw_state.motors, fmw_state.motors_count);
+ fmw_motors_stop(fmw_state.motors, fmw_state.motors_count);
}
}
return FMW_Result_Ok;
}
-void fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, FMW_Message *msg, int32_t wait_ms) {
+HAL_StatusTypeDef 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);
+ return res;
}
FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) {
// ============================================================
// Motor controller
-void fmw_motor_init(FMW_Motor motors[], int32_t count) {
+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);
- FMW_ASSERT(motors[i].dir_gpio_port);
- FMW_ASSERT(motors[i].pwm_timer);
+ 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].max_dutycycle = motors[i].pwm_timer->Instance->ARR;
motors[i].active = true;
}
- fmw_motor_brake(motors, count);
+ fmw_motors_stop(motors, count);
+}
+
+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;
+ }
}
void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) {
HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET);
}
-void fmw_motor_brake(FMW_Motor motors[], int32_t count) {
+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);
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_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_motor_enable(FMW_Motor motors[], int32_t count) {
+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);
}
}
-void fmw_motor_disable(FMW_Motor motors[], int32_t count) {
+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);
// ============================================================
// Encoder
-void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) {
+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);
}
}
+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);
+ }
+}
+
void fmw_encoder_update(FMW_Encoder *encoder) {
encoder->previous_millis = encoder->current_millis;
encoder->current_millis = HAL_GetTick();
}
void fmw_encoder_count_reset(FMW_Encoder *encoder) {
- FMW_ASSERT(encoder->timer, .callback = fmw_hook_assert_fail);
+ FMW_ASSERT(encoder->timer != NULL, .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) {
- FMW_ASSERT(encoder->timer, .callback = fmw_hook_assert_fail);
+ FMW_ASSERT(encoder->timer != NULL, .callback = fmw_hook_assert_fail);
return (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2);
}
// ============================================================
// LEDs
void fmw_led_init(FMW_Led *led) {
- FMW_ASSERT(led->timer);
- FMW_ASSERT(led->adc);
+ 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 ||
__HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, 0);
}
+void fmw_led_deinit(FMW_Led *led) {
+ HAL_StatusTypeDef status = HAL_TIM_PWM_Stop(led->timer, led->timer_channel);
+ FMW_ASSERT(status == HAL_OK);
+}
+
void fmw_led_update(FMW_Led *led) {
- FMW_ASSERT(led->timer && led->adc, .callback = fmw_hook_assert_fail);
+ 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 ||
// ============================================================
// Buzzers
// NOTE(lb): replace bool with uint8_t bitmask?
-void fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) {
+void fmw_buzzers_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;
\r
/* Private define ------------------------------------------------------------*/\r
/* USER CODE BEGIN PD */\r
-#define MOTOR_COUNT 2\r
-#define ENCODER_COUNT 2\r
#define UART_MESSANGER_HANDLE (&huart3)\r
/* USER CODE END PD */\r
\r
\r
// TODO(lb): fill with sensible default\r
static union {\r
- FMW_Encoder values[ENCODER_COUNT];\r
+ FMW_Encoder values[2];\r
struct {\r
FMW_Encoder right;\r
FMW_Encoder left;\r
};\r
\r
static union {\r
- FMW_Motor values[MOTOR_COUNT];\r
+ FMW_Motor values[2];\r
struct {\r
FMW_Motor right;\r
FMW_Motor left;\r
void start(void) {\r
// Enables UART RX interrupt\r
HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer);\r
- for (; current_mode == FMW_Mode_Config; );\r
-\r
- fmw_encoder_init(encoders.values, ENCODER_COUNT);\r
- fmw_motor_init(motors.values, MOTOR_COUNT);\r
- fmw_led_init(&pled);\r
-\r
- // Right and left motors have the same parameters\r
- pid_max = (int32_t)htim4.Instance->ARR;\r
- pid_min = -pid_max;\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
- FMW_ASSERT(timer_status == HAL_OK);\r
\r
for (;;) {\r
switch (current_mode) {\r
switch (current_mode) {\r
case FMW_Mode_Config: {\r
switch (msg->header.type) {\r
- case FMW_MessageType_StateChange_Run: {\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_led_init(&pled);\r
+\r
+ // Right and left motors have the same parameters\r
+ pid_max = (int32_t)htim4.Instance->ARR;\r
+ pid_min = -pid_max;\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
+ FMW_ASSERT(timer_status == HAL_OK);\r
+\r
current_mode = FMW_Mode_Run;\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_Run_GetStatus: // NOTE(lb): allow status messages in config mode?\r
+ case FMW_MessageType_Run_GetStatus:\r
case FMW_MessageType_Run_SetVelocity: {\r
result = FMW_Result_Error_Command_NotAvailable;\r
goto msg_contains_error;\r
(void)HAL_UART_Transmit(UART_MESSANGER_HANDLE, (uint8_t*)&msg, sizeof msg, HAL_MAX_DELAY);\r
return FMW_Result_Ok;\r
} break;\r
- case FMW_MessageType_StateChange_Config: {\r
+ case FMW_MessageType_ModeChange_Config: {\r
+ fmw_motors_stop(motors.values, ARRLENGTH(motors.values));\r
fmw_encoder_count_reset(&encoders.left);\r
fmw_encoder_count_reset(&encoders.right);\r
- fmw_motor_brake(motors.values, ARRLENGTH(motors.values));\r
+\r
+ fmw_encoders_deinit(encoders.values, ARRLENGTH(encoders.values));\r
+ fmw_motors_deinit(motors.values, ARRLENGTH(motors.values));\r
+ fmw_led_deinit(&pled);\r
+\r
+ HAL_StatusTypeDef timer_status = HAL_TIM_Base_Stop_IT(&htim6);\r
+ FMW_ASSERT(timer_status == HAL_OK);\r
+\r
current_mode = FMW_Mode_Config;\r
} break;\r
- case FMW_MessageType_StateChange_Run:\r
+ case FMW_MessageType_ModeChange_Run:\r
case FMW_MessageType_Config_Robot:\r
case FMW_MessageType_Config_PID:\r
case FMW_MessageType_Config_LED: {\r
FMW_Message response = {0};\r
response.header.type = FMW_MessageType_Response;\r
response.response.result = result;\r
- fmw_message_uart_send(UART_MESSANGER_HANDLE, hcrc, &response, 1);\r
+ HAL_StatusTypeDef send_res = fmw_message_uart_send(UART_MESSANGER_HANDLE, hcrc, &response, 1);\r
+ FMW_ASSERT(send_res == HAL_OK);\r
return result;\r
}\r
\r
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {\r
if (huart != UART_MESSANGER_HANDLE) { return; }\r
\r
- if (huart->RxState == HAL_UART_STATE_BUSY_RX) {\r
- FMW_Message response = fmw_message_from_uart_error(huart);\r
- fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY);\r
- HAL_UART_AbortReceive(huart);\r
- HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer);\r
- } else {\r
- fmw_motor_brake(motors.values, ARRLENGTH(motors.values));\r
- FMW_ASSERT(huart->gState == HAL_UART_STATE_BUSY_TX);\r
- FMW_ASSERT(false);\r
+ fmw_motors_stop(motors.values, ARRLENGTH(motors.values));\r
+ FMW_Message response = fmw_message_from_uart_error(huart);\r
+\r
+ retry:;\r
+ HAL_StatusTypeDef res = fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY);\r
+ if (res != HAL_OK) { // NOTE(lb): send help\r
+ fmw_buzzers_set(&buzzer, 1, true);\r
+ goto retry;\r
}\r
+\r
+ HAL_UART_AbortReceive(huart);\r
+ HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer);\r
}\r
\r
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {\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_motor_disable(motors.values, ARRLENGTH(motors.values));\r
+ fmw_motors_disable(motors.values, ARRLENGTH(motors.values));\r
HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_RESET);\r
- fmw_buzzer_set(&buzzer, 1, false);\r
+ fmw_buzzers_set(&buzzer, 1, false);\r
} else {\r
FMW_ASSERT(!motors.left.active);\r
FMW_ASSERT(!motors.right.active);\r
- fmw_motor_enable(motors.values, ARRLENGTH(motors.values));\r
+ fmw_motors_enable(motors.values, ARRLENGTH(motors.values));\r
HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_SET);\r
- fmw_buzzer_set(&buzzer, 1, false);\r
+ fmw_buzzers_set(&buzzer, 1, false);\r
}\r
}\r
} break;\r
case fault1_Pin:\r
case fault2_Pin: {\r
- fmw_motor_brake(motors.values, ARRLENGTH(motors.values));\r
- // stop TIM6 interrupt (used for PID control)\r
+ fmw_motors_stop(motors.values, ARRLENGTH(motors.values));\r
+ FMW_Message response = {0};\r
+ response.header.type = FMW_MessageType_Response;\r
+ response.response.result = FMW_Result_Error_FaultPinTriggered;\r
+\r
+ retry:;\r
+ HAL_StatusTypeDef res = fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY);\r
+ if (res != HAL_OK) { // NOTE(lb): send help\r
+ fmw_buzzers_set(&buzzer, 1, true);\r
+ goto retry;\r
+ }\r
+\r
HAL_TIM_Base_Stop_IT(&htim6);\r
- /* otto_status = MessageStatusCode_Fault_HBridge; */\r
} break;\r
}\r
}\r
tty.c_cc[VMIN] = 1;
tty.c_cc[VTIME] = 0;
- tcflush(fd, TCIFLUSH);
+ sleep(2); // NOTE(lb): required to make flush work, for some reason
+ tcflush(fd, TCIOFLUSH);
tcsetattr(fd, TCSANOW, &tty);
return fd;
FMW_Message response = {0};
+#if 0
+ FMW_Message msgs[] = {
+ {
+ .header = {
+ .type = FMW_MessageType_Config_Robot,
+ .crc = (uint32_t)-1,
+ },
+ .config_robot = {
+ .baseline = 2.3f,
+ .wheel_circumference_left = 4.f,
+ .wheel_circumference_right = 3.2f,
+ .ticks_per_revolution_left = 250,
+ .ticks_per_revolution_right = 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_Config_Robot,
+ .crc = (uint32_t)-1,
+ },
+ .config_robot = {
+ .baseline = 2.3f,
+ .wheel_circumference_left = 4.f,
+ .wheel_circumference_right = 3.2f,
+ .ticks_per_revolution_left = 250,
+ .ticks_per_revolution_right = 350,
+ },
+ },
+ };
+ write(fd, &msgs, sizeof(msgs));
+ for (int i = 0; i < 2; ++i) {
+ 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);
+ pprint_message(&response);
+ }
+#elif 0
+ FMW_Message msg_config_robot = {
+ .header = {
+ .type = FMW_MessageType_Config_Robot,
+ .crc = (uint32_t)-1,
+ },
+ .config_robot = {
+ .baseline = 2.3f,
+ .wheel_circumference_left = 4.f,
+ .wheel_circumference_right = 3.2f,
+ .ticks_per_revolution_left = 250,
+ .ticks_per_revolution_right = 350,
+ },
+ };
+ FMW_Message msg_config_pid = {
+ .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,
+ },
+ },
+ };
+
+ write(fd, &msg_config_robot, sizeof(FMW_Message));
+ write(fd, &msg_config_pid, sizeof(FMW_Message));
+ for (int i = 0; i < 2; ++i) {
+ 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);
+ pprint_message(&response);
+ }
+#else
FMW_Message msg_config_robot = {
.header = {
.type = FMW_MessageType_Config_Robot,
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);
printf("\tconfig:robot\n");
pprint_message(&response);
+ assert(response.header.type == FMW_MessageType_Response);
assert(response.response.result == FMW_Result_Ok);
FMW_Message msg_config_pid = {
FMW_Message msg_run = {
.header = {
- .type = FMW_MessageType_StateChange_Run,
+ .type = FMW_MessageType_ModeChange_Run,
.crc = (uint32_t)-1,
},
};
FMW_Message msg_init = {
.header = {
- .type = FMW_MessageType_StateChange_Config,
+ .type = FMW_MessageType_ModeChange_Config,
.crc = (uint32_t)-1,
},
};
assert(response.header.type == FMW_MessageType_Response);
printf("\tinit\n");
pprint_message(&response);
+#endif
close(fd);
}
projects / pioneer-stm32 / commitdiff