Programming Evolution

public class LimelightCamera {
    private NetworkTable table;
    private double targetX, targetY, targetArea;
    
    public LimelightCamera() {
        table = NetworkTableInstance.getDefault()
            .getTable("limelight");
    }
    
    public boolean hasTarget() {
        return table.getEntry("tv").getDouble(0) == 1;
    }
    
    public void updateTargetData() {
        targetX = table.getEntry("tx").getDouble(0);
        targetY = table.getEntry("ty").getDouble(0);
        targetArea = table.getEntry("ta").getDouble(0);
    }
    
    public double calculateDistance() {
        // Distance calculation based on target area
        return (TARGET_HEIGHT * FOCAL_LENGTH) / targetArea;
    }
}

public class PurePursuitPath {
    private ArrayList<Pose2d> waypoints;
    private double lookAheadDistance = 12.0;
    
    public void followPath(Pose2d currentPose) {
        // Find closest point on path
        int closestIndex = findClosestPoint(currentPose);
        
        // Find look-ahead point
        int lookAheadIndex = findLookAheadPoint(
            currentPose, closestIndex, lookAheadDistance
        );
        
        // Calculate curvature
        Pose2d targetPoint = waypoints.get(lookAheadIndex);
        double curvature = calculateCurvature(currentPose, targetPoint);
        
        // Calculate wheel powers
        double leftPower = BASE_SPEED + (curvature * TRACK_WIDTH / 2);
        double rightPower = BASE_SPEED - (curvature * TRACK_WIDTH / 2);
        
        setMotorPowers(leftPower, rightPower);
    }
}

public class TimeBasedLogic {
    private ElapsedTime timer = new ElapsedTime();
    private final double SORTER_DELAY = 0.5; // 0.5 seconds
    
    public void updateSorter() {
        if (timer.time() >= SORTER_DELAY) {
            // Activate sorter mechanism
            sorter.setPosition(SORTER_POSITION);
            timer.reset();
        }
    }
    
    public void dynamicAiming(double robotX, double robotY, double robotHeading) {
        // Calculate turret angle while moving
        double dx = TARGET_X - robotX;
        double dy = TARGET_Y - robotY;
        double targetAngle = Math.atan2(dy, dx);
        double turretAngle = targetAngle - robotHeading;
        
        turret.setPosition(turretAngle);
    }
}

public class DynamicAimingIntegration {
    private PedroPathing pathFollower;
    private LimelightCamera limelight;
    private Turret turret;
    private Shooter shooter;
    
    public void update() {
        // Get robot pose from path follower
        Pose2d robotPose = pathFollower.getPose();
        
        // Get target data from limelight
        if (limelight.hasTarget()) {
            limelight.updateTargetData();
            double distance = limelight.calculateDistance();
            
            // Calculate turret angle (dynamic aiming)
            double targetX = limelight.getTargetX();
            double targetY = limelight.getTargetY();
            double dx = targetX - robotPose.getX();
            double dy = targetY - robotPose.getY();
            double targetAngle = Math.atan2(dy, dx);
            double turretAngle = targetAngle - robotPose.getHeading();
            
            turret.setPosition(turretAngle);
            
            // Adjust shooter speed based on distance
            double shooterRPM = 6.85 * distance + 1050;
            shooter.setTargetRPM(shooterRPM);
        }
    }
}