};
typedef struct {
- float baseline; // NOTE(lb): measured in meters
- float velocity_linear;
+ float baseline; // NOTE(lb): measured in meters
+ Vec2Float position; // NOTE(lb): measured in meters
+ Vec2Float orientation;
+ Vec2Float velocity_linear; // NOTE(lb): measured in m/s
float velocity_angular;
- float setpoint_right;
- float setpoint_left;
} FMW_Odometry;
typedef struct {
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_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));
-int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) __attribute__((warn_unused_result, 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_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));
-int32_t fmw_pid_update(FMW_PidController *pid, float velocity) __attribute__((warn_unused_result, nonnull));
+void fmw_odometry_pose_update(FMW_Odometry *odometry, float meters_traveled_left, float meters_traveled_right) __attribute__((nonnull));
-void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) __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_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));
+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 *msg, int32_t wait_ms) __attribute__((nonnull, warn_unused_result));
+FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) __attribute__((nonnull, warn_unused_result));
-FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart);
+Vec2Float fmw_setpoint_from_velocities(const FMW_Odometry *odometry, float linear, float angular) __attribute__((nonnull));
#define FMW_LED_UPDATE_PERIOD 200
#define FMW_DEBOUNCE_DELAY 200
float angular;
} run_set_velocity;
- // TODO(lb): add odometry information
struct {
int32_t ticks_left;
int32_t ticks_right;
+ float position_x;
+ float position_y;
+ float orientation_x;
+ float orientation_y;
+ float velocity_linear;
+ float velocity_angular;
uint16_t delta_millis;
FMW_Result result;
} response;
} 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);
+static_assert(sizeof(uint8_t) == 1);
+static_assert(sizeof(uint16_t) == 2);
+static_assert(sizeof(uint32_t) == 4);
+static_assert(sizeof(float) == 4);
+static_assert(sizeof(FMW_Message) == 41);
#endif /* INC_COMMUNICATION_OTTO_MESSAGES_H_ */
\r
#define ARRLENGTH(Arr) (sizeof((Arr)) / sizeof(*(Arr)))\r
\r
+typedef union Vec2Float Vec2Float;\r
+union Vec2Float {\r
+ float values[2];\r
+ struct { float x, y; };\r
+ struct { float left, right; };\r
+};\r
+\r
+typedef union Mat3Float Mat3Float;\r
+union Mat3Float {\r
+ float values[3][3];\r
+ float sequence[9];\r
+};\r
+\r
void start(void) __attribute__((noreturn));\r
+Mat3Float mat3float_multiply(Mat3Float lhs, Mat3Float rhs);\r
\r
/* USER CODE END EM */\r
\r
#include "firmware/fmw_inc.h"
#include <string.h>
+#include <math.h>
static struct {
FMW_Motor *motors;
return res;
}
+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) {
}
}
-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);
- FMW_ASSERT(encoder->current_millis >= encoder->previous_millis, .callback = fmw_hook_assert_fail);
+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);
+ }
}
-float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) {
+float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled) {
float deltatime = encoder->current_millis - encoder->previous_millis;
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);
+ float linear_velocity = meters_traveled / (deltatime / 1000.f);
return linear_velocity;
}
int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) {
FMW_ASSERT(encoder->timer != NULL, .callback = fmw_hook_assert_fail);
- return (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2);
+ int32_t res = (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2);
+ return res;
}
// ============================================================
// 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;
+void fmw_odometry_pose_update(FMW_Odometry *odometry, float meters_traveled_left, float meters_traveled_right) {
+ static Mat3Float robot_pose = {{{1.f, 0.f, 0.f}, {0.f, 1.f, 0.f}, {0.f, 0.f, 1.f}}};
+ static const float tolerance = 0.005f;
+
+ Mat3Float robot_robotranslation = {0};
+ Mat3Float robot_from_cir = {0};
+ Mat3Float cir_rotation = {0};
+ Mat3Float cir_from_robot = {0};
+
+ // calcolo dell'angolo di rotazione
+ float theta = -(meters_traveled_left - meters_traveled_right) / odometry->baseline;
+
+ // soglia di tolleranza -> controllo la direzione del robot se va dritto
+ if (ABS(theta) < tolerance) {
+ /* traslazione lungo x, calcolo della matrice di rototraslazione per traiettoria dritta */
+ robot_robotranslation.values[0][0] = 1; robot_robotranslation.values[0][1] = 0; robot_robotranslation.values[0][2] = (meters_traveled_left + meters_traveled_right) / 2;
+ robot_robotranslation.values[1][0] = 0; robot_robotranslation.values[1][1] = 1; robot_robotranslation.values[1][2] = 0;
+ robot_robotranslation.values[2][0] = 0; robot_robotranslation.values[2][1] = 0; robot_robotranslation.values[2][2] = 1;
+ } else {
+ // traiettoria lungo una curva
+ // distanza del centro del robot dal CIR
+ float d_cir = (meters_traveled_right / theta) - (odometry->baseline / 2.f);
+ // matrice cir_from_robot di traslazione al CIR
+ cir_from_robot.values[0][0] = 1; cir_from_robot.values[0][1] = 0; cir_from_robot.values[0][2] = 0;
+ cir_from_robot.values[1][0] = 0; cir_from_robot.values[1][1] = 1; cir_from_robot.values[1][2] = -d_cir;
+ cir_from_robot.values[2][0] = 0; cir_from_robot.values[2][1] = 0; cir_from_robot.values[2][2] = 1;
+ // matrice r di rotazione attorno al CIR
+ cir_rotation.values[0][0] = cosf(theta); cir_rotation.values[0][1] = -sinf(theta); cir_rotation.values[0][2] = 0;
+ cir_rotation.values[1][0] = sinf(theta); cir_rotation.values[1][1] = cosf(theta); cir_rotation.values[1][2] = 0;
+ cir_rotation.values[2][0] = 0; cir_rotation.values[2][1] = 0; cir_rotation.values[2][2] = 1;
+ // matrice robot_from_cir di ri-traslazione dal CIR
+ robot_from_cir.values[0][0] = 1; robot_from_cir.values[0][1] = 0; robot_from_cir.values[0][2] = 0;
+ robot_from_cir.values[1][0] = 0; robot_from_cir.values[1][1] = 1; robot_from_cir.values[1][2] = d_cir;
+ robot_from_cir.values[2][0] = 0; robot_from_cir.values[2][1] = 0; robot_from_cir.values[2][2] = 1;
+ // calcolo matrice di ROTOTRASLAZIONE corrente
+ robot_robotranslation = mat3float_multiply(robot_from_cir, mat3float_multiply(cir_rotation, cir_from_robot));
+ }
+
+ // calcolo (nuova) ROBOT_POSE
+ robot_pose = mat3float_multiply(robot_pose, robot_robotranslation);
+ odometry->position.x = robot_pose.values[0][2];
+ odometry->position.y = robot_pose.values[1][2];
+ odometry->orientation.x = robot_pose.values[0][0];
+ odometry->orientation.y = robot_pose.values[0][1];
}
// ============================================================
}\r
}\r
\r
+Mat3Float mat3float_multiply(Mat3Float lhs, Mat3Float rhs) {\r
+ Mat3Float output = {0};\r
+ output.values[0][0] = (lhs.values[0][0] * rhs.values[0][0]) + (lhs.values[0][1] * rhs.values[1][0]) + (lhs.values[0][2] * rhs.values[2][0]);\r
+ output.values[0][1] = (lhs.values[0][0] * rhs.values[0][1]) + (lhs.values[0][1] * rhs.values[1][1]) + (lhs.values[0][2] * rhs.values[2][1]);\r
+ output.values[0][2] = (lhs.values[0][0] * rhs.values[0][2]) + (lhs.values[0][1] * rhs.values[1][2]) + (lhs.values[0][2] * rhs.values[2][2]);\r
+ output.values[1][0] = (lhs.values[1][0] * rhs.values[0][0]) + (lhs.values[1][1] * rhs.values[1][0]) + (lhs.values[1][2] * rhs.values[2][0]);\r
+ output.values[1][1] = (lhs.values[1][0] * rhs.values[0][1]) + (lhs.values[1][1] * rhs.values[1][1]) + (lhs.values[1][2] * rhs.values[2][1]);\r
+ output.values[1][2] = (lhs.values[1][0] * rhs.values[0][2]) + (lhs.values[1][1] * rhs.values[1][2]) + (lhs.values[1][2] * rhs.values[2][2]);\r
+ output.values[2][0] = (lhs.values[2][0] * rhs.values[0][0]) + (lhs.values[2][1] * rhs.values[1][0]) + (lhs.values[2][2] * rhs.values[2][0]);\r
+ output.values[2][1] = (lhs.values[2][0] * rhs.values[0][1]) + (lhs.values[2][1] * rhs.values[1][1]) + (lhs.values[2][2] * rhs.values[2][1]);\r
+ output.values[2][2] = (lhs.values[2][0] * rhs.values[0][2]) + (lhs.values[2][1] * rhs.values[1][2]) + (lhs.values[2][2] * rhs.values[2][2]);\r
+ return output;\r
+}\r
+\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
case FMW_Mode_Run: {\r
switch (msg->header.type) {\r
case FMW_MessageType_Run_SetVelocity: {\r
- fmw_odometry_setpoint_from_velocities(&odometry, msg->run_set_velocity.linear, msg->run_set_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
+ Vec2Float setpoint = fmw_setpoint_from_velocities(&odometry, 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
} // fallthrough\r
case FMW_MessageType_Run_GetStatus: {\r
// NOTE(lb): SetVelocity continues here as well because the previous case doesn't end with a `break`.\r
msg.response.delta_millis = time_millis_delta;\r
msg.response.ticks_left = current_ticks_left;\r
msg.response.ticks_right = current_ticks_right;\r
+ msg.response.position_x = odometry.position.x;\r
+ msg.response.position_y = odometry.position.y;\r
+ msg.response.orientation_x = odometry.orientation.x;\r
+ msg.response.orientation_y = odometry.orientation.y;\r
+ msg.response.velocity_linear = (odometry.velocity_linear.left + odometry.velocity_linear.right) / 2.f;\r
+ msg.response.velocity_angular = odometry.velocity_angular;\r
msg.header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)&msg, sizeof msg);\r
(void)HAL_UART_Transmit(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, (uint8_t*)&msg, sizeof msg, HAL_MAX_DELAY);\r
return FMW_Result_Ok;\r
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {\r
if (htim != &htim6) { return; }\r
\r
- // NOTE(lb): metrics taken for transmission\r
- ticks_left += fmw_encoder_count_get(&encoders.left);\r
- ticks_right += fmw_encoder_count_get(&encoders.right);\r
-\r
{ // PID control\r
- fmw_encoder_update(&encoders.left);\r
- float velocity_left = fmw_encoder_get_linear_velocity(&encoders.left);\r
- int32_t dutycycle_left = fmw_pid_update(&pid_left, velocity_left);\r
+ fmw_encoders_update(encoders.values, ARRLENGTH(encoders.values));\r
+ ticks_left += encoders.left.ticks;\r
+ ticks_right += encoders.right.ticks;\r
\r
- fmw_encoder_update(&encoders.right);\r
- float velocity_right = fmw_encoder_get_linear_velocity(&encoders.right);\r
+ float meters_traveled_left = FMW_METERS_FROM_TICKS(encoders.left.ticks, encoders.left.wheel_circumference, encoders.left.ticks_per_revolution);\r
+ float meters_traveled_right = FMW_METERS_FROM_TICKS(encoders.right.ticks, encoders.right.wheel_circumference, encoders.right.ticks_per_revolution);\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
+ odometry.velocity_linear.left = velocity_left;\r
+ odometry.velocity_linear.right = velocity_right;\r
+ odometry.velocity_angular = (velocity_right - velocity_left) / odometry.baseline;\r
+\r
+ int32_t dutycycle_left = fmw_pid_update(&pid_left, velocity_left);\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
{
if (this->stm32_config_mode) { return; }
- FMW_Message response, msg_get_status;
+ FMW_Message msg_get_status;
msg_get_status.header.type = FMW_MessageType_Run_GetStatus;
msg_get_status.header.crc = (uint32_t)-1;
- this->serial_mutex.lock();
- {
- ssize_t bytes_written = ::write(this->serial_fd, &msg_get_status, sizeof(FMW_Message));
- assert(bytes_written != -1);
- assert(bytes_written == sizeof(msg_get_status));
- for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) {
- ssize_t n = ::read(this->serial_fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read);
- if (n >= 0) { bytes_read += (uint32_t)n; }
- assert(n != -1);
- }
- }
- this->serial_mutex.unlock();
+ FMW_Message response = this->stm32_message_send(&msg_get_status);
assert(response.header.type == FMW_MessageType_Response);
this->stm32_message_print(&response);
if (response.response.result == FMW_Result_Error_Command_NotAvailable) {
output.header.frame_id = this->get_parameter("frame_id_odometry").as_string();
output.header.stamp = rclcpp::Time(0, 0, this->get_clock()->now().get_clock_type());
- // output.pose.pose.position.x = ; // TODO(lb): fill with pioneer data
- // output.pose.pose.position.y = ; // TODO(lb): fill with pioneer data
- // output.pose.pose.orientation.x = ; // TODO(lb): fill with pioneer data
- // output.pose.pose.orientation.y = ; // TODO(lb): fill with pioneer data
+ output.pose.pose.position.x = response.response.position_x;
+ output.pose.pose.position.y = response.response.position_y;
+ output.pose.pose.orientation.x = response.response.orientation_x;
+ output.pose.pose.orientation.y = response.response.orientation_y;
- // output.twist.twist.linear.x = ; // TODO(lb): fill with pioneer data
- // output.twist.twist.linear.y = ; // TODO(lb): fill with pioneer data
- // output.twist.twist.angular.x = ; // TODO(lb): fill with pioneer data
- // output.twist.twist.angular.y = ; // TODO(lb): fill with pioneer data
-
- assert(output.pose.pose.position.z == 0);
- assert(output.pose.pose.orientation.z == 0);
- assert(output.pose.pose.orientation.w == 1);
- assert(output.twist.twist.linear.z == 0);
- assert(output.twist.twist.angular.z == 0);
+ output.twist.twist.linear.x = response.response.velocity_linear;
+ output.twist.twist.angular.x = response.response.velocity_angular;
this->publisher_odometry->publish(output);
}
void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd)
{
- (void)cmd;
+ if (this->stm32_config_mode) { return; }
+
+ FMW_Message msg;
+ ::memset(&msg, 0, sizeof(msg));
+ msg.header.type = FMW_MessageType_Run_SetVelocity;
+ msg.header.crc = (uint32_t)-1;
+ msg.run_set_velocity.linear = cmd->linear.x;
+ msg.run_set_velocity.angular = cmd->angular.x;
+
+ FMW_Message response_msg = this->stm32_message_send(&msg);
+ assert(response_msg.header.type == FMW_MessageType_Response);
+ assert(response_msg.response.result == FMW_Result_Ok);
}
void P3DX_Controller_Node::callback_service_command_change_mode_s(const std::shared_ptr<pioneer3dx_controller::srv::ChangeMode::Request> request,
case FMW_MessageType_Response: {
buffer_len += snprintf(buffer + buffer_len, sizeof(buffer) - buffer_len,
"\n response:"
- "\n result = %s"
- "\n delta_millis = %d"
- "\n ticks_left = %d"
- "\n ticks_right = %d",
+ "\n result = %s"
+ "\n delta_millis = %d"
+ "\n ticks_left = %d"
+ "\n ticks_right = %d"
+ "\n position_x = %f"
+ "\n position_y = %f"
+ "\n orientation_x = %f"
+ "\n orientation_y = %f"
+ "\n velocity_linear = %f"
+ "\n velocity_angular = %f",
result_types[msg->response.result], msg->response.delta_millis,
- msg->response.ticks_left, msg->response.ticks_right);
+ msg->response.ticks_left, msg->response.ticks_right,
+ msg->response.position_x, msg->response.position_y,
+ msg->response.orientation_x, msg->response.orientation_y,
+ msg->response.velocity_linear, msg->response.velocity_angular);
assert(buffer_len < (int32_t)sizeof(buffer));
} break;
default: {
float angular;
} run_set_velocity;
- // TODO(lb): add odometry information
struct {
int32_t ticks_left;
int32_t ticks_right;
+ float position_x;
+ float position_y;
+ float orientation_x;
+ float orientation_y;
+ float velocity_linear;
+ float velocity_angular;
uint16_t delta_millis;
FMW_Result result;
} response;
} 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);
+static_assert(sizeof(uint8_t) == 1);
+static_assert(sizeof(uint16_t) == 2);
+static_assert(sizeof(uint32_t) == 4);
+static_assert(sizeof(float) == 4);
+static_assert(sizeof(FMW_Message) == 41);
#endif /* INC_COMMUNICATION_OTTO_MESSAGES_H_ */
projects / pioneer-stm32 / commitdiff