01 | A PROFILE PLANS MORE THAN A TOP SPEED

A slew limiter limits changes in the command it receives. A motion profile plans the desired position, velocity and acceleration for an entire move.

For a straight move, start from rest, accelerate, cruise if there is room, then decelerate to rest at the target. The plan gives your controller a moving reference to follow instead of one abrupt final-position request.

This guide uses physical units: millimetres, seconds and volts. It begins with a forward straight move so the core ideas remain visible.

02 | THREE PHASES FORM A TRAPEZOID

Acceleration has a positive slope, cruise is flat, and deceleration has a negative slope. The area under the velocity-time curve is distance.

A trapezoidal profile keeps acceleration constant within each ramp, but changes it abruptly at the boundaries. It smooths velocity rather than limiting every change in acceleration. More advanced S-curve profiles limit jerk.

03 | CALCULATE THE PLAN

ramp_time = maximum_speed / acceleration
ramp_distance = maximum_speed² / (2 × acceleration)
cruise_distance = total_distance − 2 × ramp_distance
cruise_time = cruise_distance / maximum_speed

Use equal acceleration and deceleration magnitudes for this starter. For a 300 mm move, 200 mm/s maximum speed and 400 mm/s² acceleration:

The robot should reach 200 mm/s after half a second, cruise for one second, then slow for half a second. Actual measured motion may lag this plan.

04 | SHORT MOVES NEED A TRIANGLE

If the acceleration and deceleration distances would exceed the requested distance, there is no room to reach maximum speed. Reduce the peak and remove the cruise phase.

peak_speed = sqrt(distance × acceleration)

For 50 mm at 400 mm/s², peak speed is √20,000 ≈ 141.42 mm/s. Each ramp lasts about 0.354 seconds; total time is about 0.707 seconds.

The plan automatically chooses the smaller of your maximum speed and this triangular peak. Never use a negative cruise distance.

def profile_plan(distance_mm, max_speed_mm_s, accel_mm_s2):
    for value in (distance_mm, max_speed_mm_s, accel_mm_s2):
        if not isinstance(value, (int, float)) or not 0 < value < float("inf"):
            raise ValueError("Profile inputs must be positive and finite")
    peak = min(max_speed_mm_s, math.sqrt(distance_mm * accel_mm_s2))
    ramp_time = peak / accel_mm_s2
    ramp_distance = 0.5 * accel_mm_s2 * ramp_time * ramp_time
    cruise_distance = max(0, distance_mm - 2 * ramp_distance)
    cruise_time = cruise_distance / peak
    total_time = 2 * ramp_time + cruise_time
    return distance_mm, peak, accel_mm_s2, ramp_time, cruise_time, total_time

def profile_sample(plan, elapsed_s):
    distance, peak, accel, ramp_t, cruise_t, total_t = plan
    t = max(0, elapsed_s)
    ramp_distance = 0.5 * accel * ramp_t * ramp_t
    if t >= total_t:
        return distance, 0, 0
    if t < ramp_t:
        return 0.5 * accel * t * t, accel * t, accel
    if t < ramp_t + cruise_t:
        return ramp_distance + peak * (t - ramp_t), peak, 0
    td = t - ramp_t - cruise_t
    position = ramp_distance + peak * cruise_t + peak * td - 0.5 * accel * td * td
    return position, peak - accel * td, -accel

05 | SAMPLE POSITION, VELOCITY AND ACCELERATION

At each loop update, sample the profile using actual elapsed time. During acceleration, v = a × t and x = ½ × a × t². During cruise, velocity is constant. During deceleration, velocity falls while position still increases.

The sampler returns the final position and zero velocity and acceleration after the planned end. That does not make the physical robot arrive automatically.

06 | FEEDFORWARD ESTIMATES REQUIRED EFFORT

Feedforward predicts the command needed for the desired motion before waiting for an error. A simple drivetrain voltage model is:

