A field-relative position controller utilizing Profiled PID loops for 3-axis robot alignment. More...

#include <valkyrie/WorldAlign.h>

Inheritance diagram for valor::WorldAlign:

Classes
struct	StateComponent
struct	StateVector

Public Types
using	TranslationalPIDF_t = valor::PIDF<units::meter, units::meters_per_second>
using	RotationalPIDF_t = valor::PIDF<units::radian, units::radians_per_second>

Public Member Functions
	WorldAlign (TranslationalPIDF_t xPIDF, TranslationalPIDF_t yPIDF, RotationalPIDF_t rotationalPIDF, frc::TrapezoidProfile< units::meter >::Constraints xConstraints, frc::TrapezoidProfile< units::meter >::Constraints yConstraints, frc::TrapezoidProfile< units::radian >::Constraints rotConstraints)
	Constructs a WorldAlign object.
void	SetPID (TranslationalPIDF_t x, TranslationalPIDF_t y, RotationalPIDF_t rot)
	A convenience method to set the PID gains for all three controllers at once.
void	SetXPID (TranslationalPIDF_t)
	Sets the PID gains for the X-axis controller.
void	SetYPID (TranslationalPIDF_t)
	Sets the PID gains for the Y-axis controller.
void	SetRotationalPID (RotationalPIDF_t)
	Sets the PID gains for the rotation controller.
std::tuple< TranslationalPIDF_t, TranslationalPIDF_t, RotationalPIDF_t >	GetPID ()
	Gets the PID gains for all three controllers.
TranslationalPIDF_t	GetXPID ()
	Gets the PID gains for the X-axis controller.
TranslationalPIDF_t	GetYPID ()
	Gets the PID gains for the Y-axis controller.
RotationalPIDF_t	GetRotationalPID ()
	Gets the PID gains for the rotation controller.
void	ResetControllers (frc::Pose2d, frc::ChassisSpeeds)
	Resets all controllers with the current robot state. This synchronizes the controller's internal state with the actual robot position and velocity.
void	ResetXController (units::meter_t, units::meters_per_second_t)
	Resets the X-axis controller with a new position and velocity.
void	ResetYController (units::meter_t, units::meters_per_second_t)
	Resets the Y-axis controller with a new position and velocity.
void	ResetRotationalController (frc::Rotation2d, units::radians_per_second_t)
	Resets the rotation controller with a new angle and angular velocity.
void	SetConstraints (frc::TrapezoidProfile< units::meter >::Constraints x, frc::TrapezoidProfile< units::meter >::Constraints y, frc::TrapezoidProfile< units::radian >::Constraints rot)
	Sets the motion profile constraints for all three controllers.
void	SetXConstraints (frc::TrapezoidProfile< units::meter >::Constraints)
	Sets the motion constraints for the X-axis controller.
void	SetYConstraints (frc::TrapezoidProfile< units::meter >::Constraints)
	Sets the motion constraints for the Y-axis controller.
void	SetRotationalConstraints (frc::TrapezoidProfile< units::radian >::Constraints)
	Sets the motion constraints for the rotation controller.
void	EnableContinuousRotation (bool enable=true, units::radian_t minimum=units::radian_t{-std::numbers::pi}, units::radian_t maximum=units::radian_t{std::numbers::pi})
	Enables or disables continuous input for the rotation controller. Continuous input is necessary for rotational systems where -180 and +180 degrees are the same.
frc::TrapezoidProfile< units::meter >::Constraints	GetXConstraints ()
	Gets the motion constraints for the X-axis controller.
frc::TrapezoidProfile< units::meter >::Constraints	GetYConstraints ()
	Gets the motion constraints for the Y-axis controller.
frc::TrapezoidProfile< units::radian >::Constraints	GetRotationalConstraints ()
	Gets the motion constraints for the rotation controller.
void	SetGoal (frc::Pose2d goal, StateVector state, bool rotate)
	Sets the desired goal state for the alignment controllers.
std::pair< frc::Pose2d, StateVector >	GetGoal ()
	Gets the current goal.
frc::ChassisSpeeds	Calculate (frc::Pose2d pose, frc::ChassisSpeeds currentSpeeds)
	Calculates the required chassis speeds to reach the last set goal. This is an overload that uses the currently stored goal state.
