Relative pos from apriltag gb

src/main/java/frc/robot/ElevatorLevel.java

@@ -1,7 +1,9 @@
 package frc.robot;
 
 
-public enum ElevatorLevel {
-    LOW, MED, HIGH, EXTRA_HIGH, STATION
+ public enum ElevatorLevel {
+     LOW, MED, HIGH, EXTRA_HIGH, STATION 
 }
 
+
+

src/main/java/frc/robot/OI.java

@@ -11,24 +11,23 @@
 public class OI {
 
     // Reference to RobotContainer for accessing subsystems
-    private static RobotContainer robotContainer;
 
     /**
      * Initialize OI with a reference to RobotContainer
      * @param container The RobotContainer instance
      */
-    public static void initialize(RobotContainer container) {
-        robotContainer = container;
-        // Removed initializeBindings call - now handled in RobotContainer
-    }
-
+
     /**
      * Controller port mappings
      */
     private static class PORT {
         private static final int DRIVE_CONTROLLER_PORT = 0;
         private static final int OPERATOR_CONTROLLER_PORT = 1; // Added operator controller port
     }
+    // Trigger thresholds
+    public static final double TRIGGER_THRESHOLD = 0.1;
+
+
     /**
      * Bidings on driver controls
      */
@@ -37,6 +36,7 @@ private static class PORT {
 
     private static class DRIVER_MAP {
     private static final int ABUTTON = XboxController.Button.kA.value;
+    private static final int BBUTTON = XboxController.Button.kB.value;
     }
 
     // drive sticks
@@ -47,43 +47,46 @@ private static class DRIVER_MAP {
 
     // drive buttons
     public static final JoystickButton driveControllerA = new JoystickButton(xboxController, DRIVER_MAP.ABUTTON); 
-
+    public static final JoystickButton driveControllerB = new JoystickButton(xboxController, DRIVER_MAP.BBUTTON); 
+    public static final Trigger driveRightTrigger = new Trigger(() -> 
+								   xboxController.getRightTriggerAxis() > TRIGGER_THRESHOLD);
+
     private static double processDriveInput(double raw) {
         if (Math.abs(raw) < 0.1) {
-            raw = 0;
+            raw = 0 ;
         }
-        return raw/2;
+        return raw;
     }
 
     private static double processRotationInput(double raw) {
         if (Math.abs(raw) < 0.1) {
             raw = 0;
         }
-        return raw;
+        return raw*4;
     }
 
     /**
      * Bindings on operator controls
      */
-    public static class OperatorBindings {
-        public static final int A_BUTTON = XboxController.Button.kA.value;
-        public static final int B_BUTTON = XboxController.Button.kB.value;
-        public static final int X_BUTTON = XboxController.Button.kX.value;
-        public static final int Y_BUTTON = XboxController.Button.kY.value;
-        public static final int LEFT_BUMPER = XboxController.Button.kLeftBumper.value;
-        public static final int RIGHT_BUMPER = XboxController.Button.kRightBumper.value;
-        public static final int START_BUTTON = XboxController.Button.kStart.value;
+    private static class operatorBindings {
+        private static final int A_BUTTON = XboxController.Button.kA.value;
+        private  static final int B_BUTTON = XboxController.Button.kB.value;
+        private static final int X_BUTTON = XboxController.Button.kX.value;
+        private static final int Y_BUTTON = XboxController.Button.kY.value;
+        private static final int LEFT_BUMPER = XboxController.Button.kLeftBumper.value;
+        private static final int RIGHT_BUMPER = XboxController.Button.kRightBumper.value;
+        private static final int START_BUTTON = XboxController.Button.kStart.value;
+        private static final int BACK_BUTTON = XboxController.Button.kBack.value;
 
-        // Trigger thresholds
-        public static final double TRIGGER_THRESHOLD = 0.1;
-
-        public static final JoystickButton operatorControllerA = new JoystickButton(operatorController, A_BUTTON);
-        public static final JoystickButton operatorControllerB = new JoystickButton(operatorController, B_BUTTON);
-        public static final JoystickButton operatorControllerX = new JoystickButton(operatorController, X_BUTTON);
-        public static final JoystickButton operatorControllerY = new JoystickButton(operatorController, Y_BUTTON);
-        public static final JoystickButton operatorControllerLeftBumper = new JoystickButton(operatorController, LEFT_BUMPER);
-        public static final JoystickButton operatorControllerRightBumper = new JoystickButton(operatorController, RIGHT_BUMPER);
-        public static final JoystickButton operatorControllerStart = new JoystickButton(operatorController, START_BUTTON);
+    }
+        public static final JoystickButton operatorControllerA = new JoystickButton(operatorController, operatorBindings.A_BUTTON);
+        public static final JoystickButton operatorControllerB = new JoystickButton(operatorController, operatorBindings.B_BUTTON);
+        public static final JoystickButton operatorControllerX = new JoystickButton(operatorController, operatorBindings.X_BUTTON);
+        public static final JoystickButton operatorControllerY = new JoystickButton(operatorController, operatorBindings.Y_BUTTON);
+        public static final JoystickButton operatorControllerLeftBumper = new JoystickButton(operatorController, operatorBindings.LEFT_BUMPER);
+        public static final JoystickButton operatorControllerRightBumper = new JoystickButton(operatorController, operatorBindings.RIGHT_BUMPER);
+        public static final JoystickButton operatorControllerStart = new JoystickButton(operatorController, operatorBindings.START_BUTTON);
+        public static final JoystickButton operatorControllerBack = new JoystickButton(operatorController, operatorBindings.BACK_BUTTON);
 
         // Trigger controls for coral claw
         public static final Trigger operatorRightTrigger = new Trigger(() -> 
@@ -95,5 +98,4 @@ public static class OperatorBindings {
         public static double processElevatorInput(double input) {
             return -Math.signum(input) * Math.pow(Math.abs(input), 2) * 0.7;
         }
-    }
 }

src/main/java/frc/robot/Robot.java

@@ -14,6 +14,8 @@
 import frc.robot.commands.test.MoveElevatorToLevelCommand;
 import frc.robot.commands.FetchCoralFromStationCommand;
 
+import frc.robot.ElevatorLevel;
+
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 

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

@@ -9,30 +9,28 @@
 import frc.robot.subsystems.*;
 import frc.robot.commands.*;
 import frc.robot.commands.auto.*;
-import frc.robot.OI.OperatorBindings;
+import frc.robot.commands.instant.*;
 
 public class RobotContainer {
 
   // Constants
   private static final double BEVEL_IN_CORRECTION = 0.25;
 
- /* init subsystems */
+    /* init subsystems */
 
     public SwerveModule leftFrontSwerve = new SwerveModule(RobotMap.leftFrontDrive, RobotMap.leftFrontRotate, RobotMap.leftFrontEncoder, 0.4675 + BEVEL_IN_CORRECTION, 0.0, 0.0, false); 
     public SwerveModule rightFrontSwerve = new SwerveModule(RobotMap.rightFrontDrive, RobotMap.rightFrontRotate, RobotMap.rightFrontEncoder, 0.7705 - BEVEL_IN_CORRECTION, 0.0, 0.0, true); 
     public SwerveModule leftBackSwerve = new SwerveModule(RobotMap.leftBackDrive, RobotMap.leftBackRotate, RobotMap.leftBackEncoder, 0.1280 - BEVEL_IN_CORRECTION, 0.0, 0.0, false); 
     public SwerveModule rightBackSwerve = new SwerveModule(RobotMap.rightBackDrive, RobotMap.rightBackRotate, RobotMap.rightBackEncoder, 0.4992 + BEVEL_IN_CORRECTION, 0.0, 0.0, true); 
 
-  // Create subsystem instances directly here instead of referencing from RobotMap
-  public Wrist wrist = new Wrist(RobotMap.wristSolenoid);
-  public CoralClaw coralClaw = new CoralClaw(RobotMap.coralClawPrimaryMotor, RobotMap.coralClawSecondaryMotor, RobotMap.coralLimitSwitch);
-  public Elevator elevator = new Elevator();
-
-
-public Gyro gyro = new Gyro(RobotMap.gyro);
-public Drivetrain drivetrain = new Drivetrain(leftBackSwerve, rightBackSwerve, leftFrontSwerve, rightFrontSwerve, gyro);
-public Camera camera = new Camera();
+    public Gyro gyro = new Gyro(RobotMap.gyro);
+    public Elevator elevator = new Elevator();
+    public Wrist wrist = new Wrist(RobotMap.wristSolenoid);
+    public CoralClaw coralClaw = new CoralClaw(RobotMap.coralClawPrimaryMotor, RobotMap.coralClawSecondaryMotor, RobotMap.coralLimitSwitch);
 
+    public Camera camera = new Camera();
+    public Drivetrain drivetrain = new Drivetrain(leftBackSwerve, rightBackSwerve, leftFrontSwerve, rightFrontSwerve, gyro);
+
   /**
    * The RobotContainer class is where the bulk of the robot should be declared. 
    * Since Command-based is a "declarative" paradigm, very little robot logic 
@@ -45,14 +43,12 @@ public class RobotContainer {
    */
   public RobotContainer() {
     // Initialize OI with a reference to this RobotContainer instance
-    OI.initialize(this);
-
     configureBindings();
-
+    drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier));
+    RobotMap.compressor.enableAnalog(70, 120);
     // set the default drive command to use the left and right stick values from the Xbox controller
     // The left stick controls the forward and backward movement of the robot, while the right stick
     // controls the rotation of the robot.
-    drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier));
   }
 
   /**
@@ -65,30 +61,50 @@ private void configureBindings() {
       OI.driveControllerA.onTrue(Commands.runOnce(() -> {
 		  drivetrain.resetPosition();
 	  }));
-
+
+      OI.driveRightTrigger.onTrue(new InstantCommand(() -> drivetrain.setSlow(true)));
+      OI.driveRightTrigger.onFalse(new InstantCommand(() -> drivetrain.setSlow(false)));
+
       // Operator controller bindings
       // Elevator level controls
-      OperatorBindings.operatorControllerA.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.LOW)));
-      OperatorBindings.operatorControllerB.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.HIGH)));
-      OperatorBindings.operatorControllerX.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.MED)));
-      OperatorBindings.operatorControllerY.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.EXTRA_HIGH)));
+      // OperatorBindings.operatorControllerA.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.LOW)));
+      // OperatorBindings.operatorControllerB.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.HIGH)));
+      // OperatorBindings.operatorControllerX.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.MED)));
+      // OperatorBindings.operatorControllerY.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.EXTRA_HIGH)));
+
+      // OPERATOR B : INTAKE
+      OI.operatorControllerB.onTrue(new IntakeCommandGroup(elevator, wrist, coralClaw));
+      // OPERATOR RB : TROUGH(L1)
+      OI.operatorControllerRightBumper.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 1)); 
+      // OPERATOR A : L2 (lowest pole)
+      OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 2));
+      // OPERATOR X : L3 (2nd pole)
+      OI.operatorControllerX.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 3));
+      // OPERATOR Y : L4 (last pole)
+      OI.operatorControllerY.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 4));
+
+      // OPERATOR LB: WRIST TOGGLE 
+      OI.operatorControllerLeftBumper.onTrue(new InstantCommand(() -> wrist.toggle()));
+
+      OI.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.trimTarget(0.0175)));
+      OI.operatorControllerBack.onTrue(new InstantCommand(() -> elevator.trimTarget(-0.0175)));
 
-      // Wrist toggle control with left bumper
-      OperatorBindings.operatorControllerLeftBumper.onTrue(new InstantCommand(() -> wrist.toggle()));
 
-      // Coral Claw controls with triggers
-      OperatorBindings.operatorRightTrigger.onTrue(new InstantCommand(() -> coralClaw.intake()));
-      OperatorBindings.operatorRightTrigger.onFalse(new InstantCommand(() -> coralClaw.stop()));
 
-      OperatorBindings.operatorLeftTrigger.onTrue(new InstantCommand(() -> coralClaw.outtake()));
-      OperatorBindings.operatorLeftTrigger.onFalse(new InstantCommand(() -> coralClaw.stop()));
+      // Coral Claw controls with triggers
+      // OI.operatorRightTrigger.onTrue(new InstantCommand(() -> coralClaw.intake()));
+      // OI.operatorRightTrigger.onFalse(new InstantCommand(() -> coralClaw.stop()));
+      OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, 1));
+      OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, 0));
+      // OI.operatorLeftTrigger.onTrue(new InstantCommand(() -> coralClaw.setOutput(coralClaw.OUTTAKE_SPEED)));
+      // OI.operatorLeftTrigger.onFalse(new InstantCommand(() -> coralClaw.stop()));
 
       // Reset encoder with Start button
-      OperatorBindings.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.resetEncoder()));
+      // OI.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.resetEncoder()));
 
       // Manual control with right stick for testing in simulation
-      OperatorBindings.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> 
-          elevator.setSpeed(OperatorBindings.processElevatorInput(OI.operatorController.getRightY())), elevator));
+      OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> 
+          elevator.setSpeed(OI.processElevatorInput(OI.operatorController.getRightY())), elevator));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/RobotMap.java

@@ -10,6 +10,8 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.PneumaticHub;
+import edu.wpi.first.wpilibj.Compressor;
 
 public class RobotMap {
 
@@ -29,8 +31,8 @@ private class CAN {
         private static final int RIGHT_BACK_DRIVE_CAN = 3;
         private static final int RIGHT_BACK_ROTATE_CAN = 4;
 
-        private static final int ELEVATOR_MOTOR_LEADER_CAN = 9; // Leader elevator motor
-        private static final int ELEVATOR_MOTOR_FOLLOWER_CAN = 10; // Follower elevator motor
+        private static final int ELEVATOR_MOTOR_LEADER_CAN = 10; // Leader elevator motor
+        private static final int ELEVATOR_MOTOR_FOLLOWER_CAN = 9; // Follower elevator motor
 
         /* PNEUMATICS */
         private static final int PCM_CAN_ID = 27; // Updated PCM ID (moved from 10)
@@ -94,6 +96,9 @@ private class Pneumatics {
     public static final SparkMax elevatorMotorFollower = new SparkMax(CAN.ELEVATOR_MOTOR_FOLLOWER_CAN, MotorType.kBrushless);
 
     // Pneumatic components
+    public static final PneumaticHub pneumaticHub = new PneumaticHub(CAN.PCM_CAN_ID);
+    public static final Compressor compressor = new Compressor(CAN.PCM_CAN_ID, PneumaticsModuleType.REVPH);
+
     public static final DoubleSolenoid wristSolenoid = new DoubleSolenoid(
         CAN.PCM_CAN_ID, 
         PneumaticsModuleType.REVPH, 

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

@@ -33,23 +33,23 @@ public ElevatorSequenceCommand(Elevator elevator) {
         // Add commands to the sequence
         addCommands(
             // Move to LOW level first
-            new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.LOW)),
+            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.LOW)),
             new WaitCommand(DELAY_SECONDS),
 
             // Move to MED level
-            new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.MED)),
+            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.MED)),
             new WaitCommand(DELAY_SECONDS),
 
             // Move to HIGH level
