Skip to content

Refactor Talon HWI: per-joint MotorConfig and revolute joint fix #61

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

Merged
IshanDutta11 merged 2 commits into from
Feb 17, 2026
Merged

Refactor Talon HWI: per-joint MotorConfig and revolute joint fix #61

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
70cea31
Refactor Talon HWI: replace hardcoded Pololu constants with per-joint…
dwarakeshbaraneetharan Feb 11, 2026
ff4e041
Merge branch 'umdloop:main' into talon-fixes
dwarakeshbaraneetharan Feb 16, 2026
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
12 changes: 11 additions & 1 deletion src/description/ros2_control/arm/arm.talon.ros2_control.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,17 @@
<joint name="gripper_claw">
<param name="node_id">2</param>
<param name="max_disp">0.06</param> <!-- meters -->
<param name="joint_type">prismatic</param> <!-- revolute or prismatic -->
<param name="joint_type">prismatic</param>
<!-- Motor config (Pololu 37D defaults) -->
<param name="encoder_resolution">64.0</param>
<param name="gear_ratio">102.08</param>
<param name="distance_per_rev">0.0015</param> <!-- m/rev -->
<param name="inverted">true</param>
<param name="sensor_phase">true</param>
<param name="kP">0.25</param>
<param name="kI">0.0</param>
<param name="kD">0.0</param>
<param name="kF">0.0</param>
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
Expand Down
24 changes: 21 additions & 3 deletions src/description/ros2_control/science/science.talon.ros2_control.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,16 @@

<joint name="talon_lift">
<param name="node_id">1</param>
<param name="joint_type">revolute</param> <!-- revolute or prismatic -->
<param name="joint_type">revolute</param>
<!-- Motor config (Pololu 37D defaults) -->
<param name="encoder_resolution">64.0</param>
<param name="gear_ratio">102.08</param>
<param name="inverted">true</param>
<param name="sensor_phase">true</param>
<param name="kP">0.25</param>
<param name="kI">0.0</param>
<param name="kD">0.0</param>
<param name="kF">0.0</param>
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
Expand All @@ -21,12 +30,21 @@

<joint name="talon_scoop">
<param name="node_id">2</param>
<param name="joint_type">revolute</param> <!-- revolute or prismatic -->
<param name="joint_type">revolute</param>
<!-- Motor config (Pololu 37D defaults) -->
<param name="encoder_resolution">64.0</param>
<param name="gear_ratio">102.08</param>
<param name="inverted">true</param>
<param name="sensor_phase">true</param>
<param name="kP">0.25</param>
<param name="kI">0.0</param>
<param name="kD">0.0</param>
<param name="kF">0.0</param>
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
</xacro:macro>
</robot>
</robot>
211 changes: 94 additions & 117 deletions src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,172 +3,149 @@
#include <cstdlib>
#include <chrono>
#include <cmath>
#include <string>

#define Phoenix_No_WPI // remove WPI dependencies
#include "ctre/Phoenix.h"
#include "ctre/phoenix/unmanaged/Unmanaged.h"

#define ENCODER_RESOLUTION 64.0 // 14-bit position resolution (Absolute RS485 Encoder)
#define GEAR_RATIO 102.08
#define MAX_RPM 500
#define QUADRATURE_ENCODING 4
#define MEASURED_CPR 5500 // idk why but this is what seems to be the correct cpr (dont use)
#include "talon_ros2_control/talon_control.h"

#define DISTANCE_PER_REV 0.0015 // in m/rev, 1.5 mm per turn
#define LENGTH_RANGE 6.0 // in inches (because I hate you)
// ── Motor initialization ─────────────────────────────────────────────────────

