Comments and Documentation

// WORKAROUND: IMU occasionally returns NaN on first read.
// Clear cache by reading twice during initialization.
imu.getRobotYawPitchRollAngles();
sleep(50);
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);

// TODO: Add timeout to prevent infinite loop if motor stalls
while (!arm.isAtTarget()) {
    sleep(10);
}

// FIXME: This calibration value is wrong for the new wheels
private static final int TICKS_PER_INCH = 50;

// NOTE: Alliance color must be set on Driver Station before running
if (allianceColor == Alliance.RED) {
    // ...
}

// Bad - code already says this
int count = 0; // Set count to 0
count++; // Increment count
motor.setPower(0); // Set motor power to 0

// Good - no comment needed, code is self-explanatory
int count = 0;
count++;
motor.setPower(0);

// Bad - comment doesn't match code anymore
// Set servo to open position (0.5)
clawServo.setPosition(0.75); // Position changed but comment didn't update

// Good - keep comments in sync or remove them
clawServo.setPosition(CLAW_OPEN_POSITION);

// Bad
/**
 * Gets the position
 * @return the position
 */
public int getPosition() {
    return position;
}

// Good - only comment if there's something non-obvious
/**
 * Gets the arm encoder position.
 * 
 * @return Position in encoder ticks, where 0 is the starting position
 *         and positive values are up.
 */
public int getPosition() {
    return armMotor.getCurrentPosition();
}

// Check if we're within the safe zone before extending arm
if (arm.getPosition() < Constants.ARM_SAFE_ZONE && intake.isExtended()) {
    intake.retract(); // Prevent collision with field perimeter
}

// Wait for all motors to finish (with 5 second timeout)
long startTime = System.currentTimeMillis();
while (motorsAreBusy() && System.currentTimeMillis() - startTime < 5000) {
    idle();
}

public class Constants {
    // DRIVETRAIN CALIBRATION
    // Measured on 2024-11-15 with new 96mm mecanum wheels
    public static final double WHEEL_DIAMETER_INCHES = 3.78;
    public static final int MOTOR_TICKS_PER_REV = 537; // GoBILDA 5203-2402-0019
    public static final double GEAR_RATIO = 1.0; // Direct drive, no gearbox
    
    // TRACK WIDTH
    // Distance between left/right wheel centers
    // Calibrated using 10-rotation test: measured 359.5", theoretical 360"
    public static final double TRACK_WIDTH_INCHES = 14.52;
    
    // ARM POSITIONS
    // Found through manual testing during Week 3 build session
    public static final int ARM_GROUND_PICKUP = 50;
    public static final int ARM_LOW_BASKET = 800;
    public static final int ARM_HIGH_BASKET = 1650;
    public static final int ARM_SAFE_TRAVEL = 200; // Clears intake when driving
}

public class MainTeleOp extends LinearOpMode {
    // ===== HARDWARE =====
    private Drivetrain drivetrain;
    private Arm arm;
    private Intake intake;
    
    // ===== STATE =====
    private boolean slowMode = false;
    private boolean lastBumperState = false;
    
    // ===== INITIALIZATION =====
    @Override
    public void runOpMode() {
        initializeHardware();
        waitForStart();
        
        while (opModeIsActive()) {
            updateGameplay();
        }
    }
    
    // ===== GAMEPLAY UPDATES =====
    private void updateGameplay() {
        handleDrivetrain();
        handleArm();
        handleIntake();
    }
    
    // ===== HELPER METHODS =====
    private void initializeHardware() {
        // ...
    }
}

// TODO: Implement encoder-based autonomous movement
// FIXME: Color sensor values are inverted (hardware issue?)
// NOTE: This autonomous mode is for the RED alliance only
// HACK: Temporary fix until we get the new IMU sensor
// OPTIMIZE: This loop could be more efficient with ArrayList instead of array

// Bad - comment adds no value
// Get the motor power
double motorPower = calculateMotorPower();

// Good - no comment needed
double motorPower = calculateMotorPower();

// Bad
double x = motor.getCurrentPosition(); // Current arm position

// Good
double currentArmPosition = motor.getCurrentPosition();

# Team 12345 FTC Robot Code - DECODE Season

## Robot Configuration

**Hardware:**
- 4x GoBILDA 5203-2402-0019 motors (drivetrain)
- 1x REV HD Hex motor (arm)
- 2x REV Servo (claw, wrist)
- 1x REV Control Hub
- 1x REV IMU
- 1x REV Color Sensor V3

**Wiring:**
See `docs/wiring-diagram.pdf`

## Running OpModes

**TeleOp:**
1. Select "Drive: Main TeleOp" from Driver Station
2. Controls:
   - Left stick: forward/strafe
   - Right stick: rotation
   - Left bumper: slow mode toggle
   - A button: intake collect
   - B button: intake eject

**Autonomous:**
- `Auto: Red Left` - Red alliance, left starting position
- `Auto: Red Right` - Red alliance, right starting position
- `Auto: Blue Left` - Blue alliance, left starting position
- `Auto: Blue Right` - Blue alliance, right starting position

## Code Structure

## Important Notes

- Always calibrate track width after changing wheels
- IMU must be mounted flat on the robot (Z-axis up)
- Run "Test: Motor Direction Test" after any wiring changes

## Contact

Lead Programmer: Jane Doe (jane@team12345.org)

/**
 * Pure Pursuit path following algorithm.
 * 
 * The robot chases a "lookahead point" on the path that's a fixed distance ahead.
 * This creates smooth curved paths instead of sharp point-to-point turns.
 * 
 * Algorithm steps:
 * 1. Find the lookahead point on the path
 * 2. Transform lookahead to robot's coordinate frame
 * 3. Calculate curvature to reach the lookahead point
 * 4. Convert curvature to left/right motor powers
 * 
 * Resources:
 * - https://www.chiefdelphi.com/t/paper-implementation-of-the-adaptive-pure-pursuit-controller/166552
 * - Team 1712's presentation: https://www.youtube.com/watch?v=YBJmYSWFjZ4
 */
public void followPath(List<Pose2D> path) {
    // Step 1: Find lookahead point
    Pose2D lookahead = findLookaheadPoint(path, robotPose, LOOKAHEAD_DISTANCE);
    
    // Step 2: Transform to robot frame
    double localX = /* transformation math */;
    double localY = /* transformation math */;
    
    // Step 3: Calculate curvature
    // Formula from Pure Pursuit paper: k = 2 * localY / L²
    double curvature = (2 * localY) / (LOOKAHEAD_DISTANCE * LOOKAHEAD_DISTANCE);
    
    // Step 4: Convert to motor powers
    // Left motor slows down, right motor speeds up for left turn
    double leftPower = basePower * (1 - curvature * TRACK_WIDTH / 2);
    double rightPower = basePower * (1 + curvature * TRACK_WIDTH / 2);
    
    drivetrain.setPower(leftPower, rightPower);
}