V_feedforward = kS × sign(velocity) + kV × velocity + kA × acceleration

At constant forward speed, the acceleration term is zero. During forward deceleration, acceleration is negative, so its term reduces the predicted voltage.

The static term uses planned velocity direction. In this starter it becomes zero at zero planned velocity. More advanced controllers can treat intended starting direction and low-speed friction separately.

07 | A NUMERICAL FEEDFORWARD EXAMPLE

For illustration only, suppose kS = 0.4 V, kV = 0.005 V per (mm/s) and kA = 0.001 V per (mm/s²).

Your robot may need very different gains. Wheel size, gearing, friction, mass and mechanical condition change the model. Never copy these illustrative values as proven calibration.

08 | FEEDBACK CORRECTS THE REMAINING ERROR

Even a good model is imperfect. Compare the profile’s target position with encoder-derived travel and add a proportional correction:

position_error = target_position − measured_position
V_command = V_feedforward + kP × position_error

If the profile requests 150 mm but a side has travelled 140 mm, its error is +10 mm. An illustrative kP of 0.02 V/mm adds 0.2 V.

Feedforward does not measure error. Feedback does. This project gives each side its own position correction, using the same straight-line reference.

A proportional-only controller may leave a small low-speed error because friction resists small corrections. A timeout makes that limitation visible. Adding integral or friction compensation needs deliberate tuning rather than arbitrary minimum-output jumps.

09 | CHOOSE THE RIGHT VEX COMMAND

The autonomous example sends signed voltage commands using MotorGroup.spin with VOLT. This lets the team-written feedforward and position controller own the output directly.

if volts > 0:
    side.spin(FORWARD, volts, VOLT)
else:
    side.spin(REVERSE, -volts, VOLT)

A percent or RPM velocity command asks the motor’s internal velocity controller to track a speed. It is a different control arrangement; do not describe it as direct voltage feedforward.

Clamp requested voltage to a chosen test limit. If it frequently saturates, the profile may demand more speed or acceleration than the available effort can deliver. Log unclamped commands so this remains visible.

Do not run this controller alongside SmartDrive or spin_for commands that also control the same motors.

10 | COMPLETE CALIBRATION-GATED PROJECT

Keep generated imports and Brain setup above this block, including import math. Paste below # Begin project code, outside the generated region, replacing the previous starter. Match all motor ports, cartridges and reversal flags and avoid duplicate device definitions.

The example assumes direct drive with equal calibrated wheels. Its feedforward and feedback gains start at zero, and PROFILE_CALIBRATED starts False. Autonomous reports CALIBRATION_REQUIRED until your measured and tuned values are entered and the flag is deliberately enabled.

Driver control remains the earlier basic split-stick scheme and can still move the robot when enabled. The gate applies to the autonomous profile, not the whole project.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER]
# =============================================================================
# ROBOT CONFIGURATION
# - FOUR-motor drivetrain:
#       Left  = Ports 3 and 4
#       Right = Ports 1 and 2
# - Example: blue cartridges, left normal, right reversed.
#   VERIFY your actual ports, cartridges and direction settings.
# DRIVER CONTROLS
# - Left joystick vertical (axis3): forward/back
# - Right joystick horizontal (axis1): turn
# - Autonomous profile is disabled until PROFILE_CALIBRATED is True.
# - Driver control still MOVES motors when enabled.
# - Hold B to cancel autonomous; driver controls retain the earlier scheme.
# - Add BELOW # Begin project code, outside generated configuration.
# - Do not also define these drive motors in the Devices panel.
# - Keep generated from vex import * and brain = Brain() above.

# =============================================================================
# FOUR-MOTOR DRIVETRAIN
# =============================================================================
left_motor_1 = Motor(Ports.PORT3, GearSetting.RATIO_6_1, False)
left_motor_2 = Motor(Ports.PORT4, GearSetting.RATIO_6_1, False)
right_motor_1 = Motor(Ports.PORT1, GearSetting.RATIO_6_1, True)
right_motor_2 = Motor(Ports.PORT2, GearSetting.RATIO_6_1, True)

