Performance Optimization
Introduction to Performance Optimization
Performance optimization is crucial for FTC robots. Your code must run efficiently to maintain consistent control loop timing and prevent crashes. This lesson covers techniques to optimize your robot's performance while maintaining reliability.
Why Performance Matters in FTC
- FTC robots require consistent 50Hz loop rates for reliable operation
- Android devices have limited memory and processing power
- Performance issues can cause robot crashes during competition
- Optimized code runs more smoothly and predictably
Loop Timing and Performance Monitoring
FTC robots operate on a tight timing schedule. The main control loop should run at approximately 50Hz (20ms per loop) to maintain smooth robot control. Monitoring loop timing helps identify performance bottlenecks.
Loop Timing Monitor - Basic Setup
public class PerformanceMonitorOpMode extends OpMode {
private static final String TAG = "PerformanceMonitor";
private static final double TARGET_LOOP_TIME = 0.020; // 20ms = 50Hz
private long lastLoopTime;
private double averageLoopTime = 0.0;
private int loopCount = 0;
private double maxLoopTime = 0.0;
private double minLoopTime = Double.MAX_VALUE;
// Performance tracking variables
private long initStartTime;
private long initEndTime;
private long loopStartTime;
@Override
public void init() {
initStartTime = System.nanoTime();
// Initialize hardware
initializeHardware();
initEndTime = System.nanoTime();
double initTime = (initEndTime - initStartTime) / 1_000_000.0; // Convert to milliseconds
Log.i(TAG, "Initialization completed in " + initTime + "ms");
telemetry.addData("Init Time (ms)", String.format("%.2f", initTime));
lastLoopTime = System.nanoTime();
}Understanding Loop Timing
Loop timing is critical for FTC robots. The main control loop should run at approximately 50Hz (20ms per loop) to maintain smooth robot control. Monitoring loop timing helps identify when your code is taking too long and potentially causing robot lag or missed commands.
Loop Timing Monitor - Main Loop and Statistics
@Override
public void loop() {
loopStartTime = System.nanoTime();
// Your main robot logic here
processGamepadInput();
updateRobotState();
controlMotors();
// Calculate loop timing
long currentTime = System.nanoTime();
double loopTime = (currentTime - loopStartTime) / 1_000_000.0; // Convert to milliseconds
// Update timing statistics
updateTimingStats(loopTime);
// Display performance metrics every 50 loops
if (loopCount % 50 == 0) {
displayPerformanceMetrics();
}
loopCount++;
lastLoopTime = currentTime;
}
private void updateTimingStats(double loopTime) {
// Update running average
averageLoopTime = (averageLoopTime * (loopCount - 1) + loopTime) / loopCount;
// Update min/max
if (loopTime > maxLoopTime) maxLoopTime = loopTime;
if (loopTime < minLoopTime) minLoopTime = loopTime;
// Log performance warnings
if (loopTime > TARGET_LOOP_TIME * 1.5) { // 30ms
Log.w(TAG, "Loop time exceeded target: " + loopTime + "ms");
}
}Performance Statistics and Monitoring
The performance monitor tracks key metrics including average, minimum, and maximum loop times. It also provides warnings when loop times exceed the target, helping you identify performance bottlenecks before they cause robot issues.
Loop Timing Monitor - Display and Helper Methods
private void displayPerformanceMetrics() {
telemetry.addData("=== Performance Metrics ===", "");
telemetry.addData("Current Loop Time (ms)", String.format("%.2f", (System.nanoTime() - loopStartTime) / 1_000_000.0));
telemetry.addData("Average Loop Time (ms)", String.format("%.2f", averageLoopTime));
telemetry.addData("Min Loop Time (ms)", String.format("%.2f", minLoopTime));
telemetry.addData("Max Loop Time (ms)", String.format("%.2f", maxLoopTime));
telemetry.addData("Loop Count", loopCount);
telemetry.addData("Target Loop Time (ms)", String.format("%.2f", TARGET_LOOP_TIME * 1000));
// Performance status
String status = averageLoopTime < TARGET_LOOP_TIME * 1000 ? "GOOD" : "SLOW";
telemetry.addData("Performance Status", status);
}
private void initializeHardware() {
// Initialize your hardware components here
// This is where you'd set up motors, servos, sensors, etc.
}
private void processGamepadInput() {
// Process gamepad input efficiently
// Avoid complex calculations here
}
private void updateRobotState() {
// Update robot state and calculations
// This is where most of your logic should go
}
private void controlMotors() {
// Set motor powers and servo positions
// Keep this simple and fast
}
}Performance Display and Robot Logic
The display method shows performance metrics on the Driver Station, helping you monitor robot performance in real-time. The helper methods are placeholders for your actual robot logic, organized to maintain good performance.
Memory Management in FTC Applications
Android devices have limited memory, and inefficient memory usage can cause your robot to crash. Understanding memory management is essential for creating stable FTC applications.
Memory-Efficient Programming - Basic Setup
public class MemoryEfficientOpMode extends OpMode {
private static final String TAG = "MemoryEfficient";
// Reuse objects instead of creating new ones
private final StringBuilder telemetryBuilder = new StringBuilder();
private final double[] motorPowers = new double[4]; // Reuse array
private final int[] sensorReadings = new int[3]; // Reuse array
// Object pooling for frequently used objects
private final Queue<RobotCommand> commandPool = new LinkedList<>();
private final int MAX_POOL_SIZE = 10;
// Avoid creating objects in the main loop
private double lastTelemetryTime = 0.0;
private static final double TELEMETRY_INTERVAL = 0.1; // 100ms
@Override
public void init() {
// Pre-allocate objects that will be used frequently
initializeObjectPool();
// Initialize hardware with memory-efficient approach
initializeHardwareEfficiently();
}Understanding Memory-Efficient Setup
The memory-efficient OpMode reuses objects instead of creating new ones in each loop iteration. Object pooling prevents garbage collection issues that can cause robot lag or crashes. Pre-allocated arrays and StringBuilder reuse reduce memory allocation overhead.
Memory-Efficient Programming - Object Pool and Hardware Initialization
private void initializeObjectPool() {
// Pre-create objects for the pool
for (int i = 0; i < MAX_POOL_SIZE; i++) {
commandPool.offer(new RobotCommand());
}
}
private void initializeHardwareEfficiently() {
// Initialize hardware without creating unnecessary objects
try {
// Use direct hardware map access
DcMotor leftMotor = hardwareMap.get(DcMotor.class, "left_motor");
DcMotor rightMotor = hardwareMap.get(DcMotor.class, "right_motor");
// Configure motors efficiently
leftMotor.setDirection(DcMotor.Direction.FORWARD);
rightMotor.setDirection(DcMotor.Direction.REVERSE);
Log.i(TAG, "Hardware initialized efficiently");
} catch (Exception e) {
Log.e(TAG, "Hardware initialization failed", e);
}
}Understanding Object Pooling and Hardware Initialization
Object pooling pre-creates frequently used objects to avoid runtime allocation. The hardware initialization method avoids creating unnecessary objects during setup, using direct hardware map access and minimal configuration.
Memory-Efficient Programming - Main Loop and Input Processing
@Override
public void loop() {
// Process input efficiently
processInputEfficiently();
// Update robot state
updateRobotStateEfficiently();
// Update telemetry periodically to reduce overhead
if (getRuntime() - lastTelemetryTime > TELEMETRY_INTERVAL) {
updateTelemetryEfficiently();
lastTelemetryTime = getRuntime();
}
}
private void processInputEfficiently() {
// Reuse arrays instead of creating new ones
motorPowers[0] = gamepad1.left_stick_y;
motorPowers[1] = gamepad1.right_stick_y;
motorPowers[2] = gamepad1.left_trigger;
motorPowers[3] = gamepad1.right_trigger;
// Apply deadband efficiently
for (int i = 0; i < motorPowers.length; i++) {
if (Math.abs(motorPowers[i]) < 0.1) {
motorPowers[i] = 0.0;
}
}
}Understanding Efficient Input Processing
The input processing method reuses pre-allocated arrays instead of creating new objects. The deadband application uses a simple loop without creating intermediate objects, maintaining performance while providing smooth control.
Memory-Efficient Programming - Robot State and Command Execution
private void updateRobotStateEfficiently() {
// Get command from pool instead of creating new one
RobotCommand command = getCommandFromPool();
if (command != null) {
command.setMotorPowers(motorPowers);
executeCommand(command);
returnCommandToPool(command);
}
}
private RobotCommand getCommandFromPool() {
return commandPool.poll();
}
private void returnCommandToPool(RobotCommand command) {
if (commandPool.size() < MAX_POOL_SIZE) {
command.reset(); // Reset the command for reuse
commandPool.offer(command);
}
}
private void executeCommand(RobotCommand command) {
// Execute the command efficiently
// Set motor powers directly without creating intermediate objects
DcMotor leftMotor = hardwareMap.get(DcMotor.class, "left_motor");
DcMotor rightMotor = hardwareMap.get(DcMotor.class, "right_motor");
leftMotor.setPower(command.getLeftPower());
rightMotor.setPower(command.getRightPower());
}Understanding Object Pooling for Commands
The robot state update uses object pooling to avoid creating new RobotCommand objects. Commands are retrieved from the pool, used, reset, and returned to the pool for reuse. This prevents garbage collection and maintains consistent performance.
Memory-Efficient Programming - Telemetry and Command Class
private void updateTelemetryEfficiently() {
// Use StringBuilder to build telemetry efficiently
telemetryBuilder.setLength(0); // Clear without creating new object
telemetryBuilder.append("Loop Time: ").append(getRuntime()).append("s");
telemetry.addData("Status", telemetryBuilder.toString());
telemetry.addData("Left Power", motorPowers[0]);
telemetry.addData("Right Power", motorPowers[1]);
}
// Efficient command class for object pooling
private static class RobotCommand {
private double leftPower = 0.0;
private double rightPower = 0.0;
public void setMotorPowers(double[] powers) {
this.leftPower = powers[0];
this.rightPower = powers[1];
}
public double getLeftPower() { return leftPower; }
public double getRightPower() { return rightPower; }
public void reset() {
leftPower = 0.0;
rightPower = 0.0;
}
}
}Understanding Efficient Telemetry and Command Class
The telemetry method reuses StringBuilder by clearing it instead of creating a new one. The RobotCommand class is designed for object pooling with a reset method that prepares the object for reuse without creating new instances.
Algorithm Optimization for Robot Control
Complex algorithms can slow down your main control loop. Breaking down complex calculations into smaller, more efficient pieces helps maintain performance.
Optimized PID Control Implementation - Basic Structure
public class OptimizedPIDController {
private double kp, ki, kd;
private double setpoint;
private double previousError = 0.0;
private double integral = 0.0;
private double output = 0.0;
// Pre-calculated constants to avoid repeated calculations
private double integralLimit;
private double outputLimit;
private boolean isEnabled = false;
public OptimizedPIDController(double kp, double ki, double kd) {
this.kp = kp;
this.ki = ki;
this.kd = kd;
// Pre-calculate limits for efficiency
this.integralLimit = 1.0 / ki; // Prevent integral windup
this.outputLimit = 1.0; // Limit output to motor range
}Understanding Optimized PID Structure
The optimized PID controller pre-calculates constants to avoid repeated calculations during runtime. The integral and output limits are calculated once during initialization, improving performance during the main control loop.
Optimized PID Control - Setpoint and Calculation Methods
public void setSetpoint(double setpoint) {
this.setpoint = setpoint;
// Reset integral when setpoint changes to prevent windup
this.integral = 0.0;
}
public double calculate(double measurement) {
if (!isEnabled) return 0.0;
// Calculate error efficiently
double error = setpoint - measurement;
// Proportional term
double pTerm = kp * error;
// Integral term with anti-windup
integral += error;
if (Math.abs(integral) > integralLimit) {
integral = Math.signum(integral) * integralLimit;
}
double iTerm = ki * integral;
// Derivative term (avoid division by zero)
double derivative = error - previousError;
double dTerm = kd * derivative;
// Calculate output efficiently
output = pTerm + iTerm + dTerm;
// Limit output
if (Math.abs(output) > outputLimit) {
output = Math.signum(output) * outputLimit;
}
// Store error for next iteration
previousError = error;
return output;
}Understanding PID Calculation Optimization
The PID calculation method is optimized to minimize computational overhead. It uses pre-calculated limits, avoids division operations, and efficiently combines the PID terms. The anti-windup protection prevents integral term issues.
Optimized PID Control - Control Methods and OpMode Integration
public void enable() { isEnabled = true; }
public void disable() { isEnabled = false; }
public void reset() {
integral = 0.0;
previousError = 0.0;
output = 0.0;
}
}
// Optimized OpMode using the PID controller
public class OptimizedPIDOpMode extends OpMode {
private OptimizedPIDController drivePID;
private OptimizedPIDController turnPID;
private DcMotor leftMotor, rightMotor;
// Pre-allocated variables to avoid object creation
private double targetDistance = 0.0;
private double targetAngle = 0.0;
private double currentDistance = 0.0;
private double currentAngle = 0.0;
@Override
public void init() {
// Initialize PID controllers efficiently
drivePID = new OptimizedPIDController(0.5, 0.1, 0.05);
turnPID = new OptimizedPIDController(0.3, 0.05, 0.02);
// Initialize hardware
leftMotor = hardwareMap.get(DcMotor.class, "left_motor");
rightMotor = hardwareMap.get(DcMotor.class, "right_motor");
// Enable PID controllers
drivePID.enable();
turnPID.enable();
}Understanding PID Control Methods and OpMode Integration
The control methods provide simple enable/disable functionality and reset capabilities. The OpMode integration uses pre-allocated variables to avoid object creation during the main loop, maintaining performance while providing sophisticated control.
Optimized PID OpMode - Main Loop and Sensor Updates
@Override
public void loop() {
// Update sensor readings efficiently
updateSensorReadings();
// Calculate PID outputs efficiently
double driveOutput = drivePID.calculate(currentDistance);
double turnOutput = turnPID.calculate(currentAngle);
// Apply outputs to motors efficiently
double leftPower = driveOutput + turnOutput;
double rightPower = driveOutput - turnOutput;
// Limit powers efficiently
leftPower = Math.max(-1.0, Math.min(1.0, leftPower));
rightPower = Math.max(-1.0, Math.min(1.0, rightPower));
// Set motor powers
leftMotor.setPower(leftPower);
rightMotor.setPower(rightPower);
// Update telemetry periodically
if (getRuntime() % 0.1 < 0.01) { // Every 100ms
telemetry.addData("Target Distance", targetDistance);
telemetry.addData("Current Distance", currentDistance);
telemetry.addData("Drive Output", driveOutput);
telemetry.addData("Left Power", leftPower);
telemetry.addData("Right Power", rightPower);
}
}
private void updateSensorReadings() {
// Update sensor readings efficiently
// This would typically read from encoders, IMU, etc.
// For this example, we'll simulate readings
currentDistance = getRuntime() * 0.1; // Simulate distance
currentAngle = Math.sin(getRuntime()) * 0.1; // Simulate angle
}
}Understanding Optimized PID Loop
The main loop efficiently combines PID outputs and applies them to motors. Power limiting uses Math.max/min for efficiency, and telemetry updates are periodic to reduce overhead. The sensor update method simulates readings for demonstration purposes.
Resource Management (Motors, Servos, Sensors)
Proper resource management prevents conflicts and improves performance. Efficient use of motors, servos, and sensors ensures smooth robot operation.
Efficient Resource Management - Basic Setup
public class ResourceManagerOpMode extends OpMode {
private static final String TAG = "ResourceManager";
// Hardware components
private DcMotor[] driveMotors = new DcMotor[4];
private Servo[] servos = new Servo[2];
private ColorSensor colorSensor;
private DistanceSensor distanceSensor;
// Resource state tracking
private boolean[] motorBusy = new boolean[4];
private boolean[] servoBusy = new boolean[2];
private long lastSensorRead = 0;
private static final long SENSOR_READ_INTERVAL = 50; // 50ms between reads
// Pre-allocated arrays for efficiency
private double[] motorPowers = new double[4];
private double[] servoPositions = new double[2];
private int[] sensorReadings = new int[3];
@Override
public void init() {
initializeHardwareEfficiently();
resetResourceStates();
}Understanding Resource Management Setup
The resource manager uses arrays to efficiently manage multiple hardware components. State tracking prevents conflicts, and pre-allocated arrays avoid object creation during the main loop. Sensor reading intervals prevent excessive sensor access.
Efficient Resource Management - Hardware Initialization
private void initializeHardwareEfficiently() {
try {
// Initialize drive motors efficiently
String[] motorNames = {"front_left", "front_right", "back_left", "back_right"};
for (int i = 0; i < 4; i++) {
driveMotors[i] = hardwareMap.get(DcMotor.class, motorNames[i]);
driveMotors[i].setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
// Initialize servos efficiently
String[] servoNames = {"arm_servo", "claw_servo"};
for (int i = 0; i < 2; i++) {
servos[i] = hardwareMap.get(Servo.class, servoNames[i]);
}
// Initialize sensors efficiently
colorSensor = hardwareMap.get(ColorSensor.class, "color_sensor");
distanceSensor = hardwareMap.get(DistanceSensor.class, "distance_sensor");
Log.i(TAG, "Hardware initialized successfully");
} catch (Exception e) {
Log.e(TAG, "Hardware initialization failed", e);
}
}
private void resetResourceStates() {
// Reset all resource states
for (int i = 0; i < 4; i++) {
motorBusy[i] = false;
motorPowers[i] = 0.0;
}
for (int i = 0; i < 2; i++) {
servoBusy[i] = false;
servoPositions[i] = 0.5; // Default position
}
lastSensorRead = 0;
}Understanding Hardware Initialization and State Management
The hardware initialization uses arrays and loops to efficiently set up multiple components. The resource state reset method initializes all tracking variables to safe default values, ensuring consistent robot behavior.
Efficient Resource Management - Main Loop and Input Processing
@Override
public void loop() {
// Process input efficiently
processInputEfficiently();
// Update sensors periodically
updateSensorsEfficiently();
// Apply motor controls efficiently
applyMotorControls();
// Apply servo controls efficiently
applyServoControls();
// Update telemetry periodically
if (getRuntime() % 0.2 < 0.01) { // Every 200ms
updateTelemetryEfficiently();
}
}
private void processInputEfficiently() {
// Process gamepad input and update motor powers efficiently
double forward = -gamepad1.left_stick_y;
double turn = gamepad1.right_stick_x;
// Calculate motor powers efficiently
motorPowers[0] = forward + turn; // Front left
motorPowers[1] = forward - turn; // Front right
motorPowers[2] = forward + turn; // Back left
motorPowers[3] = forward - turn; // Back right
// Apply deadband efficiently
for (int i = 0; i < 4; i++) {
if (Math.abs(motorPowers[i]) < 0.1) {
motorPowers[i] = 0.0;
}
}
// Process servo controls
if (gamepad1.a) {
servoPositions[0] = 1.0; // Arm up
} else if (gamepad1.b) {
servoPositions[0] = 0.0; // Arm down
}
if (gamepad1.x) {
servoPositions[1] = 1.0; // Claw open
} else if (gamepad1.y) {
servoPositions[1] = 0.0; // Claw closed
}
}Understanding Efficient Input Processing
The input processing method efficiently calculates motor powers for a four-wheel drive system using arrays. Deadband application uses a simple loop, and servo controls are handled with direct button mapping for responsiveness.
Efficient Resource Management - Sensor Updates and Motor Controls
private void updateSensorsEfficiently() {
long currentTime = System.currentTimeMillis();
if (currentTime - lastSensorRead > SENSOR_READ_INTERVAL) {
// Read sensors efficiently
if (colorSensor != null) {
sensorReadings[0] = colorSensor.red();
sensorReadings[1] = colorSensor.green();
sensorReadings[2] = colorSensor.blue();
}
lastSensorRead = currentTime;
}
}
private void applyMotorControls() {
// Apply motor powers efficiently
for (int i = 0; i < 4; i++) {
if (driveMotors[i] != null && !motorBusy[i]) {
driveMotors[i].setPower(motorPowers[i]);
}
}
}
private void applyServoControls() {
// Apply servo positions efficiently
for (int i = 0; i < 2; i++) {
if (servos[i] != null && !servoBusy[i]) {
servos[i].setPosition(servoPositions[i]);
}
}
}Understanding Sensor Updates and Motor Controls
Sensor updates are throttled to prevent excessive reading frequency. Motor and servo controls check for null components and busy states to prevent conflicts. The array-based approach efficiently manages multiple components.
Efficient Resource Management - Telemetry and Control Methods
private void updateTelemetryEfficiently() {
telemetry.addData("=== Resource Status ===", "");
telemetry.addData("Front Left Motor", motorPowers[0]);
telemetry.addData("Front Right Motor", motorPowers[1]);
telemetry.addData("Back Left Motor", motorPowers[2]);
telemetry.addData("Back Right Motor", motorPowers[3]);
telemetry.addData("Arm Position", servoPositions[0]);
telemetry.addData("Claw Position", servoPositions[1]);
if (colorSensor != null) {
telemetry.addData("Color R", sensorReadings[0]);
telemetry.addData("Color G", sensorReadings[1]);
telemetry.addData("Color B", sensorReadings[2]);
}
}
// Method to mark resources as busy (for complex operations)
public void setMotorBusy(int motorIndex, boolean busy) {
if (motorIndex >= 0 && motorIndex < 4) {
motorBusy[motorIndex] = busy;
}
}
public void setServoBusy(int servoIndex, boolean busy) {
if (servoIndex >= 0 && servoIndex < 2) {
servoBusy[servoIndex] = busy;
}
}
}Understanding Telemetry and Resource Control
The telemetry method efficiently displays all resource statuses using pre-allocated arrays. The busy control methods allow external code to mark resources as unavailable, preventing conflicts during complex operations.
Practice: Performance Optimization
Practice optimizing FTC code performance with these exercises:
- Create a basic OpMode and measure its performance
- Identify performance bottlenecks in the code
- Optimize the OpMode using the techniques learned
- Measure the performance improvements
- Create a comprehensive optimized OpMode