From 2e76d35987a5c062094c7c9b119fb0c5734f08ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Mon, 18 Nov 2024 18:25:54 +0100 Subject: [PATCH] Apply fixes from leocore version of firmware (#6) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański Co-authored-by: Błażej Sowa --- include/configuration.hpp | 14 ++++------ platformio.ini | 2 +- src/main.cpp | 57 ++++++++++++++++++++++----------------- 3 files changed, 39 insertions(+), 34 deletions(-) diff --git a/include/configuration.hpp b/include/configuration.hpp index 5dc589b..88b984b 100644 --- a/include/configuration.hpp +++ b/include/configuration.hpp @@ -18,7 +18,7 @@ constexpr const char* ROS_NODE_NAME = "firmware"; constexpr const char* ROS_NAMESPACE = ""; // Number of encoder readings to remember when estimating the wheel velocity -constexpr uint32_t ENCODER_BUFFER_SIZE = 10; +constexpr uint32_t VELOCITY_ROLLING_WINDOW_SIZE = 10; // The period (in number of calls to the update() function) at which the battery // voltage is probed @@ -78,25 +78,21 @@ constexpr diff_drive_lib::RobotConfiguration ROBOT_CONFIG = { .wheel_FL_conf = { .motor = MotC, - .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY, - .velocity_rolling_window_size = ENCODER_BUFFER_SIZE, + .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY }, .wheel_RL_conf = { .motor = MotD, - .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY, - .velocity_rolling_window_size = ENCODER_BUFFER_SIZE, + .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY }, .wheel_FR_conf = { .motor = MotA, - .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY, - .velocity_rolling_window_size = ENCODER_BUFFER_SIZE, + .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY }, .wheel_RR_conf = { .motor = MotB, - .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY, - .velocity_rolling_window_size = ENCODER_BUFFER_SIZE, + .op_mode = diff_drive_lib::WheelOperationMode::VELOCITY }, }; \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 7b24abf..653829a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -15,7 +15,7 @@ board_microros_distro = humble-fictionlab board_microros_transport = custom board_microros_user_meta = colcon.meta lib_deps = - https://github.com/fictionlab/diff_drive_lib.git#1.6 + https://github.com/fictionlab/diff_drive_lib.git#2.0 https://github.com/fictionlab/micro_ros_platformio#fictionlab-v3 https://github.com/byq77/encoder-mbed.git#dfac32c https://github.com/byq77/drv88xx-driver-mbed.git#61854e8 diff --git a/src/main.cpp b/src/main.cpp index 15146f6..ae92ce2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -46,9 +46,8 @@ static std_msgs__msg__Float32 battery_averaged; static rcl_publisher_t battery_pub; static rcl_publisher_t battery_averaged_pub; static analogin_t battery_adc; -static float battery_buffer_memory[BATTERY_BUFFER_SIZE]; -static diff_drive_lib::CircularBuffer battery_buffer( - BATTERY_BUFFER_SIZE, battery_buffer_memory); +static diff_drive_lib::CircularBuffer + battery_buffer; static std::atomic_bool publish_battery(false); static leo_msgs__msg__WheelOdom wheel_odom; @@ -74,7 +73,7 @@ static rcl_publisher_t param_trigger_pub; static std::atomic_bool publish_param_trigger(true); static bool mecanum_wheels = false; -static std::atomic_bool controller_replacement(false); +static std::atomic_bool controller_initialized(false); static uint32_t reset_pointer_position; @@ -127,9 +126,9 @@ MotorController MotC(MOT_C_CONFIG); MotorController MotD(MOT_D_CONFIG); static uint8_t - controller_buffer[std::max(sizeof(diff_drive_lib::DiffDriveController), - sizeof(diff_drive_lib::MecanumController))]; -static diff_drive_lib::RobotController* controller; + controller_buffer[std::max(sizeof(diff_drive_lib::DiffDriveController), + sizeof(diff_drive_lib::MecanumController))]; +static diff_drive_lib::RobotController* controller; static mbed::I2C i2c_(SENS2_PIN4, SENS2_PIN3); static ImuReceiver imu_receiver(i2c_); @@ -140,14 +139,17 @@ static std::atomic_bool reload_parameters(false); static void cmdVelCallback(const void* msgin) { const geometry_msgs__msg__Twist* msg = (const geometry_msgs__msg__Twist*)msgin; - controller->setSpeed(msg->linear.x, msg->linear.y, msg->angular.z); + if (controller_initialized) + controller->setSpeed(msg->linear.x, msg->linear.y, msg->angular.z); } static void resetOdometryCallback(const void* reqin, void* resin) { std_srvs__srv__Trigger_Response* res = (std_srvs__srv__Trigger_Response*)resin; - controller->resetOdom(); - res->success = true; + if (controller_initialized) { + controller->resetOdom(); + res->success = true; + } } static void resetBoardCallback(const void* reqin, void* resin) { @@ -175,18 +177,22 @@ static void getBoardTypeCallback(const void* reqin, void* resin) { static void wheelCmdPWMDutyCallback(const void* msgin, void* context) { const std_msgs__msg__Float32* msg = (std_msgs__msg__Float32*)msgin; - diff_drive_lib::WheelController* wheel = - (diff_drive_lib::WheelController*)context; - wheel->disable(); - wheel->motor.setPWMDutyCycle(msg->data); + auto wheel = static_cast< + diff_drive_lib::WheelController*>(context); + if (controller_initialized) { + wheel->disable(); + wheel->motor.setPWMDutyCycle(msg->data); + } } static void wheelCmdVelCallback(const void* msgin, void* context) { const std_msgs__msg__Float32* msg = (std_msgs__msg__Float32*)msgin; - diff_drive_lib::WheelController* wheel = - (diff_drive_lib::WheelController*)context; - wheel->enable(); - wheel->setTargetVelocity(msg->data); + auto wheel = static_cast< + diff_drive_lib::WheelController*>(context); + if (controller_initialized) { + wheel->enable(); + wheel->setTargetVelocity(msg->data); + } } static void bootFirmwareCallback(const void* reqin, void* resin) { @@ -411,19 +417,23 @@ void initController() { &wheel_odom_mecanum_pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(leo_msgs, msg, WheelOdomMecanum), "~/wheel_odom_mecanum"); - controller = - new (controller_buffer) diff_drive_lib::MecanumController(ROBOT_CONFIG); + controller = new (controller_buffer) + diff_drive_lib::MecanumController( + ROBOT_CONFIG); } else { rclc_publisher_init_best_effort( &wheel_odom_pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(leo_msgs, msg, WheelOdom), "~/wheel_odom"); controller = new (controller_buffer) - diff_drive_lib::DiffDriveController(ROBOT_CONFIG); + diff_drive_lib::DiffDriveController( + ROBOT_CONFIG); } controller->init(params); + controller_initialized = true; } void finiController() { + controller_initialized = false; if (mecanum_wheels) { (void)!rcl_publisher_fini(&wheel_odom_mecanum_pub, &node); } else { @@ -515,10 +525,8 @@ static void loop() { if (reload_parameters.exchange(false)) { params.update(¶m_server); if (params.mecanum_wheels != mecanum_wheels) { - controller_replacement = true; finiController(); initController(); - controller_replacement = false; } else { controller->updateParams(params); } @@ -526,6 +534,7 @@ static void loop() { break; case AgentStatus::AGENT_LOST: controller->disable(); + finiController(); finiROS(); imu_receiver.stop(); status = AgentStatus::CONNECTING_TO_AGENT; @@ -615,7 +624,7 @@ static void update() { } } - if (status != AgentStatus::AGENT_CONNECTED || controller_replacement) return; + if (status != AgentStatus::AGENT_CONNECTED || !controller_initialized) return; controller->update(UPDATE_PERIOD);