frc::ChassisSpeeds	Calculate (frc::Pose2d pose, frc::ChassisSpeeds currentSpeeds, StateVector state)
	Calculates the required chassis speeds to move towards the goal state.
void	LoggablePeriodic () override
	Periodic callback for logging updates.
Public Member Functions inherited from valor::Loggable
	Loggable (std::string_view name)
	Construct a Loggable that registers a top-level table name.
	Loggable (std::shared_ptr< nt::NetworkTable > table)
	Construct a Loggable that uses an existing NetworkTable.
	Loggable ()
	Default construct an uninitialized Loggable.
void	LogChild (std::string_view name, Loggable *child)
	Register a child Loggable under a named subtree.
void	LogChild (std::string_view name, wpi::Sendable *child)
	Register a child Sendable under a named subtree.
void	SetPeriodicLevel (LogLevel level)
	Set the logging level for LoggablePeriodic.
template<typename T>
T	WriteLog (std::string_view field, const T &data, LogLevel level=LogLevel::Normal)
	Publish a value to NetworkTables under the given field.
template<typename T>
T	ReadLog (std::string_view field, const T &defaultValue={})
	Read a value from NetworkTables for the given field.
template<>
decltype(Loggable::subscribers) ::iterator	addSubscriber (std::string_view field, const std::vector< bool > &defaultValue)
template<>
void	WriteLogImpl (nt::Publisher *pub, const std::vector< bool > &data)
template<>
std::vector< bool >	ReadLogImpl (nt::Subscriber *sub)

Static Public Member Functions
static frc::Pose2d	OffsetGetter (frc::Pose2d, frc::Transform2d)
	Calculates a new pose by applying a transformation to a reference pose. This is a static utility function.
Static Public Member Functions inherited from valor::Loggable
static units::millisecond_t	GetLoggingTime ()
static constexpr Loggable &	GetRoot ()

Additional Inherited Members
Protected Member Functions inherited from valor::Loggable
virtual void	OnLoggingStart ()
	Hook invoked when logging is started for this object.

Detailed Description

A field-relative position controller utilizing Profiled PID loops for 3-axis robot alignment.

WorldAlign provides a high-level interface for driving a robot to a specific target pose (X, Y, and Rotation) within the world coordinate frame. It encapsulates three independent frc::ProfiledPIDController instances, allowing for smooth, trapezoidal motion-profiled movement towards a goal.

This class is designed to handle:

Field-Relative Control: All calculations and setpoints are handled in the world frame.
Motion Profiling: Uses velocity and acceleration constraints to ensure robot movements stay within physical limits.
Rotation Wrapping: Supports continuous input for the rotational axis to determine the shortest path between angles (e.g., -180 to 180 degrees).
Telemetry: Inherits from valor::Loggable to facilitate real-time monitoring of setpoints and controller states

Note: This controller expects field-relative frc::ChassisSpeeds and frc::Pose2d as inputs and returns field-relative frc::ChassisSpeeds as outputs

Constructor & Destructor Documentation

◆ WorldAlign()

valor::WorldAlign::WorldAlign	(	TranslationalPIDF_t	xPIDF,
		TranslationalPIDF_t	yPIDF,
		RotationalPIDF_t	rotationalPIDF,
		frc::TrapezoidProfile< units::meter >::Constraints	xConstraints,
		frc::TrapezoidProfile< units::meter >::Constraints	yConstraints,
		frc::TrapezoidProfile< units::radian >::Constraints	rotConstraints )

Constructs a WorldAlign object.

This controller uses three independent ProfiledPIDControllers to align the robot to a target pose in the world coordinate frame (field-relative).

Parameters

xPIDF	PIDF gains for the X-axis controller.
yPIDF	PIDF gains for the Y-axis controller.
rotationalPIDF	PIDF gains for the rotation controller.
xConstraints	Trapezoidal motion profile constraints for the X-axis.
yConstraints	Trapezoidal motion profile constraints for the Y-axis.
rotConstraints	Trapezoidal motion profile constraints for the rotational axis.

Member Function Documentation

◆ Calculate() [1/2]

frc::ChassisSpeeds valor::WorldAlign::Calculate	(	frc::Pose2d	pose,
		frc::ChassisSpeeds	currentSpeeds )