-            new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.HIGH)),
+            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.HIGH)),
             new WaitCommand(DELAY_SECONDS),
 
             // Move to EXTRA_HIGH level
-            new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.EXTRA_HIGH)),
+            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.EXTRA_HIGH)),
             new WaitCommand(DELAY_SECONDS),
 
             // Return to initial position (LOW)
-            new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.LOW))
+            new InstantCommand(() -> elevator.setTarget(ElevatorLevel.LOW))
         );
     }
 }

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

@@ -23,7 +23,7 @@ public FetchCoralFromStationCommand(Elevator elevator, CoralClaw coralClaw) {
 
     addCommands(
       // Move the elevator to STATION level
-      new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.STATION), elevator),
+      new InstantCommand(() -> elevator.setTarget(ElevatorLevel.STATION), elevator),
 
       // Wait until the elevator reaches the target position
       new WaitUntilCommand(elevator::isAtTarget),

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

@@ -0,0 +1,20 @@
+
+package frc.robot.commands;
+
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Wrist;
+import frc.robot.subsystems.CoralClaw;
+
+
+import frc.robot.ElevatorLevel;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeCommandGroup extends ParallelCommandGroup {
+    public IntakeCommandGroup(Elevator elevator, Wrist wrist, CoralClaw claw) {
+	addCommands(
+		    new InstantCommand(() -> elevator.setTarget(ElevatorLevel.STATION)),
+		    new InstantCommand(() -> wrist.retract())
+		    );
+    }
+}

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

@@ -3,9 +3,9 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.ElevatorLevel;
 import frc.robot.subsystems.CoralClaw;
 import frc.robot.subsystems.Elevator;
+import frc.robot.ElevatorLevel;
 
 /**
  * A command that handles waiting for coral release and then moving the elevator back to the STATION position.
@@ -32,7 +32,7 @@ public ReleaseCoralCommand(CoralClaw coralClaw, Elevator elevator) {
                 SmartDashboard.putString("Coral Claw State", "Coral released - returning to station")),
 
             // Move elevator to STATION level
-            Commands.runOnce(() -> elevator.moveToLevel(ElevatorLevel.STATION))
+            Commands.runOnce(() -> elevator.setTarget(ElevatorLevel.STATION))
         );
     }
 }