Pre testing fixes

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1"
+    id "edu.wpi.first.GradleRIO" version "2025.2.1"
     id "com.diffplug.spotless" version "6.22.0"
 }
 

src/main/java/com/stuypulse/robot/Robot.java

@@ -5,23 +5,12 @@
 
 package com.stuypulse.robot;
 
-import com.stuypulse.robot.constants.Settings.RobotType;
-
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class Robot extends TimedRobot {
 
-    public static final RobotType ROBOT;
-
-    static {
-        if (Robot.isSimulation())
-            ROBOT = RobotType.SIM;
-        else
-            ROBOT = RobotType.fromString(System.getenv("serialnum"));
-    }
-
     private RobotContainer robot;
     private Command auto;
 

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -5,30 +5,10 @@
 
 package com.stuypulse.robot;
 
-import java.lang.annotation.ElementType;
-import com.stuypulse.robot.commands.Elevator.ElevatorToBottom;
-import com.stuypulse.robot.commands.Elevator.ElevatorToLvl1;
-import com.stuypulse.robot.commands.Elevator.ElevatorToLvl2;
-import com.stuypulse.robot.commands.Elevator.ElevatorToLvl3;
-import com.stuypulse.robot.commands.Elevator.ElevatorToLvl4;
-import com.stuypulse.robot.commands.auton.DoNothingAuton;
-import com.stuypulse.robot.constants.Ports;
-import com.stuypulse.robot.subsystems.Elevator.Elevator;
-import javax.management.openmbean.OpenType;
-
-// Algae Commands
-import com.stuypulse.robot.commands.algae.AlgaeGroundPickup;
-import com.stuypulse.robot.commands.algae.AlgaeL2;
-import com.stuypulse.robot.commands.algae.AlgaeL3;
-import com.stuypulse.robot.commands.algae.AlgaeProcessorScore;
-import com.stuypulse.robot.commands.algae.AlgaeReefKnockoff;
-import com.stuypulse.robot.commands.algae.AlgaeStopRoller;
-import com.stuypulse.robot.commands.algae.AlgaeStow;
-
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.subsystems.algae.Algae;
-
+import com.stuypulse.robot.subsystems.elevator.Elevator;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
@@ -79,41 +59,6 @@ private void configureDriverBindings() {
 
     private void configureOperatorBindings() {
 
-        operator.getDPadDown()
-            .onTrue(new ElevatorToLvl1());
-
-        operator.getDPadLeft()
-            .onTrue(new ElevatorToLvl2());
-
-        operator.getDPadRight()
-            .onTrue(new ElevatorToLvl3());    
-
-        operator.getDPadUp()
-            .onTrue(new ElevatorToLvl4());
-
-        operator.getLeftButton()
-            .onTrue(new ElevatorToBottom());
-
-        operator.getBottomButton()
-            .onTrue(new AlgaeStow());
-
-        operator.getTopButton()
-            .onTrue(new AlgaeGroundPickup());
-
-        operator.getLeftButton()
-            .onTrue(new AlgaeL2());
-
-        operator.getRightButton()
-            .onTrue(new AlgaeL3());
-
-        operator.getLeftBumper()
-            .onTrue(new AlgaeProcessorScore());
-
-        operator.getRightBumper()
-            .onTrue(new AlgaeReefKnockoff());
-
-        operator.getLeftMenuButton()
-            .onTrue(new AlgaeStopRoller());
     }
 
     /**************/

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java

@@ -1,10 +1,10 @@
-package com.stuypulse.robot.commands.Elevator;
+package com.stuypulse.robot.commands.elevator;
 
 import com.stuypulse.robot.constants.Settings.Elevator;
 
 public class ElevatorToBottom extends ElevatorToHeight {
 
     public ElevatorToBottom() {
-        super(Elevator.MIN_HEIGHT);
+        super(Elevator.MIN_HEIGHT_METERS);
     }
 }

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java

@@ -1,6 +1,6 @@
-package com.stuypulse.robot.commands.Elevator;
+package com.stuypulse.robot.commands.elevator;
 
-import com.stuypulse.robot.subsystems.Elevator.Elevator;
+import com.stuypulse.robot.subsystems.elevator.Elevator;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;   
 

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java

@@ -1,9 +1,9 @@
-package com.stuypulse.robot.commands.Elevator;
+package com.stuypulse.robot.commands.elevator;
 
 import com.stuypulse.robot.constants.Settings.Elevator;
 
 public class ElevatorToLvl1 extends ElevatorToHeight{
     public ElevatorToLvl1(){
-        super(Elevator.L1);
+        super(Elevator.L1_HEIGHT_METERS);
     }
 }

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java

@@ -1,9 +1,9 @@
-package com.stuypulse.robot.commands.Elevator;
+package com.stuypulse.robot.commands.elevator;
 
 import com.stuypulse.robot.constants.Settings.Elevator;
 
 public class ElevatorToLvl2 extends ElevatorToHeight {
     public ElevatorToLvl2(){
-        super(Elevator.L2);
+        super(Elevator.L2_HEIGHT_METERS);
     }
 }

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java

@@ -1,9 +1,9 @@
-package com.stuypulse.robot.commands.Elevator;
+package com.stuypulse.robot.commands.elevator;
 
 import com.stuypulse.robot.constants.Settings.Elevator;
 
 public class ElevatorToLvl3 extends ElevatorToHeight {
     public ElevatorToLvl3(){
-        super(Elevator.L3);
+        super(Elevator.L3_HEIGHT_METERS);
     }
 }

src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java

@@ -1,9 +1,9 @@
-package com.stuypulse.robot.commands.Elevator;
+package com.stuypulse.robot.commands.elevator;
 
 import com.stuypulse.robot.constants.Settings.Elevator;
 
 public class ElevatorToLvl4 extends ElevatorToHeight{
     public ElevatorToLvl4(){
-        super(Elevator.L4);
+        super(Elevator.L4_HEIGHT_METERS);
     }
 }

src/main/java/com/stuypulse/robot/commands/algae/AlgaeAcquireOver.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.algae;
+
+import com.stuypulse.robot.subsystems.algae.Algae;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AlgaeAcquireOver extends Command{
+
+    private final Algae algae;
+
+    public AlgaeAcquireOver() {
+        this.algae = Algae.getInstance();
+        addRequirements(algae);
+    }
+
+    @Override
+    public void initialize() {
+        algae.acquireOver();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return algae.hasAlgae();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        algae.stopRollers();
+    }
+}

src/main/java/com/stuypulse/robot/commands/algae/AlgaeAcquireUnder.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.algae;
+
+import com.stuypulse.robot.subsystems.algae.Algae;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AlgaeAcquireUnder extends Command{
+
+    private final Algae algae;
+
+    public AlgaeAcquireUnder() {
+        this.algae = Algae.getInstance();
+        addRequirements(algae);
+    }
+
+    @Override
+    public void initialize() {
+        algae.acquireUnder();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return algae.hasAlgae();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        algae.stopRollers();
+    }
+}

src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetPivot.java → src/main/java/com/stuypulse/robot/commands/algae/AlgaeDeacquireOver.java

@@ -4,21 +4,18 @@
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class AlgaeSetPivot extends InstantCommand {
+public class AlgaeDeacquireOver extends InstantCommand {
 
-    protected final Algae algae; 
-    private final double angle; //target angle
+    private final Algae algae; 
 
-    public AlgaeSetPivot(double angle) {
+    public AlgaeDeacquireOver() {
         algae = Algae.getInstance();
-
-        this.angle = angle;
         addRequirements(algae);
     }
 
     @Override
     public void initialize() {
-        algae.setTargetAngle(angle);
+        algae.deacquireOver();
     }
 
 }

src/main/java/com/stuypulse/robot/commands/algae/AlgaeDeacquireUnder.java

@@ -0,0 +1,21 @@
+package com.stuypulse.robot.commands.algae;
+
+import com.stuypulse.robot.subsystems.algae.*;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class AlgaeDeacquireUnder extends InstantCommand {
+
+    private final Algae algae; 
+
+    public AlgaeDeacquireUnder() {
+        algae = Algae.getInstance();
+        addRequirements(algae);
+    }
+
+    @Override
+    public void initialize() {
+        algae.deacquireUnder();
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetAngle.java

@@ -0,0 +1,24 @@
+package com.stuypulse.robot.commands.algae;
+
+import com.stuypulse.robot.subsystems.algae.*;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class AlgaeSetAngle extends InstantCommand {
+
+    private final Algae algae; 
+    private final Number targetAngle;
+
+    public AlgaeSetAngle(Number targetAngle) {
+        algae = Algae.getInstance();
+
+        this.targetAngle = targetAngle;
+        addRequirements(algae);
+    }
+
+    @Override
+    public void initialize() {
+        algae.setTargetAngle(targetAngle.doubleValue());
+    }
+
+}