left_drive = MotorGroup(left_motor_1, left_motor_2)
right_drive = MotorGroup(right_motor_1, right_motor_2)

# =============================================================================
# CONTROLLER, SETTINGS AND HELPERS
# =============================================================================
controller = Controller(PRIMARY)  # Do not duplicate a generated Controller
DEADBAND = 5
DRIVE_LIMIT_PERCENT = 30          # Start low; increase only after testing
TURN_GAIN = 0.7
LOOP_DELAY_MS = 20
WHEEL_DIAMETER_MM = 101.6
WHEEL_TRAVEL_MM = math.pi * WHEEL_DIAMETER_MM  # Direct drive, equal wheels
MAX_SPEED_MM_S = 200
ACCEL_MM_S2 = 400
MAX_AUTO_VOLTS = 3.0           # Voltage clamp for initial tests
PROFILE_CALIBRATED = False    # Enable only after measuring and tuning gains
# Replace these zero placeholders with measured/tuned values.
KS_VOLTS = 0.0
KV_VOLTS_PER_MM_S = 0.0
KA_VOLTS_PER_MM_S2 = 0.0
KP_VOLTS_PER_MM = 0.0
POSITION_TOLERANCE_MM = 10
SETTLED_SPEED_MM_S = 10
SETTLED_DWELL_MS = 150
SETTLE_ALLOWANCE_S = 2.0

def apply_deadband(value):
    return 0 if abs(value) <= DEADBAND else value

def drive_mix(forward, turn):
    left = forward + turn
    right = forward - turn
    largest = max(100, abs(left), abs(right))
    left = left * 100 / largest
    right = right * 100 / largest
    return (left * DRIVE_LIMIT_PERCENT / 100,
            right * DRIVE_LIMIT_PERCENT / 100)

def set_side_output(side, output):
    if abs(output) < 0.01:
        side.stop(BRAKE)
    elif output > 0:
        side.spin(FORWARD, output, PERCENT)
    else:
        side.spin(REVERSE, -output, PERCENT)

def profile_plan(distance_mm, max_speed_mm_s, accel_mm_s2):
    for value in (distance_mm, max_speed_mm_s, accel_mm_s2):
        if not isinstance(value, (int, float)) or not 0 < value < float("inf"):
            raise ValueError("Profile inputs must be positive and finite")
    peak = min(max_speed_mm_s, math.sqrt(distance_mm * accel_mm_s2))
    ramp_time = peak / accel_mm_s2
    ramp_distance = 0.5 * accel_mm_s2 * ramp_time * ramp_time
    cruise_distance = max(0, distance_mm - 2 * ramp_distance)
    cruise_time = cruise_distance / peak
    total_time = 2 * ramp_time + cruise_time
    return distance_mm, peak, accel_mm_s2, ramp_time, cruise_time, total_time

def profile_sample(plan, elapsed_s):
    distance, peak, accel, ramp_t, cruise_t, total_t = plan
    t = max(0, elapsed_s)
    ramp_distance = 0.5 * accel * ramp_t * ramp_t
    if t >= total_t:
        return distance, 0, 0
    if t < ramp_t:
        return 0.5 * accel * t * t, accel * t, accel
    if t < ramp_t + cruise_t:
        return ramp_distance + peak * (t - ramp_t), peak, 0
    td = t - ramp_t - cruise_t
    position = ramp_distance + peak * cruise_t + peak * td - 0.5 * accel * td * td
    return position, peak - accel * td, -accel

def feedforward(speed_mm_s, accel_mm_s2):
    # Static term follows planned velocity; it is zero when velocity is zero.
    sign = 1 if speed_mm_s > 0 else (-1 if speed_mm_s < 0 else 0)
    return KS_VOLTS * sign + KV_VOLTS_PER_MM_S * speed_mm_s + KA_VOLTS_PER_MM_S2 * accel_mm_s2

