🐠 Team 4534 Robot Code Repository

Next-gen FIRST robotics codebase for efficient reef scoring automation 😎

---

📚 Table of Contents

• 🎯 High-Level Goals
• 🚀 Features & Progress
  • Elevator
  • Wrist
  • Drivetrain
  • Path Planning
  • Vision Integration
• 📖 Resources
  • Guides
  • Subsystems
• 🌳 Repository Structure
  • Branch Strategy
  • Workflow

---

🎯 High-Level Goals

• Drivetrain: Optimize swerve responsiveness + odometry accuracy.
• Path Planning: Seamless autonomous navigation via Path Planner.
• Vision: Real-time AprilTag tracking + multi-camera support.

---

🚀 Features & Progress

Elevator

• Tune PID + feedforward
• Real-world position conversion (meters via pulley ratios)

✅ Completed Items

• Subsystem framework
• Manual + PID control commands
• Motion profiling (TrapezoidProfile)
• Safety: Software limits + output clamping

📁 Code Snippets

// Position conversion
elevatorEncoder.setPositionConversionFactor(C_Elevator.ENCODER_DISTANCE_PER_PULSE);
elevatorEncoder.setVelocityConversionFactor(C_Elevator.ENCODER_VELOCITY_FACTOR);

---

Wrist

• Tune PID + feedforward + motion profiling
• Implement safety checks for mechanism limits
• PID tuning

📁 Code Snippets

// Separate logic for CoralIn and AlgaeOut
if (mechanismType == MechanismType.ROLLER) {
    // Roller-specific logic
    rollerMotor.set(coralInSpeed);
} else if (mechanismType == MechanismType.BELT) {
    // Belt-specific logic
    beltMotor.set(algaeOutSpeed);
}

// Safety checks
if (isAtLimit()) {
    motor.stop();
} else {
    motor.set(desiredSpeed);
}

✅ Completed Items

• Subsystem setup
  • Support main wrist joint movement
  • Support actual intake mechanism (e.g., rollers, belts)
• Manual + PID controls
• Separate CoralIn/AlgaeOut logic if mechanisms differ (e.g., rollers vs. belts).

---

Drivetrain

• Full swerve diagnostic tuning
• Odometry calibration
• Path following optimization

✅ Completed Items

• Swerve base code (Advantage Kit)

---

Path Planning

• Constraint tuning

✅ Completed Items

• Path Planner evaluation
• Integration of Path Planner into codebase and test creation of basic paths (TI)
• Define robot constraints for path planning
• Implement basic path commands
• Implement basic event triggers for use in paths

---

Vision Integration

• AssistedMove (e.g., aligning to AprilTags on the reef/barge)
• PipelineCommand to dynamically switch vision pipelines for different targets

📁 Code Snippets

// AssistedMove command
public class AssistedMove extends CommandBase {
    private final Drivetrain drivetrain;
    private final VisionSubsystem vision;

    public AssistedMove(Drivetrain drivetrain, VisionSubsystem vision) {
        this.drivetrain = drivetrain;
        this.vision = vision;
        addRequirements(drivetrain, vision);
    }

    @Override
    public void initialize() {
        vision.enableAprilTagTracking();
    }

    @Override
    public void execute() {
        Pose2d targetPose = vision.getAprilTagPose();
        drivetrain.alignToPose(targetPose);
    }

    @Override
    public void end(boolean interrupted) {
        vision.disableAprilTagTracking();
    }

    @Override
    public boolean isFinished() {
        return drivetrain.isAligned();
    }
}

// PipelineCommand to switch vision pipelines
public class PipelineCommand extends InstantCommand {
    private final VisionSubsystem vision;
    private final int pipeline;

    public PipelineCommand(VisionSubsystem vision, int pipeline) {
        this.vision = vision;
        this.pipeline = pipeline;
    }

    @Override
    public void initialize() {
        vision.setPipeline(pipeline);
    }
}

✅ Completed Items

• Limelight selected (legacy compatibility)
• Advantage Kit + Limelight setup (BM)
• Dual camera configuration
• AprilTag pose estimation

LED Strip

• Subsystem setup
• Basic color control
• Pattern animations (e.g., rainbow, chase)
• Integrate with match events (e.g., alliance color, scoring feedback)

📁 Code Snippets

// Basic color control
public void setColor(Color color) {
    ledStrip.set(color);
}

// Pattern animation example
public void setRainbowPattern() {
    // Implementation for rainbow pattern
    for (int i = 0; i < ledStrip.length(); i++) {
        ledStrip.set(i, Color.getHSBColor((i / (float) ledStrip.length()), 1.0, 1.0));
    }
    ledStrip.show();
}

✅ Completed Items

• None yet

---

📖 Resources

Guides

• Advantage Kit Vision Docs
• Path Planner Tutorials
• Limelight Setup Guide
• REV Spark Flex API

Subsystems

• Elevator
  • 🔗 PID + Feedforward Tuning
  • 🔗 SysId Best Practices
  • 🔗 Driving an elevator and a wrist while having forbidden positions
• Vision
  • 🔗 AprilTag Filtering

---

🌳 Repository Structure

Branch Strategy

• Gillbert: Swerve prototype base 🧪
• develop: Active integration + testing 🔄
• main: Competition-ready stable releases 🏆

Workflow

1. Protect Branches: Require PR reviews for main/develop.
2. Develop in develop: All features tested here first.
3. PR Reviews: Approved by leads (Max/Greg) before merging.
4. Release to main: Only after rigorous validation.

---

Made with ❤️ by Team 4534 - The Wired Wizards