Skip to content

Extend tb3 to read pins A0-A5 #342

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Extend tb3 to read pins A0-A5 #342

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6d4b6cf
extend tb3 to read pins A0-A5
travis-mendoza Apr 16, 2025
dfee182
Initialize device_status to avoid segmentation fault in TB3 bringup
travis-mendoza May 15, 2025
3ab3444
Add device_ready flag
travis-mendoza May 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 58 additions & 4 deletions arduino/opencr_arduino/opencr/libraries/turtlebot3_ros2/src/turtlebot3/turtlebot3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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,

Expand Down Expand Up @@ -244,6 +245,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{
Expand All @@ -252,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;
Expand Down Expand Up @@ -312,6 +321,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;
Expand Down Expand Up @@ -349,6 +360,10 @@ void TurtleBot3Core::begin(const char* model_name)
DEBUG_PRINTLN("Version : V221004R1");
DEBUG_PRINTLN("Begin Start...");

// Initialize device_status
control_items.device_ready = false;
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.");
Expand Down Expand Up @@ -397,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);
Expand Down Expand Up @@ -452,6 +468,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;
Expand Down Expand Up @@ -562,9 +586,13 @@ void TurtleBot3Core::begin(const char* model_name)
sensors.initIMU();
sensors.calibrationGyro();

//To indicate that the initialization is complete.
sensors.makeMelody(1);
// 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

// Print a version message to Serial
DEBUG_PRINTLN("Running ANALOG-ENABLED firmware V20250410");
DEBUG_PRINTLN("Begin End...");
}

Expand Down Expand Up @@ -607,6 +635,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();
Expand Down Expand Up @@ -784,6 +813,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;
Expand Down Expand Up @@ -1056,4 +1090,24 @@ void test_motors_with_buttons(uint8_t buttons)
move[VelocityType::ANGULAR] = false;
}
}
}
}

/*******************************************************************************
* 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);
}
}