Skip to main content

​
Overview

Mars-RS implements several sophisticated movement algorithms for differential drive robots. These algorithms combine PID control, geometric path following, and motion profiling to achieve smooth and accurate robot movement.

​
Core Movement Functions

​
pidMTP (PID Move-to-Point)

The fundamental movement algorithm that drives the robot to a target position using PID control for both linear and angular velocity. 
pub fn pidMTP(
    robot: &Arc<Mutex<robot::Robot>>, 
    target: (f32, f32), 
    rotationCut: f32, 
    timeout: u16, 
    lConstants: util::PidConstants, 
    rConstants: util::PidConstants, 
    min: f32
)
The pidMTP function uses separate PID controllers for left and right wheel velocities, allowing independent control of linear and angular motion.
Key Features:
  • Calculates linear error (distance to target)
  • Computes rotation error (heading difference)
  • Scales velocity based on rotation error using cosine function
  • Minimum velocity threshold to prevent stalling

​
Velocity Calculation

The velocity calculation in pidMTPVel uses a clever scaling approach: 
let scale = 90.0 / rotationCut;
let cre = if rotationError.abs() > rotationCut {
    0.1
} else {
    (scale * rotationError.to_radians()).cos()
};
let linearVel = (cre * lCont.out(linearError)).max(min);
The cosine-based scaling reduces forward velocity when the robot is misaligned, forcing it to turn toward the target before moving forward.
The rotationCut parameter defines the angle (in degrees) at which the robot begins to significantly reduce forward velocity. Lower values make the robot more cautious about heading errors.

​
Boomerang

The boomerang algorithm creates a smooth curved path to the target point with a specified final heading. It uses a “carrot” point that leads the robot along a natural arc. 
pub fn boomerang(
    robot: &Arc<Mutex<robot::Robot>>, 
    target: (f32, f32), 
    timeout: u16, 
    dLead: f32, 
    thetaEnd: f32, 
    rotationCut: f32, 
    lConstants: util::PidConstants, 
    rConstants: util::PidConstants, 
    min: f32
)
Carrot Point Calculation: 
let h = (pos.0 - target.0).hypot(pos.1 - target.1);
let carrot = (
    target.0 - (h * thetaEnd.sin() * dLead), 
    target.1 - (h * thetaEnd.cos() * dLead)
);
The carrot point is offset from the target based on:
  • dLead: Distance multiplier (typically 0-1)
  • thetaEnd: Desired final heading angle
  • h: Current distance to target
The algorithm creates a curved path that resembles a boomerang shape, especially when the final heading differs significantly from the direct line to the target. This allows the robot to approach the target from a specific direction.

​
followPath

Follows a sequence of waypoints using the boomerang approach for each segment, automatically transitioning between waypoints. 
pub fn followPath(
    robot: &Arc<Mutex<robot::Robot>>, 
    path: Vec<(f32,f32)>, 
    timeout: u32, 
    dLead: f32, 
    thetaEnd: f32, 
    rotationCut: f32, 
    lConstants: util::PidConstants, 
    rConstants: util::PidConstants, 
    min: f32
)
Features:
  • Automatically advances to next waypoint when within 40 units
  • Calculates heading angle between consecutive waypoints
  • Uses boomerang-style carrot points for smooth transitions
if util::dist(pos, target) < 40.0 {
    if pathIndex < (path.len()-1) {pathIndex += 1};
}

​
eulerTurn

Performs curved turns with gradually increasing curvature, creating smooth and natural rotation movements. 
pub fn eulerTurn(
    robot: &Arc<Mutex<robot::Robot>>, 
    theta: f32, 
    rate: f32, 
    curvature: f32, 
    timeout: u32, 
    dir: i8, 
    constants: util::PidConstants
) -> f32
Curvature-Based Wheel Velocities: 
let sl = error * (1.0 / curvature + 15.0);
let sr = error * (1.0 / curvature - 15.0);
let ratio = sl/sr;
The algorithm:
  1. Gradually increases curvature over time
  2. Calculates wheel velocity ratio from curvature
  3. Applies PID control to the rotation error
  4. Returns final curvature for chaining multiple turns
The 15.0 constant represents half the track width (wheelbase) in the curvature calculation, affecting how sharply the robot can turn.

​
Algorithm Comparison

AlgorithmUse CasePath ShapeHeading Control
pidMTPSimple point-to-pointDirect lineAutomatic
BoomerangApproach with specific headingSmooth curveExplicit final angle
followPathMulti-waypoint navigationPiecewise curvesBetween waypoints
eulerTurnIn-place or curved rotationSpiralExplicit target
moveToPurePursuitSmooth path followingFollows exact pathAlong path tangent

​
Control Loop Timing

All movement algorithms use a 10ms control loop: 
thread::sleep(Duration::from_millis(10));
This provides a 100Hz update rate, balancing responsiveness with computational efficiency.

PID Control

Learn about PID tuning and constants

Pure Pursuit

Deep dive into pure pursuit algorithm

Robot Physics

Understand differential drive kinematics

Build docs developers (and LLMs) love

Get started for freeTalk to us