District

src/main/java/frc/robot/IDLookUp.java

@@ -0,0 +1,50 @@
+package frc.robot;
+
+public class IDLookUp {
+    public static double getIdAngle(int id) {
+	double output = 0.0;
+	switch (id) {
+	case 6:
+	    output = Math.toRadians(300.0);
+	    break;
+	case 7:
+	    output = Math.toRadians(0.0);
+	    break;
+	case 8:
+	    output = Math.toRadians(60.0);
+	    break;
+	case 9:
+	    output = Math.toRadians(120.0);
+	    break;
+	case 10:
+	    output = Math.toRadians(180.0);
+	    break;
+	case 11:
+	    output = Math.toRadians(240.0);
+	    break;
+	case 17:
+	    output = Math.toRadians(60.0);
+	    break;
+	case 18:
+	    output = Math.toRadians(0.0);
+	    break;
+	case 19:
+	    output = Math.toRadians(300.0);
+	    break;
+	case 20:
+	    output = Math.toRadians(240.0);
+	    break;
+	case 21:
+	    output = Math.toRadians(180.0);
+	    break;
+	case 22:
+	    output = Math.toRadians(120.0);
+	    break;
+	default:
+	    output = Math.toRadians(0.0);
+	    break;
+	}
+	return output;
+
+    }
+}

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

@@ -1,6 +1,7 @@
 package frc.robot;
 
 import java.util.function.DoubleSupplier;