Calculates the required chassis speeds to reach the last set goal. This is an overload that uses the currently stored goal state.

Parameters

pose	The current pose of the robot in the world frame.
currentSpeeds	The current chassis speeds of the robot (must be field-relative).

Returns: The calculated field-relative chassis speeds.

◆ Calculate() [2/2]

frc::ChassisSpeeds valor::WorldAlign::Calculate	(	frc::Pose2d	pose,
		frc::ChassisSpeeds	currentSpeeds,
		StateVector	state )

Calculates the required chassis speeds to move towards the goal state.

Parameters

pose	The current robot pose in the world frame.
currentSpeeds	The current robot chassis speeds (must be field-relative).
state	The target state (position and velocity) for the controllers.

Returns: The calculated field-relative chassis speeds.

◆ EnableContinuousRotation()

void valor::WorldAlign::EnableContinuousRotation	(	bool	enable = true,
		units::radian_t	minimum = units::radian_t{-std::numbers::pi},
		units::radian_t	maximum = units::radian_t{std::numbers::pi} )

Enables or disables continuous input for the rotation controller. Continuous input is necessary for rotational systems where -180 and +180 degrees are the same.

Parameters

enable	True to enable, false to disable.
minimum	The minimum angle for the continuous range.
maximum	The maximum angle for the continuous range.

◆ GetGoal()

std::pair< frc::Pose2d, StateVector > valor::WorldAlign::GetGoal ( )

Gets the current goal.

Returns: A pair containing the goal pose and the goal state vector, both in the world frame.

◆ GetPID()

std::tuple< TranslationalPIDF_t, TranslationalPIDF_t, RotationalPIDF_t > valor::WorldAlign::GetPID ( )

Gets the PID gains for all three controllers.

Returns: An array containing the PIDF values for the X, Y, and rotation controllers.

◆ GetRotationalConstraints()

frc::TrapezoidProfile< units::radian >::Constraints valor::WorldAlign::GetRotationalConstraints ( )

Gets the motion constraints for the rotation controller.

Returns: The current constraints.

◆ GetRotationalPID()

RotationalPIDF_t valor::WorldAlign::GetRotationalPID ( )

Gets the PID gains for the rotation controller.

Returns: The current PIDF values for rotation.

◆ GetXConstraints()

frc::TrapezoidProfile< units::meter >::Constraints valor::WorldAlign::GetXConstraints ( )

Gets the motion constraints for the X-axis controller.

Returns: The current constraints.

◆ GetXPID()

TranslationalPIDF_t valor::WorldAlign::GetXPID ( )

Gets the PID gains for the X-axis controller.

Returns: The current PIDF values for the X-axis.

◆ GetYConstraints()

frc::TrapezoidProfile< units::meter >::Constraints valor::WorldAlign::GetYConstraints ( )

Gets the motion constraints for the Y-axis controller.

Returns: The current constraints.

◆ GetYPID()

TranslationalPIDF_t valor::WorldAlign::GetYPID ( )

Gets the PID gains for the Y-axis controller.

Returns: The current PIDF values for the Y-axis.

◆ LoggablePeriodic()

void valor::WorldAlign::LoggablePeriodic ( )

overridevirtual

Periodic callback for logging updates.

If a derived class needs to perform periodic updates to published values, override this method. It is intended to be called by the logging framework; this base class provides an empty implementation.

Reimplemented from valor::Loggable.

◆ OffsetGetter()

frc::Pose2d valor::WorldAlign::OffsetGetter	(	frc::Pose2d	,
		frc::Transform2d	)

static

Calculates a new pose by applying a transformation to a reference pose. This is a static utility function.

Parameters

referencePoint	The starting pose in the world frame.
relativeOffsets	The transformation to apply.

Returns: The resulting pose in the world frame.

◆ ResetControllers()

void valor::WorldAlign::ResetControllers	(	frc::Pose2d	,
		frc::ChassisSpeeds	)

Resets all controllers with the current robot state. This synchronizes the controller's internal state with the actual robot position and velocity.

Parameters

pose	The current pose of the robot in the world frame.
velocity	The current chassis speeds of the robot (must be field-relative).

◆ ResetRotationalController()

void valor::WorldAlign::ResetRotationalController	(	frc::Rotation2d	,
		units::radians_per_second_t	)

