Skip to main content

​
Introduction

Mars-RS simulates a differential drive robot, where motion is controlled by independently setting the velocities of left and right wheels. Understanding the physics of differential drive is essential for effective motion control.

​
Robot Structure

The Robot struct maintains the robot’s state: 
pub struct Robot {
    pub position: (f32, f32),   // (x, y) coordinates
    pub heading: f32,            // Orientation in radians
    pub robotSize: f32           // Half of the track width
}
robotSize represents half the distance between the left and right wheels (half the track width). This is crucial for rotation calculations.

​
Robot Dimensions

The robot size is calculated based on a scale factor: 
self.robotSize = ft() / 12.0 * 15.0 / 2.0;
Where:
  • ft() returns a scaling factor for the field
  • The robot is 15 units wide (in scaled coordinates)
  • Division by 2 gives the half-width

​
Differential Drive Kinematics

​
The Step Function

The step function updates the robot’s position based on left and right wheel velocities: 
pub fn step(&mut self, d: (f32, f32)) {
    let deltaRotation = (d.1 - d.0) / (self.robotSize);
    self.heading += deltaRotation;
    
    if deltaRotation == 0.0 {
        // Straight line motion
        self.position.0 -= (PI/2.0 + self.heading).cos() * d.1;
        self.position.1 -= (PI/2.0 + self.heading).sin() * d.1;
    }
    else {
        // Curved motion
        let r = d.0 / deltaRotation + self.robotSize / 2.0;
        let relativeY = 2.0 * (deltaRotation / 2.0).sin() * r;
        let rotationOffset = self.heading + (deltaRotation / 2.0);
        let theta = PI / 2.0 + rotationOffset;
        let radius = relativeY;
        self.position.0 -= radius * theta.cos();
        self.position.1 -= radius * theta.sin();
    }
}
Parameters:
  • d.0: Left wheel velocity
  • d.1: Right wheel velocity

​
Rotation Calculation

The change in heading (rotation) is determined by the velocity difference: Δθ=vR−vLw\Delta\theta = \frac{v_R - v_L}{w} Where:
  • Δθ\Delta\theta = change in heading (deltaRotation)
  • vRv_R = right wheel velocity (d.1)
  • vLv_L = left wheel velocity (d.0)
  • ww = track width (2 × robotSize)
let deltaRotation = (d.1 - d.0) / (self.robotSize);
Positive rotation: Right wheel faster than left (robot turns left)Negative rotation: Left wheel faster than right (robot turns right)Zero rotation: Both wheels at same speed (straight line)

​
Motion Cases

​
Case 1: Straight Line Motion

When both wheels move at the same speed (vL=vRv_L = v_R): 
if deltaRotation == 0.0 {
    self.position.0 -= (PI/2.0 + self.heading).cos() * d.1;
    self.position.1 -= (PI/2.0 + self.heading).sin() * d.1;
}
Mathematics: The robot moves distance dd in the direction of its heading: xnew=xold+d⋅cos⁡(θ+π2)ynew=yold+d⋅sin⁡(θ+π2)\begin{align*} x_{new} &= x_{old} + d \cdot \cos(\theta + \frac{\pi}{2}) \\ y_{new} &= y_{old} + d \cdot \sin(\theta + \frac{\pi}{2}) \end{align*}
The π2\frac{\pi}{2} (90°) offset converts from the robot’s heading representation (where 0 points up) to standard mathematical convention (where 0 points right).This coordinate system choice is common in robotics where “forward” is considered the positive y-direction.

​
Case 2: Curved Motion

When wheels move at different speeds, the robot follows a circular arc: 
else {
    let r = d.0 / deltaRotation + self.robotSize / 2.0;
    let relativeY = 2.0 * (deltaRotation / 2.0).sin() * r;
    let rotationOffset = self.heading + (deltaRotation / 2.0);
    let theta = PI / 2.0 + rotationOffset;
    let radius = relativeY;
    self.position.0 -= radius * theta.cos();
    self.position.1 -= radius * theta.sin();
}

​
Instantaneous Center of Curvature (ICC)

The robot rotates around a point called the ICC: R=vLΔθ+w2R = \frac{v_L}{\Delta\theta} + \frac{w}{2} 
let r = d.0 / deltaRotation + self.robotSize / 2.0;
Where RR is the radius from the robot’s center to the ICC.
The ICC lies along a line perpendicular to the robot’s heading, offset to one side depending on which wheel is faster.

​
Arc Length Calculation

The chord length (straight-line distance traveled) is: chord=2Rsin⁡(Δθ2)chord = 2R \sin\left(\frac{\Delta\theta}{2}\right) 
let relativeY = 2.0 * (deltaRotation / 2.0).sin() * r;
This represents the displacement of the robot’s center during the rotation.

​
Position Update

The final position is calculated using the chord length and the average heading: 
let rotationOffset = self.heading + (deltaRotation / 2.0);
let theta = PI / 2.0 + rotationOffset;
self.position.0 -= radius * theta.cos();
self.position.1 -= radius * theta.sin();
The rotationOffset represents the heading at the midpoint of the motion, giving a more accurate position update.