+import java.util.function.BooleanSupplier;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -9,9 +10,9 @@
 import edu.wpi.first.wpilibj.XboxController;
 
 public class OI {
-    
+
     // Reference to RobotContainer for accessing subsystems
-    
+
     /**
      * Initialize OI with a reference to RobotContainer
      * @param container The RobotContainer instance
@@ -21,8 +22,8 @@ public class OI {
      * 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
+	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;
@@ -33,10 +34,11 @@ private static class PORT {
      */
     private static final XboxController driveController = new XboxController(PORT.DRIVE_CONTROLLER_PORT);
     public static final XboxController operatorController = new XboxController(PORT.OPERATOR_CONTROLLER_PORT); // Changed to public
-    
+
     private static class DRIVER_MAP {
 	private static final int ABUTTON = XboxController.Button.kA.value;
 	private static final int BBUTTON = XboxController.Button.kB.value;
+	private static final int LEFT_BUMPER = XboxController.Button.kLeftBumper.value;
     }
 
     // drive sticks
@@ -46,60 +48,63 @@ private static class DRIVER_MAP {
     public static DoubleSupplier xboxRightStickYSupplier = () -> { return processRotationInput(driveController.getRightY()); };
 
     // drive buttons
-    public static final JoystickButton driveControllerA = new JoystickButton(driveController, DRIVER_MAP.ABUTTON); 
+    public static final JoystickButton driveControllerA = new JoystickButton(driveController, DRIVER_MAP.ABUTTON);
     public static final JoystickButton driveControllerB = new JoystickButton(driveController, DRIVER_MAP.BBUTTON);
+    public static final JoystickButton driveControllerLB = new JoystickButton(driveController, DRIVER_MAP.LEFT_BUMPER);
+
     public static DoubleSupplier driveControllerRightTriggerSupplier = () -> { return driveController.getRightTriggerAxis();};
-
-
-
+    public static DoubleSupplier driveControllerLeftTriggerSupplier = () -> { return driveController.getLeftTriggerAxis();};
+
+
+
     private static double processDriveInput(double raw) {
-        if (Math.abs(raw) < 0.1) {
-            raw = 0 ;
-        }
-        return raw;
+	if (Math.abs(raw) < 0.1) {
+	    raw = 0 ;
+	}
+	return raw;
     }
 
     private static double processRotationInput(double raw) {
-        if (Math.abs(raw) < 0.1) {
-            raw = 0;
-        }
-        return raw*4;
+	if (Math.abs(raw) < 0.1) {
+	    raw = 0;
+	}
+	return raw*3;
     }
-    
+
     /**
      * Bindings on operator controls
      */
     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;
-        
+	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;
+
     }
-        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 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 DoubleSupplier operatorRightTriggerSupplier= () -> { return operatorController.getRightTriggerAxis();};
-        public static final Trigger operatorRightTrigger = new Trigger(() -> 
-            operatorController.getRightTriggerAxis() > TRIGGER_THRESHOLD);
-
-    public static DoubleSupplier operatorLeftTriggerSupplier= () -> { return operatorController.getLeftTriggerAxis();};
+    public static final Trigger operatorRightTrigger = new Trigger(() ->
+								   operatorController.getRightTriggerAxis() > TRIGGER_THRESHOLD);
 
-        public static final Trigger operatorLeftTrigger = new Trigger(() -> 
-            operatorController.getLeftTriggerAxis() > TRIGGER_THRESHOLD);
-
-        public static double processElevatorInput(double input) {
-            return -Math.signum(input) * Math.pow(Math.abs(input), 2) * 0.7;
-        }
+    public static DoubleSupplier operatorLeftTriggerSupplier= () -> { return operatorController.getLeftTriggerAxis();};
+    public static final Trigger operatorLeftTrigger = new Trigger(() ->
+								  operatorController.getLeftTriggerAxis() > TRIGGER_THRESHOLD);
+    public static BooleanSupplier operatorLeftStickButtonSupplier = () -> { return operatorController.getLeftStickButton(); };
+
+    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

@@ -45,23 +45,23 @@ public String toString() {
   }
 
   // Create SendableChooser for test options
-  private final SendableChooser<TestOption> testChooser = new SendableChooser<>();
+  // private final SendableChooser<TestOption> testChooser = new SendableChooser<>();
 
   public Robot() {
     m_robotContainer = new RobotContainer();
 
     // Initialize test chooser
-    testChooser.setDefaultOption("No Test", TestOption.NONE);
-    testChooser.addOption("Elevator to Low", TestOption.ELEVATOR_LOW);
-    testChooser.addOption("Elevator to Mid", TestOption.ELEVATOR_MID);
-    testChooser.addOption("Elevator to High", TestOption.ELEVATOR_HIGH);
-    testChooser.addOption("Elevator to Station", TestOption.ELEVATOR_STATION);
-    testChooser.addOption("Run Coral Claw Intake", TestOption.CORAL_CLAW_INTAKE);
-    testChooser.addOption("Fetch Coral From Station", TestOption.FETCH_CORAL_STATION);
+    // testChooser.setDefaultOption("No Test", TestOption.NONE);
+    // testChooser.addOption("Elevator to Low", TestOption.ELEVATOR_LOW);
+    // testChooser.addOption("Elevator to Mid", TestOption.ELEVATOR_MID);
+    // testChooser.addOption("Elevator to High", TestOption.ELEVATOR_HIGH);
+    // testChooser.addOption("Elevator to Station", TestOption.ELEVATOR_STATION);
+    // testChooser.addOption("Run Coral Claw Intake", TestOption.CORAL_CLAW_INTAKE);
+    // testChooser.addOption("Fetch Coral From Station", TestOption.FETCH_CORAL_STATION);
     // Add more options as needed
 
     // Add the chooser to the dashboard
-    SmartDashboard.putData("Test Selector", testChooser);
+    // SmartDashboard.putData("Test Selector", testChooser);
   }
 
   @Override
@@ -114,33 +114,33 @@ public void teleopExit() {}
   public void testInit() {
     CommandScheduler.getInstance().cancelAll();
       // Get the selected test option
-      TestOption selectedTest = testChooser.getSelected();
+      // TestOption selectedTest = testChooser.getSelected();
 
       // Run the selected test command
-      switch(selectedTest) {
-        case ELEVATOR_LOW:
-          new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.LOW).schedule();
-          break;
-        case ELEVATOR_MID:
-          new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.MED).schedule();
-          break;
-        case ELEVATOR_HIGH:
-          new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.HIGH).schedule();
-          break;
-        case ELEVATOR_STATION:
-          new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.STATION).schedule();
-          break;
-        case CORAL_CLAW_INTAKE:
-          new InstantCommand(() -> m_robotContainer.coralClaw.intake()).schedule();
-          break;
-        case FETCH_CORAL_STATION:
-          new FetchCoralFromStationCommand(m_robotContainer.elevator, m_robotContainer.coralClaw).schedule();
-          break;
-        case NONE:
-        default:
+      // switch(selectedTest) {
+        // case ELEVATOR_LOW:
+          // new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.LOW).schedule();
+          // break;
+        // case ELEVATOR_MID:
+          // new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.MED).schedule();
+          // break;
+        // case ELEVATOR_HIGH:
+          // new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.HIGH).schedule();
+          // break;
+        // case ELEVATOR_STATION:
+          // new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.STATION).schedule();
+          // break;
+        // case CORAL_CLAW_INTAKE:
+          // new InstantCommand(() -> m_robotContainer.coralClaw.intake()).schedule();
+          // break;
+        // case FETCH_CORAL_STATION:
+          // new FetchCoralFromStationCommand(m_robotContainer.elevator, m_robotContainer.coralClaw).schedule();
+          // break;
+        // case NONE:
+        // default:
           // Do nothing
-          break;
-      }
+          // break;
+      // }
   }
 
   @Override

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

