From 6d4b6cf3627f73fe9a0e322a9cc2d509c4d17b77 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Wed, 16 Apr 2025 12:00:50 -0700 Subject: [PATCH 1/3] extend tb3 to read pins A0-A5 --- .../src/turtlebot3/turtlebot3.cpp | 48 +++++++++++++++++-- 1 file changed, 44 insertions(+), 4 deletions(-) diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp index a1ae26d5..22c7bfb1 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp @@ -119,7 +119,7 @@ static void update_motor_status(uint32_t interval_ms); static void update_battery_status(uint32_t interval_ms); static void update_analog_sensors(uint32_t interval_ms); static void update_joint_status(uint32_t interval_ms); - +static void update_analog_pins(uint32_t interval_ms); DYNAMIXEL::USBSerialPortHandler port_dxl_slave(SERIAL_DXL_SLAVE); DYNAMIXEL::Slave dxl_slave(port_dxl_slave, MODEL_NUM_DXL_SLAVE); @@ -244,6 +244,13 @@ enum ControlTableItemAddr{ ADDR_GOAL_CURRENT_WR_GRIPPER = 343, ADDR_GOAL_CURRENT_RD = 344, + ADDR_ANALOG_A0 = 350, + ADDR_ANALOG_A1 = 352, + ADDR_ANALOG_A2 = 354, + ADDR_ANALOG_A3 = 356, + ADDR_ANALOG_A4 = 358, + ADDR_ANALOG_A5 = 360, + }; typedef struct ControlItemVariables{ @@ -312,6 +319,8 @@ typedef struct ControlItemVariables{ bool joint_goal_current_wr_gripper; bool joint_goal_current_rd; + uint16_t analog_pins[6]; // For A0-A5 + }ControlItemVariables; static ControlItemVariables control_items; @@ -452,6 +461,14 @@ void TurtleBot3Core::begin(const char* model_name) dxl_slave.addControlItem(ADDR_PROFILE_ACC_L, control_items.profile_acceleration[MortorLocation::LEFT]); dxl_slave.addControlItem(ADDR_PROFILE_ACC_R, control_items.profile_acceleration[MortorLocation::RIGHT]); + // Items for Analog pins + dxl_slave.addControlItem(ADDR_ANALOG_A0, control_items.analog_pins[0]); + dxl_slave.addControlItem(ADDR_ANALOG_A1, control_items.analog_pins[1]); + dxl_slave.addControlItem(ADDR_ANALOG_A2, control_items.analog_pins[2]); + dxl_slave.addControlItem(ADDR_ANALOG_A3, control_items.analog_pins[3]); + dxl_slave.addControlItem(ADDR_ANALOG_A4, control_items.analog_pins[4]); + dxl_slave.addControlItem(ADDR_ANALOG_A5, control_items.analog_pins[5]); + if (p_tb3_model_info->has_manipulator == true) { control_items.joint_goal_position_wr_joint = false; control_items.joint_goal_position_wr_gripper = false; @@ -562,9 +579,11 @@ void TurtleBot3Core::begin(const char* model_name) sensors.initIMU(); sensors.calibrationGyro(); - //To indicate that the initialization is complete. - sensors.makeMelody(1); + // To indicate that the initialization is complete. + sensors.makeMelody(3); // To indicate that we are running modified firmware + // Print a version message to Serial + DEBUG_PRINTLN("Running ANALOG-ENABLED firmware V20250410"); DEBUG_PRINTLN("Begin End..."); } @@ -607,6 +626,7 @@ void TurtleBot3Core::run() update_battery_status(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); update_analog_sensors(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); update_joint_status(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); + update_analog_pins(INTERVAL_MS_TO_UPDATE_CONTROL_ITEM); // Packet processing with ROS2 Node. dxl_slave.processPacket(); @@ -1056,4 +1076,24 @@ void test_motors_with_buttons(uint8_t buttons) move[VelocityType::ANGULAR] = false; } } -} \ No newline at end of file +} + +/******************************************************************************* +* Function definition to update analog pin values +*******************************************************************************/ +void update_analog_pins(uint32_t interval_ms) +{ + static uint32_t pre_time = 0; + + if(millis() - pre_time >= interval_ms){ + pre_time = millis(); + + // Read all analog pins + control_items.analog_pins[0] = analogRead(A0); + control_items.analog_pins[1] = analogRead(A1); + control_items.analog_pins[2] = analogRead(A2); + control_items.analog_pins[3] = analogRead(A3); + control_items.analog_pins[4] = analogRead(A4); + control_items.analog_pins[5] = analogRead(A5); + } +} From dfee182a70a34a54d8a652d551dfc6c7d95aacae Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Thu, 15 May 2025 14:00:21 -0700 Subject: [PATCH 2/3] Initialize device_status to avoid segmentation fault in TB3 bringup --- .../turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp index 22c7bfb1..a25da953 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp @@ -358,6 +358,9 @@ void TurtleBot3Core::begin(const char* model_name) DEBUG_PRINTLN("Version : V221004R1"); DEBUG_PRINTLN("Begin Start..."); + // Initialize device_status + control_items.device_status = STATUS_NOT_CONNECTED_MOTORS; + // Setting for Dynamixel motors ret = motor_driver.init(); DEBUG_PRINTLN(ret==true?"Motor driver setup completed.":"Motor driver setup failed."); @@ -804,6 +807,11 @@ static void dxl_slave_write_callback_func(uint16_t item_addr, uint8_t &dxl_err_c switch(item_addr) { + case ADDR_DEVICE_STATUS: + // Prevent direct writing to device_status from outside + dxl_err_code = DXL_ERR_ACCESS; + break; + case ADDR_MODEL_INFORM: control_items.model_inform = p_tb3_model_info->model_info; dxl_err_code = DXL_ERR_ACCESS; From 3ab34449f772ae2caeae35d37eb8a897638d5892 Mon Sep 17 00:00:00 2001 From: Travis Mendoza Date: Tue, 20 May 2025 12:28:24 -0700 Subject: [PATCH 3/3] Add device_ready flag --- .../libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp index a25da953..81469a18 100644 --- a/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp +++ b/arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp @@ -133,6 +133,7 @@ enum ControlTableItemAddr{ ADDR_CONNECT_ROS2 = 15, ADDR_CONNECT_MANIP = 16, + ADDR_DEVICE_READY = 17, // Explicit readiness flag ADDR_DEVICE_STATUS = 18, ADDR_HEARTBEAT = 19, @@ -259,6 +260,7 @@ typedef struct ControlItemVariables{ uint32_t dev_time_millis; uint32_t dev_time_micros; + bool device_ready; // Device ready flag int8_t device_status; uint8_t heart_beat; bool debug_mode; @@ -359,6 +361,7 @@ void TurtleBot3Core::begin(const char* model_name) DEBUG_PRINTLN("Begin Start..."); // Initialize device_status + control_items.device_ready = false; control_items.device_status = STATUS_NOT_CONNECTED_MOTORS; // Setting for Dynamixel motors @@ -409,6 +412,7 @@ void TurtleBot3Core::begin(const char* model_name) dxl_slave.addControlItem(ADDR_CONNECT_MANIP, control_items.is_connect_manipulator); // Items to inform device status + dxl_slave.addControlItem(ADDR_DEVICE_READY, control_items.device_ready); dxl_slave.addControlItem(ADDR_DEVICE_STATUS, control_items.device_status); // Items to check connection state with node dxl_slave.addControlItem(ADDR_HEARTBEAT, control_items.heart_beat); @@ -582,6 +586,8 @@ void TurtleBot3Core::begin(const char* model_name) sensors.initIMU(); sensors.calibrationGyro(); + // Mark device as ready after all initialization is complete + control_items.device_ready = true; // To indicate that the initialization is complete. sensors.makeMelody(3); // To indicate that we are running modified firmware