Merge branch 'main' into ArmSubsystem

README.md

@@ -1 +1 @@
-BitBuckets robot code for 2025
+BitBuckets robot code for 2025

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.1"
 }
 
 java {
@@ -17,7 +17,7 @@ deploy {
             // or from command line. If not found an exception will be thrown.
             // You can use getTeamOrDefault(team) instead of getTeamNumber if you
             // want to store a team number in this file.
-            team = project.frc.getTeamNumber()
+            team = 4183;
             debug = project.frc.getDebugOrDefault(false)
 
             artifacts {

simgui-ds.json

@@ -4,16 +4,6 @@
       "visible": false
     }
   },
-  "Keyboard 0 Settings": {
-    "window": {
-      "visible": true
-    }
-  },
-  "System Joysticks": {
-    "window": {
-      "visible": false
-    }
-  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

src/main/FRC 2025 Path Choreo plan (1).chor

@@ -1,6 +1,6 @@
 {
  "name":"FRC 2025 Path Choreo plan",
- "version":1,
+ "version":,
  "type":"Swerve",
  "variables":{
   "expressions":{},

src/main/java/frc/robot/OperatorInput.java

@@ -29,6 +29,9 @@ public class OperatorInput {
 
   final Trigger manualArmCommand = operatorControl.axisMagnitudeGreaterThan(XboxController.Axis.kRightY.value, 0.1);
 
+  final Trigger openClaw = driver.leftStick();
+  final Trigger closeClaw = driver.rightStick();
+
   final Trigger IntakeOn = driver.a();
 
   final Trigger armbendup = operatorControl.povUp();

src/main/java/frc/robot/RobotContainer.java

@@ -33,17 +33,19 @@
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.DriveConstants;
 import frc.robot.generated.TunerConstants;
-import frc.robot.subsystems.AlgaeManagementSubsystem.AlgaeManagementSubsystem;
 import frc.robot.subsystems.Auto.AutoSubsystem;
 import frc.robot.subsystems.ClawSubsystem.ClawSubsystem;
+import frc.robot.subsystems.ClawSubsystem.*;
 import frc.robot.subsystems.DriveSubsystem.*;
 import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
 import frc.robot.subsystems.DriveSubsystem.GyroIO;
 import frc.robot.subsystems.DriveSubsystem.GyroIOPigeon2;
 import frc.robot.subsystems.DriveSubsystem.ModuleIO;
 import frc.robot.subsystems.DriveSubsystem.ModuleIOSim;
 import frc.robot.subsystems.ElevatorSubsystem.*;
-import frc.robot.subsystems.GroundIntakeSubsystem.GroundIntakeSubsystem;
+import frc.robot.subsystems.AlgaeIntakeSubsystem.AlgaeIntakeSubsystem;
+import frc.robot.subsystems.AlgaeIntakeSubsystem.IntakeIOSparkMax;
+import frc.robot.subsystems.AlgaeIntakeSubsystem.IntakeIOSim;
 import frc.robot.subsystems.LEDSubsytem.LEDSubsystem;
 import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIOEncoder;
 import frc.robot.subsystems.SingleJointedArmSubsystem.SingleJointedArmIOSim;
@@ -55,6 +57,7 @@
 import frc.robot.subsystems.VisionSubsystem.VisionSubsystem;
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoral;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
@@ -64,12 +67,10 @@
 public class RobotContainer {
   // Subsystems
   public final DriveSubsystem drive;
-  // private final Flywheel flywheel;
   public final OperatorInput operatorInput;
   private final ElevatorSubsystem elevatorSubsystem;
-  private final AlgaeManagementSubsystem algaeManagementSubsystem;
   private final ClawSubsystem clawSubsystem;
-  private final GroundIntakeSubsystem groundIntakeSubsystem;
+  private final AlgaeIntakeSubsystem groundIntakeSubsystem;
   private final LEDSubsystem ledSubsystem;
   private final SingleJointedArmSubsystem singleJointedArmSubsystem;
   private final VisionSubsystem visionSubsystem;
@@ -104,13 +105,11 @@ public RobotContainer() {
                 new ModuleIOHybrid(3, TunerConstants.BackRight));
 
         elevatorSubsystem =
-                new ElevatorSubsystem(new ElevatorIOSparkMax(), new ElevatorEncoderIOSim()); //TODO
-        algaeManagementSubsystem =
-                new AlgaeManagementSubsystem(); //TODO
+                new ElevatorSubsystem(new ElevatorIOSparkMax(), new ElevatorEncoderIOThroughbore()); //TODO
         clawSubsystem =
-                new ClawSubsystem(); //TODO
+                new ClawSubsystem(new EndEffectorIOSparkMax(new EndEffectorEncoderIOSim()));
         groundIntakeSubsystem =
-                new GroundIntakeSubsystem(); //TODO
+                new AlgaeIntakeSubsystem(new IntakeIOSparkMax(13, 12)); //TODO replace placeholder
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
@@ -132,21 +131,15 @@ public RobotContainer() {
                     new ModuleIOSim(driveSimulation.getModules()[1]),
                     new ModuleIOSim(driveSimulation.getModules()[2]),
                     new ModuleIOSim(driveSimulation.getModules()[3]));
-//        new DriveSubsystem(
-//                new GyroIOSim(driveSimulation.getGyroSimulation()),
-//                new ModuleIOHybridSim(0, TunerConstants.FrontLeft, driveSimulation.getModules()[0]),
-//                new ModuleIOHybridSim(1, TunerConstants.FrontRight,driveSimulation.getModules()[1]),
-//                new ModuleIOHybridSim(2, TunerConstants.BackLeft,driveSimulation.getModules()[2]),
-//                new ModuleIOHybridSim(3, TunerConstants.BackRight,driveSimulation.getModules()[3]));
-        // flywheel = new Flywheel(new FlywheelIOSim());
+
+        ElevatorIOSim elevatorIOSim = new ElevatorIOSim();
         elevatorSubsystem =
-                new ElevatorSubsystem(new ElevatorIOSim(), new ElevatorEncoderIOSim()); //TODO
-        algaeManagementSubsystem =
-                new AlgaeManagementSubsystem(); //TODO
+                new ElevatorSubsystem(elevatorIOSim, new ElevatorEncoderIOSim(elevatorIOSim.elevatorMotor1Sim));
+
         clawSubsystem =
-                new ClawSubsystem(); //TODO
+                new ClawSubsystem(new EndEffectorIOSim());
         groundIntakeSubsystem =
-                new GroundIntakeSubsystem(); //TODO
+                new AlgaeIntakeSubsystem(new IntakeIOSim(driveSimulation)); //TODO
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
@@ -165,15 +158,12 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {},
                 new ModuleIO() {});
-        // flywheel = new Flywheel(new FlywheelIO() {});
         elevatorSubsystem =
                 new ElevatorSubsystem(new ElevatorIO() {}, new ElevatorEncoderIO() {}); //TODO
-        algaeManagementSubsystem =
-                new AlgaeManagementSubsystem(); //TODO
         clawSubsystem =
-                new ClawSubsystem(); //TODO
+                new ClawSubsystem(new EndEffectorIO() {});
         groundIntakeSubsystem =
-                new GroundIntakeSubsystem(); //TODO
+                new AlgaeIntakeSubsystem(new IntakeIOSparkMax(13, 12)); //TODO replace with real intake
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         singleJointedArmSubsystem =
@@ -229,6 +219,9 @@ void loadCommands() {
     operatorInput.manualElevator.whileTrue(new ManualElevatorCommand(elevatorSubsystem, elevatorAndArmController::getLeftY));
 
 
+    operatorInput.openClaw.onTrue(new OpenClawCommand(clawSubsystem));
+    operatorInput.closeClaw.onTrue(new CloseClawCommand(clawSubsystem));
+
     operatorInput.movementDesired.whileTrue(
             new BaseDriveCommand.basedrivecommand(
                 drive,
@@ -253,9 +246,24 @@ public void resetSimulationField() {
     SimulatedArena.getInstance().resetFieldForAuto();
   }
 
+  public void addCoral() {
+    if (Constants.currentMode != Constants.Mode.SIM) return;
+
+    SimulatedArena.getInstance().addGamePiece(new ReefscapeCoral(new Pose2d(2, 2, Rotation2d.fromDegrees(90))));
+    Logger.recordOutput("FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral"));
+  }
+
   public void displaySimFieldToAdvantageScope() {
     if (Constants.currentMode != Constants.Mode.SIM) return;
 
     Logger.recordOutput("FieldSimulation/RobotPosition", driveSimulation.getSimulatedDriveTrainPose());
   }
+
+  public void periodic() {
+    if (Constants.currentMode != Constants.Mode.SIM) return;
+
+    Pose3d[] coralPoses = SimulatedArena.getInstance()
+            .getGamePiecesArrayByType("Coral");
+    Logger.recordOutput("FieldSimulation/CoralPositions", coralPoses);
+  }
 }

src/main/java/frc/robot/commands/BaseDriveCommand.java

@@ -50,19 +50,19 @@ public void execute() {
                             .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
                             .getTranslation();
 
-            // Convert to field relative speeds & send command
-            boolean isFlipped =
-                    DriverStation.getAlliance().isPresent()
-                            && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
-            drive.runVelocityPP(
-                    ChassisSpeeds.fromFieldRelativeSpeeds(
-                            linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
-                            linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
-                            omega * drive.getMaxAngularSpeedRadPerSec(),
-                            isFlipped
-                                    ? drive.getRotation().plus(new Rotation2d(Math.PI))
-                                    : drive.getRotation()));
-        }
+        // Convert to field relative speeds & send command
+        boolean isFlipped =
+                DriverStation.getAlliance().isPresent()
+                        && DriverStation.getAlliance().get() == Alliance.Red;
+        drive.runVelocity(
+                ChassisSpeeds.fromFieldRelativeSpeeds(
+                        linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
+                        linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
+                        omega * drive.getMaxAngularSpeedRadPerSec(),
+                        isFlipped
+                                ? drive.getRotation().plus(new Rotation2d(Math.PI))
+                                : drive.getRotation()));
+    }
 
         @Override
         public void end(boolean interrupted) {

src/main/java/frc/robot/commands/CloseClawCommand.java

@@ -0,0 +1,21 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ClawSubsystem.ClawSubsystem;
+
+public class CloseClawCommand extends Command {
+    private final ClawSubsystem clawSubsystem;
+
+    public CloseClawCommand(ClawSubsystem clawSubsystem) {
+        this.clawSubsystem = clawSubsystem;
+        addRequirements(clawSubsystem);
+    }
+
+    public void initialize() {
+        clawSubsystem.close();
+    }
+
+    public boolean isFinished() {
+        return true;
+    }
+}

src/main/java/frc/robot/commands/ElevatorCommands/ElevatorSetPointCommand.java

@@ -43,7 +43,7 @@ public void execute(){
     @Override
     public void end(boolean interrupted) {
         if (interrupted) {
-            elevator.setElevatorVoltage(ElevatorConstants.kG);
+            elevator.setElevatorVoltage(ElevatorConstants.kGSim);
 
         }
     }

src/main/java/frc/robot/commands/ElevatorCommands/ManualElevatorCommand.java

@@ -1,35 +1,45 @@
 package frc.robot.commands.ElevatorCommands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem;
 import org.littletonrobotics.junction.Logger;
 
 import java.util.function.DoubleSupplier;
 
 public class ManualElevatorCommand extends Command {
-    private static final double DEADBAND = 0.1;
     public ElevatorSubsystem elevator;
+    private final double kG;
     DoubleSupplier yStickDistanceSupplier;
 
     public ManualElevatorCommand(ElevatorSubsystem elevator, DoubleSupplier yDoubleSupplier) {
         this.elevator = elevator;
         addRequirements(elevator);
         this.yStickDistanceSupplier = yDoubleSupplier;
+        this.kG = (Constants.currentMode != Constants.Mode.SIM) ? ElevatorConstants.kG : ElevatorConstants.kGSim;
     }
 
     @Override
     public void execute() {
-        double manualVelocity = yStickDistanceSupplier.getAsDouble() * -1.0;
+        double manualVelocity = yStickDistanceSupplier.getAsDouble();
         double calculatedVolts = elevator.elevatorFF.calculate(manualVelocity);
         Logger.recordOutput("ElevatorSubsystem/target_voltage", calculatedVolts);
         this.elevator.setElevatorVoltage(calculatedVolts);
+        if (elevator.getLoadHeight() >= ElevatorConstants.maxHeight) {
+            // We are going up and top limit is tripped so stop
+            elevator.setElevatorVoltage(kG);
+        }
+        if (elevator.getLoadHeight() <= ElevatorConstants.minHeight) {
+            // We are going down and bottom limit is tripped so stop
+            elevator.setElevatorVoltage(kG);
+        }
     }
 
     @Override
     public void end(boolean interrupted) {
         if (interrupted) {
-            double calculatedVolts = ElevatorConstants.kG;
+            double calculatedVolts = kG;
             Logger.recordOutput("ElevatorSubsystem/target_voltage", calculatedVolts);
             this.elevator.setElevatorVoltage(calculatedVolts);
         }

src/main/java/frc/robot/commands/ElevatorCommands/ResetElevatorEncoderCommand.java

@@ -1,22 +1,18 @@
 package frc.robot.commands.ElevatorCommands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.ElevatorSubsystem.ElevatorEncoderIO;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem;
 import org.littletonrobotics.junction.Logger;
 
-public class ResetElevatorEncoderCommand extends Command implements ElevatorEncoderIO {
+public class ResetElevatorEncoderCommand extends Command  {
     public ElevatorSubsystem elevator;
-    double loadHeight = 0;
-
     public ResetElevatorEncoderCommand(ElevatorSubsystem elevator) {
         this.elevator = elevator;
         addRequirements(elevator);
     }
     @Override
     public void execute(){
-        this.loadHeight = 0.01;
-        Logger.recordOutput("ElevatorSubsystem/loadHeight", loadHeight);
 
+        elevator.resetLoadHeightEncoderValue();
     }
 }

src/main/java/frc/robot/commands/IntakePickupCommand.java

@@ -0,0 +1,21 @@
+package frc.robot.commands;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeIntakeSubsystem.AlgaeIntakeSubsystem;
+
+public class IntakePickupCommand extends Command {
+    private final AlgaeIntakeSubsystem intakeSubsystem;
+
+    public IntakePickupCommand(AlgaeIntakeSubsystem intakeSubsystem) {
+        this.intakeSubsystem = intakeSubsystem;
+        addRequirements(intakeSubsystem);
+    }
+
+    public void initialize() {
+        intakeSubsystem.pickup();
+    }
+
+    public boolean isFinished() {
+        return true;
+    }
+}
+

src/main/java/frc/robot/commands/OpenClawCommand.java

@@ -0,0 +1,22 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ClawSubsystem.ClawSubsystem;
+
+public class OpenClawCommand extends Command {
+    private final ClawSubsystem clawSubsystem;
+
+    public OpenClawCommand(ClawSubsystem clawSubsystem) {
+        this.clawSubsystem = clawSubsystem;
+        addRequirements(clawSubsystem);
+    }
+
+    public void initialize() {
+        clawSubsystem.open();
+    }
+
+    public boolean isFinished() {
+        return true;
+    }
+}
+