@@ -14,15 +14,15 @@
 import frc.robot.commands.instant.*;
 
 public class RobotContainer {
-  public final String[] AUTOS = {"none", "pass line", "side 1", "side 2", "side 6"};
-  public final String AUTO_DEFAULT = AUTOS[1];
-  public static String autoSelected;
-  public static SendableChooser<String> autoSelector = new SendableChooser<>();
-
+    public final String[] AUTOS = {"none", "pass line", "side 1", "side 2", "side 3", "side 5", "side 6"};
+    public final String AUTO_DEFAULT = AUTOS[1];
+    public static String autoSelected;
+    public static SendableChooser<String> autoSelector = new SendableChooser<>();
+    
 
 
-  // Constants
-  private static final double BEVEL_IN_CORRECTION = 0.25;
+    // Constants
+    private static final double BEVEL_IN_CORRECTION = 0.25;
 
     /* init subsystems */
 
@@ -34,7 +34,7 @@ public class RobotContainer {
     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 CoralClaw coralClaw = new CoralClaw(RobotMap.coralClawPrimaryMotor, RobotMap.coralClawSecondaryMotor, RobotMap.algaeClawMotor, RobotMap.coralLimitSwitch);
 
     public Camera camera = new Camera();
     public Drivetrain drivetrain = new Drivetrain(leftBackSwerve, rightBackSwerve, leftFrontSwerve, rightFrontSwerve, gyro);
@@ -52,7 +52,7 @@ public class RobotContainer {
   public RobotContainer() {
     // Initialize OI with a reference to this RobotContainer instance
     configureBindings();
-    drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier, OI.driveControllerRightTriggerSupplier));
+    drivetrain.setDefaultCommand(new DriveCommand(drivetrain, elevator, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier, OI.driveControllerRightTriggerSupplier, OI.operatorLeftStickButtonSupplier));
     RobotMap.compressor.enableAnalog(70, 120);
 
     autoSelector.setDefaultOption("default side (2)", AUTO_DEFAULT);
@@ -76,7 +76,8 @@ private void configureBindings() {
       OI.driveControllerA.onTrue(Commands.runOnce(() -> {
 		  drivetrain.resetPosition();
 	  }));
-
+      OI.driveControllerB.whileTrue(new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH));
+      OI.driveControllerLB.whileTrue(new AlgaeRemoverCommand(coralClaw));
       // Operator controller bindings
       // Elevator level controls
       // OPERATOR B : INTAKE
@@ -99,9 +100,9 @@ private void configureBindings() {
       OI.operatorControllerBack.onTrue(new InstantCommand(() -> elevator.trimTarget(-0.0175)));
 
       // OPERATOR RT : INTAKE
-      OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorRightTriggerSupplier, 1));
+      OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, elevator, OI.operatorRightTriggerSupplier, 1));
       // OPERATOR LT : OUTTAKE
-      OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorLeftTriggerSupplier, 0));
+      OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, elevator, OI.operatorLeftTriggerSupplier, 0));
 
       // Manual control with right stick for testing in simulation
       // OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> 
@@ -118,7 +119,11 @@ public Command getAutonomousCommand() {
       return new ScoreTest(1, false, drivetrain, gyro, camera, elevator, wrist, coralClaw);
     } else if(auto == AUTOS[3]) { // side 2 (right)
       return new ScoreTest(2, true, drivetrain, gyro, camera, elevator, wrist, coralClaw);
-    } else if(auto == AUTOS[4]) { // sifre
+    } else if(auto == AUTOS[4]) { // side 3 (right 2)
+	  return new ScoreTest(3, true, drivetrain, gyro, camera, elevator, wrist, coralClaw);
+    } else if(auto == AUTOS[5]) { // side 5
+	  return new ScoreTest(5, true, drivetrain, gyro, camera, elevator, wrist, coralClaw);
+    } else if(auto == AUTOS[6]) { // side 6 
       return new ScoreTest(6, true, drivetrain, gyro, camera, elevator, wrist, coralClaw);
     } else {
       return null;

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

@@ -40,6 +40,8 @@ private class CAN {
         /* MECHANISMS */
         private static final int CORAL_CLAW_MOTOR_PRIMARY_CAN = 11; // First coral claw motor
         private static final int CORAL_CLAW_MOTOR_SECONDARY_CAN = 12; // Second coral claw motor
+        private static final int ALGAE_CLAW_MOTOR_CAN = 13; // Algae claw motor
+
 
         /**
          * FRONT
@@ -109,6 +111,7 @@ private class Pneumatics {
     // Create two NEO motors for the coral claw
     public static final SparkMax coralClawPrimaryMotor = new SparkMax(CAN.CORAL_CLAW_MOTOR_PRIMARY_CAN, MotorType.kBrushless);
     public static final SparkMax coralClawSecondaryMotor = new SparkMax(CAN.CORAL_CLAW_MOTOR_SECONDARY_CAN, MotorType.kBrushless);
+    public static final SparkMax algaeClawMotor = new SparkMax(CAN.ALGAE_CLAW_MOTOR_CAN, MotorType.kBrushless);
 
     // Coral claw limit switch
     public static final DigitalInput coralLimitSwitch = new DigitalInput(DIO.CORAL_LIMIT_SWITCH);