def apply_voltage(side, volts):
    volts = max(-MAX_AUTO_VOLTS, min(MAX_AUTO_VOLTS, volts))
    if abs(volts) < 0.01:
        side.stop(BRAKE)
    else:
        direction = FORWARD if volts > 0 else REVERSE
        side.spin(direction, abs(volts), VOLT)

def profiled_forward(distance_mm):
    plan = profile_plan(distance_mm, MAX_SPEED_MM_S, ACCEL_MM_S2)
    if not PROFILE_CALIBRATED:
        stop_drive()
        return "CALIBRATION_REQUIRED"
    motors = (left_motor_1, left_motor_2, right_motor_1, right_motor_2)
    starts = [motor.position(DEGREES) for motor in motors]
    start_ms = brain.timer.time(MSEC)
    deadline_s = plan[-1] + SETTLE_ALLOWANCE_S
    settled_start_ms = None
    report_ms = start_ms
    try:
        while True:
            if not (competition.is_enabled() and competition.is_autonomous()):
                return "MODE_CHANGED"
            if controller.buttonB.pressing():
                return "CANCELLED"
            now_ms = brain.timer.time(MSEC)
            elapsed_s = (now_ms - start_ms) / 1000
            if elapsed_s >= deadline_s:
                return "TIMEOUT"
            target_mm, target_speed, target_accel = profile_sample(plan, elapsed_s)
            travel = [(motor.position(DEGREES) - start) / 360 * WHEEL_TRAVEL_MM
                      for motor, start in zip(motors, starts)]
            left_mm = (travel[0] + travel[1]) / 2
            right_mm = (travel[2] + travel[3]) / 2
            ff = feedforward(target_speed, target_accel)
            left_voltage = ff + KP_VOLTS_PER_MM * (target_mm - left_mm)
            right_voltage = ff + KP_VOLTS_PER_MM * (target_mm - right_mm)

            # Completion needs the profile to end AND all motor readings to settle.
            speeds = [abs(motor.velocity(RPM)) / 60 * WHEEL_TRAVEL_MM
                      for motor in motors]
            near_target = max(abs(distance_mm - x) for x in travel) <= POSITION_TOLERANCE_MM
            slow = max(speeds) <= SETTLED_SPEED_MM_S
            if elapsed_s >= plan[-1] and near_target and slow:
                if settled_start_ms is None:
                    settled_start_ms = now_ms
                if now_ms - settled_start_ms >= SETTLED_DWELL_MS:
                    return "ENCODER_TARGET_SETTLED"
            else:
                settled_start_ms = None

            apply_voltage(left_drive, left_voltage)
            apply_voltage(right_drive, right_voltage)
            if now_ms >= report_ms:
                print("PROFILE", elapsed_s, target_mm, target_speed,
                      left_mm, right_mm, left_voltage, right_voltage)
                report_ms = now_ms + 200
            wait(LOOP_DELAY_MS, MSEC)
    finally:
        stop_drive()

def show_status(message):
    brain.screen.clear_screen()
    brain.screen.set_cursor(1, 1)
    brain.screen.print(message)

def stop_drive():
    left_drive.stop(BRAKE)
    right_drive.stop(BRAKE)

# =============================================================================
# PRE-AUTON: CALLED ONCE AT PROJECT STARTUP
# =============================================================================
def pre_autonomous():
    stop_drive()
    show_status("Setup ready")
    # Later: initialise sensors and starting values here.
    # Finish setup before the robot is enabled for a match.

# =============================================================================
# AUTONOMOUS: A FINITE ROUTINE
# =============================================================================
def autonomous():
    show_status("Profiled forward test")
    result = profiled_forward(300)
    show_status(result)
    # No next movement: inspect logs and physical travel.