​
Differential Drive Properties

Set wheel velocities to equal magnitudes but opposite directions:
robot.step((50.0, -50.0));  // Spin right
robot.step((-50.0, 50.0));  // Spin left
The robot rotates around its center point.
The minimum turning radius occurs when one wheel is stopped:Rmin=w2R_{min} = \frac{w}{2}This equals robotSize in the code.
Maximum radius (straightest possible turn) approaches infinity as wheel velocities become equal:lim⁡vL→vRR=∞\lim_{v_L \to v_R} R = \infty

​
Velocity to Wheel Speed Conversion

Movement algorithms calculate linear and angular velocities, then convert to wheel speeds: 
let linearVel = lCont.out(linearError);
let angularVel = rCont.out(rotationError);
let rVel = linearVel - angularVel;
let lVel = linearVel + angularVel;
Formulas: vL=vlinear+vangularvR=vlinear−vangular\begin{align*} v_L &= v_{linear} + v_{angular} \\ v_R &= v_{linear} - v_{angular} \end{align*} Where:
  • vlinearv_{linear} = desired forward velocity
  • vangularv_{angular} = desired rotational velocity
Positive angular velocity increases left wheel speed and decreases right wheel speed, causing a left turn.Negative angular velocity does the opposite, causing a right turn.

​
Coordinate System

Mars-RS uses a screen-based coordinate system:
  • Origin: Top-left corner
  • X-axis: Increases to the right
  • Y-axis: Increases downward
  • Heading: 0 radians points up (negative Y direction)
// Heading = 0: pointing up
// Heading = π/2: pointing right
// Heading = π: pointing down
// Heading = 3π/2: pointing left
The negative signs in the step function (-=) account for the inverted Y-axis where positive motion in standard coordinates corresponds to negative screen coordinates.

​
Rendering the Robot

The render method draws the robot using transformed vertices: 
pub fn render(&mut self) {
    self.robotSize = ft() / 12.0 * 15.0 / 2.0;
    let v1 = util::rotate![-self.robotSize, self.robotSize, self.heading, self.position];
    let v2 = util::rotate![self.robotSize, self.robotSize, self.heading, self.position];
    let v3 = util::rotate![-self.robotSize, -self.robotSize, self.heading, self.position];
    let v4 = util::rotate![self.robotSize, -self.robotSize, self.heading, self.position];
    draw_triangle(v1, v2, v3, Color::from_hex(0x6F2232));
    draw_triangle(v3, v4, v2, Color::from_hex(0x6F2232));
    draw_line(v3.x, v3.y, v4.x, v4.y, 4.0, Color::from_hex(0x950740));
}
The robot is drawn as a square with a line indicating the front (heading direction).

​
Rotation Macro

The rotate! macro transforms local coordinates to world coordinates: 
macro_rules! rotate {
    ($x: expr, $y: expr, $t: expr, $p: expr) => {
        {
            let st = $t.sin();
            let ct = $t.cos();
            Vec2 {
                x: (ct * $x - st * $y) + $p.0, 
                y: (st * $x + ct * $y) + $p.1
            }
        }
    };
}
This implements the 2D rotation matrix: [x′y′]=[cos⁡θ−sin⁡θsin⁡θcos⁡θ][xy]+[pxpy]\begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix} + \begin{bmatrix} p_x \\ p_y \end{bmatrix}

​
Control Loop Integration

The step function integrates seamlessly with movement algorithms: 
while start.elapsed().as_millis() < timeout.into() {
    {
        let mut robot = robot.lock().unwrap();
        let pos = robot.position;
        let heading = robot.heading.to_degrees() % 360.0;
        robot.step(pidMTPVel(pos, heading, target, rotationCut, 
                            &mut lCont, &mut rCont, min));
    }
    thread::sleep(Duration::from_millis(10));
}
The 10ms update rate (100Hz) provides smooth motion simulation.

​
Physical Constraints

Real robots have constraints that the simulation could incorporate: 
let maxVel = 127.0;
let lVel = lVel.clamp(-maxVel, maxVel);
let rVel = rVel.clamp(-maxVel, maxVel);
The current Mars-RS implementation doesn’t enforce velocity limits, but the eulerTurn function shows an example of clamping:
vel = if vel.abs() >= 127.0 {127.0 * vel.signum()} else {vel};

​
Practical Applications

​
Example: Tank Drive

Direct control for teleoperation: 
let leftJoystick = getUserInput().left;   // -127 to 127
let rightJoystick = getUserInput().right; // -127 to 127
robot.step((leftJoystick, rightJoystick));

​
Example: Arcade Drive

Single-stick control: 
let forward = getJoystick().y;   // Forward/backward
let turn = getJoystick().x;      // Left/right
let leftPower = forward + turn;
let rightPower = forward - turn;
robot.step((leftPower, rightPower));

Movement Algorithms

High-level motion control using robot physics

PID Control

Convert errors to wheel velocities

Pure Pursuit

Path following with differential drive

Build docs developers (and LLMs) love

Get started for freeTalk to us