3D motion/path controller using PID for position and orientation. More...

#include <MotionController3D.h>

Collaboration diagram for MotionController3D:

[legend]

Public Member Functions
	MotionController3D (IMotionState3D &motionStateRef, OnGoalAction onGloal, float positionToleranceM=2.0)

void	configurePositionPID (float dt, float maxOut, float minOut, float kp, float ki, float kd)

void	configureOrientationPID (float dt, float minOut, float maxOut, float kp, float ki, float kd)

void	setPath (Path< Coordinate< DistanceM > > path)

void	begin ()
	Initialize controller and set target from first path coordinate if available.

void	end ()
	Stop the controller and the vehicle.

bool	update ()
	Update the controller and compute new commands.

Speed3D	getLinearCommand () const

AngularVelocity3D	getAngularCommand () const

MotionState3D	getTarget () const

bool	isActive () const

void	setOnGoalAction (OnGoalAction action)
	Set the OnGoalAction behavior (Stop, HoldPosition, Circle). Resets circle mode state if changed.

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.

void	setCircleRadius (float radius)
	Set the radius for circular motion (meters).

void	setCircleAngularSpeed (float angularSpeed)
	Set the angular speed for circular motion (radians per update).

Protected Member Functions
bool	initializeDtFromUpdates ()

bool	advanceWaypoint (float positionTolerance=0.1f)

void	handleOnGoalAction ()

Protected Attributes
bool	is_active = false

float	positionToleranceM

OnGoalAction	onGoalAction

Path< Coordinate< DistanceM > >	path

IMotionState3D &	motionState

MotionState3D	target

PIDController< float >	pidX

PIDController< float >	pidY

PIDController< float >	pidZ

PIDController< float >	pidYaw

PIDController< float >	pidPitch

PIDController< float >	pidRoll

Speed3D	linearCmd = Speed3D(0, 0, 0, SpeedUnit::MPS)

AngularVelocity3D	angularCmd

bool(*	onGoalCallback )(void *) = nullptr

void *	onGoaldRef = this

int	updateCount = 0

unsigned long	updateStartTimeMs = 0

unsigned long	updateEndTimeMs = 0

bool	dtSetFromUpdates = false

float	circlePhase = 0.0f

float	circleRadius = 5.0f

float	circleAngularSpeed = 0.5f

Coordinate< DistanceM >	circleCenter

bool	circleInitialized = false

Detailed Description

3D motion/path controller using PID for position and orientation.

This class provides high-level 3D path following and pose control for robots, drones, or vehicles. It uses independent PID controllers for each axis (x, y, z) and orientation (yaw, pitch, roll).

Features

Accepts a reference to an IMotionState3D for real-time feedback
Supports a path of 3D waypoints (Path<Coordinate<DistanceM>>)
Automatically advances to the next waypoint when the current one is reached
PID gains and limits are configurable for both position and orientation
Provides update() to compute new commands, and getters for linear/angular velocity commands
begin() initializes the target from the first path coordinate

Usage

Create with a reference to your IMotionState3D implementation (e.g., IMU3D)
Set a path of waypoints with setPath()
Call begin() to initialize the first target
In your control loop:
- Call update() to compute new commands
- Use getLinearCommand() and getAngularCommand() for actuation

Limitations

Only the position is updated from the path; orientation, speed, and angular velocity are kept from the previous target

Example

MotionController3D controller(imu3d);
controller.setPath(path);
controller.begin();
while (!controller.isPathComplete()) {
  controller.update();
  auto v = controller.getLinearCommand();
  auto w = controller.getAngularCommand();
  // send v, w to actuators
  controller.advanceWaypoint();
}

Member Function Documentation

◆ initializeDtFromUpdates()

bool initializeDtFromUpdates ( )

inlineprotected

Handles dt initialization from first 10 updates, but does not block control logic

◆ update()

bool update ( )

inline

Update the controller and compute new commands.

Returns: true if we still have a path to follow, false if path is complete or controller is inactive.

Member Data Documentation

◆ angularCmd

AngularVelocity3D angularCmd

protected

Initial value:

=

AngularVelocity3D(0, 0, 0, AngularVelocityUnit::RadPerSec)

◆ target

MotionState3D target