Resets the rotation controller with a new angle and angular velocity.

Parameters

rotation	The current rotation in the world frame.
omega	The current angular velocity.

◆ ResetXController()

void valor::WorldAlign::ResetXController	(	units::meter_t	,
		units::meters_per_second_t	)

Resets the X-axis controller with a new position and velocity.

Parameters

xCoord	The current X coordinate in the world frame.
velocity	The current velocity in the X direction in the world frame.

◆ ResetYController()

void valor::WorldAlign::ResetYController	(	units::meter_t	,
		units::meters_per_second_t	)

Resets the Y-axis controller with a new position and velocity.

Parameters

yCoord	The current Y coordinate in the world frame.
velocity	The current velocity in the Y direction in the world frame.

◆ SetConstraints()

void valor::WorldAlign::SetConstraints	(	frc::TrapezoidProfile< units::meter >::Constraints	x,
		frc::TrapezoidProfile< units::meter >::Constraints	y,
		frc::TrapezoidProfile< units::radian >::Constraints	rot )

Sets the motion profile constraints for all three controllers.

Parameters

x	The constraints for the X-axis (max velocity and acceleration).
y	The constraints for the Y-axis.
rot	The constraints for rotation.

◆ SetGoal()

void valor::WorldAlign::SetGoal	(	frc::Pose2d	goal,
		StateVector	state,
		bool	rotate )

Sets the desired goal state for the alignment controllers.

Parameters

goal	The target pose (position and rotation) in the world frame.
state	The target state vector (final position and velocity) for each axis.
rotate	A flag to enable or disable the rotation controller for this goal.

◆ SetPID()

void valor::WorldAlign::SetPID	(	TranslationalPIDF_t	x,
		TranslationalPIDF_t	y,
		RotationalPIDF_t	rot )

A convenience method to set the PID gains for all three controllers at once.

Parameters

x	The PIDF values for the X-axis controller.
y	The PIDF values for the Y-axis controller.
rot	The PIDF values for the rotation controller.

◆ SetRotationalConstraints()

void valor::WorldAlign::SetRotationalConstraints ( frc::TrapezoidProfile< units::radian >::Constraints )

Sets the motion constraints for the rotation controller.

Parameters

rot	The new constraints.

◆ SetRotationalPID()

void valor::WorldAlign::SetRotationalPID ( RotationalPIDF_t )

Sets the PID gains for the rotation controller.

Parameters

rot	The new PIDF values.

◆ SetXConstraints()

void valor::WorldAlign::SetXConstraints ( frc::TrapezoidProfile< units::meter >::Constraints )

Sets the motion constraints for the X-axis controller.

Parameters

x	The new constraints.

◆ SetXPID()

void valor::WorldAlign::SetXPID ( TranslationalPIDF_t )

Sets the PID gains for the X-axis controller.

Parameters

x	The new PIDF values.

◆ SetYConstraints()

void valor::WorldAlign::SetYConstraints ( frc::TrapezoidProfile< units::meter >::Constraints )

Sets the motion constraints for the Y-axis controller.

Parameters

y	The new constraints.

◆ SetYPID()

void valor::WorldAlign::SetYPID ( TranslationalPIDF_t )

Sets the PID gains for the Y-axis controller.

Parameters

y	The new PIDF values.

The documentation for this class was generated from the following file:

src/main/include/valkyrie/WorldAlign.h

Classes

Public Types

Public Member Functions

Static Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ WorldAlign()

Member Function Documentation

◆ Calculate() [1/2]

◆ Calculate() [2/2]

◆ EnableContinuousRotation()

◆ GetGoal()

◆ GetPID()

◆ GetRotationalConstraints()

◆ GetRotationalPID()

◆ GetXConstraints()

◆ GetXPID()

◆ GetYConstraints()

◆ GetYPID()

◆ LoggablePeriodic()

◆ OffsetGetter()

◆ ResetControllers()

◆ ResetRotationalController()

◆ ResetXController()

◆ ResetYController()

◆ SetConstraints()

◆ SetGoal()

◆ SetPID()

◆ SetRotationalConstraints()

◆ SetRotationalPID()

◆ SetXConstraints()

◆ SetXPID()

◆ SetYConstraints()

◆ SetYPID()