Forward Kinematics

Mecanum Drive Simulator

Drive with X/Y/yaw — via the sliders, or the keyboard: W/S for x, A/D for y, </> for yaw. The drive-mixing function turns these into four wheel speeds, which are fed into forward kinematics to move the chassis.

Drive Input

xforward W / S
0
yright D / A
0
yawclockwise > / <
0
ch5reserve switch
BOOST0% reserve
Hold a key to command 100% on that axis, release to return to 0%. Opposing keys held together cancel out.

Wheel Speeds

▲ front
FLfront-left
0
FRfront-right
0
RLrear-left
0
RRrear-right
0
rear
Kinematics used
// worst case: |x|+|y|+|yaw| = 300 when all three are maxed => k = 3
// zero-clipping reserve = (1 − 100/300)·100 = 66.7%, gated by ch5
s   = (100 − reserve) / 100  reserve = ch5 > 0 ? 66.7 : 0
x,y,yaw ×= s

FR = x + y + yaw   FL = x − y − yaw
RR = x − y + yaw   RL = x + y − yaw
k  = max(|FR|,|FL|,|RR|,|RL|) / 100
if k > 1: wheel /= k  // else pass through unscaled

// forward kinematics: wheel input mapped [-255,255] to angular rate [rad/s]
w = (input / 255) · maxRate

vx   = r/4 · ( FL + FR + RL + RR )
vy   = r/4 · (−FL + FR + RL − RR )
ω    = r / (4·(L+W)) · (−FL + FR − RL + RR )

// world-frame integration, θ clockwise-positive
ẋ = vx·sinθ + vy·cosθ   ẏ = −vx·cosθ + vy·sinθ  θ̇ = ω
TOP-DOWN VIEW ● running
0.5 m