# =============================================================================
# DRIVER CONTROL: REPEATING UPDATES WHILE ENABLED
# =============================================================================
def driver_control():
    show_status("Driver control")
    while competition.is_enabled() and competition.is_driver_control():
        forward = apply_deadband(controller.axis3.position())
        turn = apply_deadband(controller.axis1.position()) * TURN_GAIN
        left_output, right_output = drive_mix(forward, turn)
        set_side_output(left_drive, left_output)
        set_side_output(right_drive, right_output)
        wait(LOOP_DELAY_MS, MSEC)
    stop_drive()

# =============================================================================
# PROGRAM START: SETUP, THEN CALLBACK REGISTRATION
# =============================================================================
pre_autonomous()
competition = Competition(driver_control, autonomous)

# Keep the main task alive without running a driving loop here.
while True:
    wait(100, MSEC)

The controller may command a small reverse correction after overshooting. Allow clear space around the target. Holding B, leaving autonomous or reaching the overall deadline stops the drivetrain.

Completion requires the plan to end and all four motor readings to stay near the target at low speed for a dwell interval. That verifies an encoder-based result, not ground position or a perfectly straight path.

11 | MEASURE GAINS BEFORE ENABLING THE PROFILE

1. Verify ports, encoder signs, wheel travel and direct-drive assumptions.
2. Run separate bounded low-voltage tests in a clear area, with explicit stop and cancel conditions.
3. Measure steady wheel speed from output-shaft RPM using wheel circumference. With direct drive, speed in mm/s = RPM ÷ 60 × wheel travel.
4. Use several steady-speed voltage measurements to estimate a relationship V ≈ kS + kV × speed. Inspect whether that relationship is useful for your operating range.
5. Begin profile tests with modest targets, measured kS/kV, kA = 0 and a conservative position gain. Tune one change at a time.
6. Only introduce kA from measured transient behaviour; acceleration estimates are noisy, so avoid inferring it from a single trial.

These are horizontal drivetrain gains. A lift would need a different model, including gravity compensation. Do not reuse drivetrain values for a loaded vertical mechanism.

If the example’s voltage limit cannot achieve your desired motion, reduce the profile constraints before increasing output. Retest with the actual payload and battery conditions.

12 | LOG THE REFERENCE AND THE RESPONSE

The sample prints elapsed time, target position, target velocity, both side positions and raw voltage requests every 200 ms. Use more frequent logging only if it does not materially disturb the control loop.

Plot target and measured position if possible. Consistent lag, oscillation and saturation suggest different problems. Do not respond to all three by simply increasing kP.

13 | APPLY ONE PROFILE TO AN ARC

For the side distances from 03.4, choose a common progress profile. Scale its position, velocity and acceleration references by each side’s share of the longest path.

left_reference = common_reference × left_distance / longest_distance
right_reference = common_reference × right_distance / longest_distance

Apply that scaling to all three reference quantities, not just speed. Each side then follows its own consistent position and velocity plan with a shared timeline.

This is an extension to design and test after straight-line tracking works. A stationary side, signed reverse travel and unequal gearing need explicit handling.

CHALLENGES | PLAN, PREDICT AND VERIFY

1. Calculate: plan a 600 mm move with 300 mm/s maximum speed and 600 mm/s² acceleration.
2. Identify: determine whether a 60 mm move with the same limits is trapezoidal or triangular.
3. Sample: predict position and velocity at several points in each plan.
4. Explain: distinguish feedforward from position feedback.
5. Check units: state the units of kS, kV, kA and kP in this guide.
6. Measure: collect bounded steady-speed measurements before enabling the profile.
7. Evaluate: log target and measured travel and identify saturation or lag.
8. Extend: design a shared timeline for inside and outside arc references.

OFFICIAL REFERENCES

• VEX motor voltage commands and encoder readings
• WPILib explanation of feedforward models

WPILib is a reference for the control concepts; this project uses VEX Python and does not import WPILib. The profile and controller are team-written logic.