/* Set up motor config */
void initMotor(TalonSRX *motor) {
std::system("cansend can0 123#00000000");
void initMotor(TalonSRX * motor, const MotorConfig & config, const std::string & can_interface) {
// Wake up the CAN bus (send a dummy frame so the kernel driver is active)
std::string cmd = "cansend " + can_interface + " 123#00000000";
std::system(cmd.c_str());

motor->ConfigFactoryDefault(100);
motor->SetInverted(true);
motor->ConfigFactoryDefault(config.config_timeout_ms);
motor->SetInverted(config.inverted);

motor->ConfigSelectedFeedbackSensor(ctre::phoenix::motorcontrol::FeedbackDevice::QuadEncoder, 0, 100);
motor->SetSensorPhase(true);
motor->ConfigSelectedFeedbackSensor(
ctre::phoenix::motorcontrol::FeedbackDevice::QuadEncoder,
0, config.config_timeout_ms);
motor->SetSensorPhase(config.sensor_phase);

motor->Config_kP(0, 0.25, 100);
motor->Config_kD(0, 0.0, 100);
motor->Config_kF(0, 0.0, 100);
motor->Config_kP(0, config.kP, config.config_timeout_ms);
motor->Config_kI(0, config.kI, config.config_timeout_ms);
motor->Config_kD(0, config.kD, config.config_timeout_ms);
motor->Config_kF(0, config.kF, config.config_timeout_ms);
}

// double convertDegreestoTalonSRXUnits(float degrees) {
// float outputRotationPerDegree = (1.0 / 360.0);
// float motorRotationsPerOutputRotation = (GEAR_RATIO / 1.0); // gearbox
// float countsPerMotorRotation = (ENCODER_RESOLUTION / 1.0); // cpr of the encoder
// return degrees * outputRotationPerDegree * motorRotationsPerOutputRotation * countsPerMotorRotation;
// }
// ── Conversion helpers ───────────────────────────────────────────────────────

// double convertTalonSRXUnitsToDegrees(float nativeSensorUnits) {
// float motorRotationPerCount = (1.0 / ENCODER_RESOLUTION); // cpr of the encoder
// float outputRotationPerMotorRotation = (1.0 / GEAR_RATIO); // gearbox
// float degreesPerOutputRotation = (360.0 / 1.0);
// return nativeSensorUnits * motorRotationPerCount * outputRotationPerMotorRotation * degreesPerOutputRotation;
// }

double convertRevtoTalonUnits(float rev) {
// float counts_per_revolution = ENCODER_RESOLUTION*GEAR_RATIO*QUADRATURE_ENCODING;
float counts_per_revolution = ENCODER_RESOLUTION*GEAR_RATIO;
return rev * counts_per_revolution;
// return rev * MEASURED_CPR;
double convertRevtoTalonUnits(double rev, const MotorConfig & config) {
double counts_per_revolution = config.encoder_resolution * config.gear_ratio;
return rev * counts_per_revolution;
}

double convertTalonUnitstoRev(float counts) {
// float counts_per_revolution = ENCODER_RESOLUTION*GEAR_RATIO*QUADRATURE_ENCODING;
float counts_per_revolution = ENCODER_RESOLUTION*GEAR_RATIO;
return counts / counts_per_revolution;
// return counts / MEASURED_CPR;
double convertTalonUnitstoRev(double counts, const MotorConfig & config) {
double counts_per_revolution = config.encoder_resolution * config.gear_ratio;
return counts / counts_per_revolution;
}

double convertRevToDistance(float rev) {
return rev * DISTANCE_PER_REV;
double convertRevToDistance(double rev, const MotorConfig & config) {
return rev * config.distance_per_rev;
}

double convertDistanceToRev(float distance) {
return distance / DISTANCE_PER_REV;
double convertDistanceToRev(double distance, const MotorConfig & config) {
return distance / config.distance_per_rev;
}

// void setDegrees(TalonSRX *motor, double degrees) {
// motor.Set(ControlMode::Position, convertRevtoTalonUnits(degrees));
// }
// ── Position getters ─────────────────────────────────────────────────────────

float getPositionTalonUnits(TalonSRX *motor) {
return motor->GetSelectedSensorPosition(0);
float getPositionTalonUnits(TalonSRX * motor) {
return motor->GetSelectedSensorPosition(0);
}

float getPositionRevolutions(TalonSRX *motor) {
// return motor.GetSelectedSensorPosition(0);
float revs = convertTalonUnitstoRev(motor->GetSelectedSensorPosition(0)); // measures in Talon Units -> revolutions
return revs;
float getPositionRevolutions(TalonSRX * motor, const MotorConfig & config) {
return static_cast<float>(
convertTalonUnitstoRev(motor->GetSelectedSensorPosition(0), config));
}

float getPositionDistance(TalonSRX *motor) {
// return motor.GetSelectedSensorPosition(0);
float dist = getPositionRevolutions(motor) * DISTANCE_PER_REV;
return dist;
float getPositionDistance(TalonSRX * motor, const MotorConfig & config) {
// Output revolutions -> meters (for prismatic joints)
return getPositionRevolutions(motor, config) * static_cast<float>(config.distance_per_rev);
}

float getVelocityRPM(TalonSRX *motor) {
float hundred_ms_to_minute = motor->GetSelectedSensorVelocity(0)*600; // measures Talon units per 100ms -> units per minute
return convertTalonUnitstoRev(hundred_ms_to_minute); // units per minute -> rev per minute (RPM)
float getPositionRadians(TalonSRX * motor, const MotorConfig & config) {
// Output revolutions -> radians (for revolute joints)
return getPositionRevolutions(motor, config) * static_cast<float>(2.0 * M_PI);
}

float getClawVelocity(TalonSRX *motor) {
float hundred_ms_to_seconds = motor->GetSelectedSensorVelocity(0)*10; // measures Talon units per 100ms -> units per second
float units_to_rev = convertTalonUnitstoRev(hundred_ms_to_seconds); // units per second -> rev/sec
float m_per_sec = units_to_rev * DISTANCE_PER_REV; // rev/sec to m/s
return m_per_sec;
}
// ── Velocity getters ─────────────────────────────────────────────────────────

void setDutyCycle(TalonSRX *motor, double dutyCycle, int ms) {
motor->Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, dutyCycle);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds
float getVelocityRPM(TalonSRX * motor, const MotorConfig & config) {
// TalonSRX reports velocity in encoder units per 100 ms
float units_per_minute = motor->GetSelectedSensorVelocity(0) * 600.0f;
return static_cast<float>(convertTalonUnitstoRev(units_per_minute, config));
}

float setVelocityFromLinearVelocity(TalonSRX *motor, double claw_vel, int ms) {
// float desired_rpm = joy_input * MAX_RPM;
// float desired_rev_per_100ms = desired_rpm / 600; // 600 100ms to 1 minute
float getLinearVelocity(TalonSRX * motor, const MotorConfig & config) {
// Talon units/100ms -> units/s -> rev/s -> m/s
float units_per_second = motor->GetSelectedSensorVelocity(0) * 10.0f;
float rev_per_second = static_cast<float>(convertTalonUnitstoRev(units_per_second, config));
return rev_per_second * static_cast<float>(config.distance_per_rev);
}

//TO DO: why does the 5 fix this. I dont have a clue rn
float desired_talon_units_per_100ms = convertRevtoTalonUnits(convertDistanceToRev((claw_vel*5)/10)); // m/s -> m/100ms -> rev/100ms -> talon/100ms
float getAngularVelocity(TalonSRX * motor, const MotorConfig & config) {
// Talon units/100ms -> units/s -> rev/s -> rad/s
float units_per_second = motor->GetSelectedSensorVelocity(0) * 10.0f;
float rev_per_second = static_cast<float>(convertTalonUnitstoRev(units_per_second, config));
return rev_per_second * static_cast<float>(2.0 * M_PI);
}

motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, desired_talon_units_per_100ms);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds
// ── Motor command setters ────────────────────────────────────────────────────

return desired_talon_units_per_100ms;
void setDutyCycle(TalonSRX * motor, double dutyCycle, int ms) {
motor->Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, dutyCycle);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms);
}

float setVelocityFromAngularVelocity(TalonSRX *motor, double joint_velocity, int ms) {
float desired_talon_units_per_100ms = convertRevtoTalonUnits(joint_velocity / (2*M_PI*10)); // rad/s -> rad/100ms -> rev/100ms -> talon/100ms
float setVelocityFromLinearVelocity(TalonSRX * motor, double vel_m_per_s, int ms, const MotorConfig & config) {
// m/s -> m/100ms -> rev/100ms -> talon units/100ms
double vel_m_per_100ms = vel_m_per_s / 10.0;
double rev_per_100ms = convertDistanceToRev(vel_m_per_100ms, config);
double talon_units_per_100ms = convertRevtoTalonUnits(rev_per_100ms, config);

motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, desired_talon_units_per_100ms);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds
motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, talon_units_per_100ms);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms);

return desired_talon_units_per_100ms;
return static_cast<float>(talon_units_per_100ms);
}

float setPositionFromDisplacement(TalonSRX *motor, double claw_displacement, int ms) {
// float length_range = LENGTH_RANGE * 2.54; // in cm
// float desired_pos = (joy_input * length_range) / DISTANCE_PER_REV; // in rev
// float desired_talon_units = convertRevtoTalonUnits(desired_pos);
float setVelocityFromAngularVelocity(TalonSRX * motor, double vel_rad_per_s, int ms, const MotorConfig & config) {
// rad/s -> rev/s -> rev/100ms -> talon units/100ms
double rev_per_100ms = vel_rad_per_s / (2.0 * M_PI * 10.0);
double talon_units_per_100ms = convertRevtoTalonUnits(rev_per_100ms, config);

float desired_talon_units = convertRevtoTalonUnits(convertDistanceToRev(claw_displacement)); // m -> rev -> talon units
motor->Set(ctre::phoenix::motorcontrol::ControlMode::Position, desired_talon_units);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds
motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, talon_units_per_100ms);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms);

return desired_talon_units;
return static_cast<float>(talon_units_per_100ms);
}

float setPositionFromJointCommand(TalonSRX *motor, double joint_position, int ms) {
float desired_talon_units = convertRevtoTalonUnits(joint_position / (2*M_PI)); // rad -> rev -> talon units
motor->Set(ctre::phoenix::motorcontrol::ControlMode::Position, desired_talon_units);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds
float setPositionFromDisplacement(TalonSRX * motor, double displacement_m, int ms, const MotorConfig & config) {
// m -> rev -> talon units
double talon_units = convertRevtoTalonUnits(convertDistanceToRev(displacement_m, config), config);

return desired_talon_units;
}

void stopMotor(TalonSRX *motor, int ms){
motor->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0.0);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds
motor->Set(ctre::phoenix::motorcontrol::ControlMode::Position, talon_units);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms);

return static_cast<float>(talon_units);
}

// float runMotor(TalonSRX *motor, double speed, int ms) {
// float velocity = setVelocityFromLinearVelocity(motor, speed); // motor receives talon units/100ms, function returns rev/s
// // setDutyCycle(motor, speed); // set percent output and direction
// ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds
// return velocity;
// }
float setPositionFromJointCommand(TalonSRX * motor, double position_rad, int ms, const MotorConfig & config) {
// rad -> rev -> talon units
double talon_units = convertRevtoTalonUnits(position_rad / (2.0 * M_PI), config);

motor->Set(ctre::phoenix::motorcontrol::ControlMode::Position, talon_units);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms);

// float setMotorPosition(TalonSRX *motor, double joy_input, int ms) {
// float position = setPositionFromDisplacement(motor, joy_input); // in Talon Units
// ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds
// return position;
// }
return static_cast<float>(talon_units);
}

// void closeClaw(TalonSRX *motor) {
// while (abs(motor.GetStatorCurrent()) < 0.1) {
// runMotor(motor, -1, 10);
// sleep(0.01);
// }
// }
void stopMotor(TalonSRX * motor, int ms) {
motor->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0.0);
ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms);
}
Loading