MotionController2D< T > Class Template Reference

2D motion controller for path following and vehicle control. More...

#include <MotionController2D.h>

Inheritance diagram for MotionController2D< T >:

Collaboration diagram for MotionController2D< T >:

Public Member Functions
	MotionController2D (IMotionState2D &motionState, float maxSpeedKmh=10, float maxSteeringAngleDeg=30.0f, float accelDistanceM=2.0f)
	MotionController2D (IMotionState2D &motionState, Speed maxSpeedKmh, Angle maxSteeringAngle, Distance accelDistanceM)
void	configureSpeedPID (float minOut, float maxOut, float kp, float ki, float kd)
	Configure the PID controller for speed-to-throttle mapping.
void	configureSteeringPID (float minOut, float maxOut, float kp, float ki, float kd)
	Configure the PID controller for steering.
void	setPath (Path< Coordinate< DistanceM > > path)
	Defines the path to follow as a sequence of waypoints.
void	addWaypoint (Coordinate< DistanceM > target)
	Adds a single waypoint to the path (appended to the end)
void	setTargetAccuracy (float accuracyM)
	Set the target accuracy (meters) for reaching waypoints.
bool	begin ()
	Start the controller and initialize state.
void	end ()
	Stop the controller and the vehicle.
void	update ()
	Main control loop to be called periodically (e.g., in a timer or main loop)
void	setOnGoalCallback (bool(*callback)(void *), void *ref=nullptr)
	Set a custom callback to be called when reaching the goal. The callback should return true if it handled the goal action, or false to allow default handling.
float	getThrottlePercent () const
	Get the last computed throttle percent.
float	getSteeringAngleDeg () const
	Get the last computed steering angle in degrees.
Angle	getSteeringAngle () const
	Get the last computed steering angle as an Angle object.
float	getTargetAccuracy (DistanceUnit unit) const
	Get the Target Accuracy object.
bool	isGoalReached () const
	Returns true if the goal has been reached.
void	setThrottleMode (ThrottleMode mode)
	Set the throttle control mode.
void	setInvertedSteering (bool inverted)
	Set the Inverted Steering logic: by default we use ROS logic for steering correction (positive angle = turn left), but some vehicles may require the opposite. Set to true to invert the steering direction.
Public Member Functions inherited from MessageSource
void	subscribe (MessageHandler &handler, MessageOrigin origin=MessageOrigin::Undefined, MessageContent content=MessageContent::Undefined)
	Subscribe a message handler to this source, with optional filtering.
void	unsubscribeAll ()
	Remove all registered message handlers.
void	sendMessage (Message< float > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< Coordinate< float > > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< GPSCoordinate > &msg)
	Publish a message to all registered handlers.
void	sendMessage (const Message< MotionState3D > &msg)
	Overload for MotionState3D messages.

Protected Member Functions
float	getDesiredSpeed (float distanceFromStartM, float distanceToTarget, float currentSpeedKmh)
	Calculate desired speed based on distance to target and start.
bool	handleWaypoint (const Coordinate< DistanceM > &currentPos, Coordinate< DistanceM > &targetPos, float &distanceToTargetM, float &desiredHeadingDeg)
	Handle the current waypoint: calculate distance and desired heading.
float	computeHeadingError (float desiredHeading, float currentHeading)
	Compute angle difference range [-180, 180].
float	feedforwardThrottle (float desiredSpeedKmh) const
	Model-based feedforward throttle estimate: 100% throttle = maxSpeedKmh.
float	getThrottlePercent (float currentSpeedKmh)
	Compute the throttle percent based on the selected mode.
void	sendControlMessages (float throttlePercent, float steeringAngleDeg)
	Send control messages for throttle and steering angle.
bool	initializeDtFromUpdates ()
	Handles dt initialization from first 10 updates.

Protected Attributes
IMotionState2D &	motionStateSource
Path< Coordinate< DistanceM > >	path
bool	is_active = false
bool	has_distance = false
float	accelDistanceM = 2.0f
float	maxSpeedKmh = 10.0f
float	desiredSpeedKmh = 0.0f
float	maxSteeringAngleDeg = 30.0f
PIDController< float >	pidSteering_
PIDController< float >	pidSpeed_
Coordinate< DistanceM >	startCoordinate
bool	hasStartCoordinate = false
float	targetAccuracyM = 0.10f
float	distanceToTargetM = 0
bool(*	onGoalCallback )(void *) = nullptr
void *	onGoalRef = this
float	resultThrottlePercent = 0.0f
float	resultSteeringAngleDeg = 0.0f
ThrottleMode	throttleMode = ThrottleMode::Combined
	Throttle control mode.
int	updateCount = 0
unsigned long	updateStartTimeMs = 0
unsigned long	updateEndTimeMs = 0
bool	dtSetFromUpdates = false
bool	isInverted = false
Protected Attributes inherited from MessageSource
std::vector< MessageHandlerEntry >	messageHandlers_

Detailed Description

template<typename T = float>
class tinyrobotics::MotionController2D< T >

2D motion controller for path following and vehicle control.

This class implements a flexible 2D motion controller for robotic vehicles, supporting both model-based (feedforward) and feedback (PID) control for speed (throttle) and steering. It is designed for path following, smooth acceleration/deceleration, and robust integration with IMU and odometry sensors.

Features:

• Configurable PID controllers for both speed (throttle) and steering angle correction.
• Multiple throttle control strategies: Feedforward (model-based), PID (feedback), or Combined (feedforward + feedback).
• Path following with a sequence of waypoints, including automatic waypoint advancement and goal detection.
• Smooth acceleration and deceleration profiles based on distance to target and from start.
• Dynamic time step (dt) initialization for PID controllers based on update frequency.
• Modular design for use with different vehicle types, IMU sensors, and coordinate systems.
• Integration with the TinyRobotics message system for sending throttle and steering commands.
• Customizable goal callback for user-defined actions upon reaching the final waypoint.

Usage Example:

MotionController2D<> controller(motionState, 10.0f, 2.0f);
controller.setPath(path);
controller.setThrottleMode(ThrottleMode::Combined);
controller.begin();
// In main loop:
controller.update();

Integration:

• Use as a standalone controller for path following and vehicle control.
• Integrate with a message bus to receive and send control messages.
• Extend or customize by overriding protected methods or providing custom callbacks.

Template Parameters

T	Numeric type for calculations (default: float)

Author

Phil Schatzmann

Member Function Documentation

configureSpeedPID()

void configureSpeedPID ( float  minOut, float  maxOut, float  kp, float  ki, float  kd  )

inline

Configure the PID controller for speed-to-throttle mapping.

Parameters

dt	Time step (seconds)
maxOut	Maximum output (throttle)
minOut	Minimum output (throttle)
kp	Proportional gain
ki	Integral gain
kd	Derivative gain

configureSteeringPID()

void configureSteeringPID ( float  minOut, float  maxOut, float  kp, float  ki, float  kd  )

inline

Configure the PID controller for steering.

Parameters

minOut	Minimum output
maxOut	Maximum output
kp	Proportional gain
ki	Integral gain
kd	Derivative gain

getTargetAccuracy()

float getTargetAccuracy ( DistanceUnit  unit ) const

inline

Get the Target Accuracy object.

Parameters

unit

Returns

float

setInvertedSteering()

void setInvertedSteering ( bool  inverted )

inline

Set the Inverted Steering logic: by default we use ROS logic for steering correction (positive angle = turn left), but some vehicles may require the opposite. Set to true to invert the steering direction.

Parameters

inverted

setTargetAccuracy()

void setTargetAccuracy ( float  accuracyM )

inline

Set the target accuracy (meters) for reaching waypoints.

Parameters

accuracyM

The new target accuracy in meters.

setThrottleMode()

void setThrottleMode ( ThrottleMode  mode )

inline

Set the throttle control mode.

Parameters

mode	The throttle control strategy to use (FeedforwardOnly, PIDOnly, Combined).

---

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

• src/TinyRobotics/control/MotionController2D.h