Initial Fixes

src/main/java/frc/robot/Constants.java

@@ -100,4 +100,11 @@ public static final class AutoConstants {
   public static final class NeoMotorConstants {
     public static final double kFreeSpeedRpm = 5676;
   }
+
+  public static final class ClimberConstants{
+    public static final int climberMotor = 13;
+  }  
+  public static final class CoralIntakeConstants{
+    public static final int intakeMotor = 15;
+  }
 }

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

@@ -18,14 +18,21 @@
 import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.OIConstants;
+import frc.robot.subsystems.AlgaeIntake;
+import frc.robot.subsystems.Climber;
+import frc.robot.subsystems.CoralIntake;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Wrist;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import java.util.List;
+import java.util.function.BooleanSupplier;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -37,11 +44,16 @@
 public class RobotContainer {
   // The robot's subsystems
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  private final AlgaeIntake m_algaeIntake = new AlgaeIntake();
+  private final Climber m_climber = new Climber();
+  private final Elevator m_elevator = new Elevator();
+  private final CoralIntake m_coralIntake = new CoralIntake();
+  private final Wrist m_wrist = new Wrist();
 
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
-    // The operators controller
-    XboxController m_operatorController = new XboxController(OIConstants.kOperatorControllerPort);
+  // The operators controller
+  CommandXboxController m_operatorController = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -78,13 +90,36 @@ private void configureButtonBindings() {
             () -> m_robotDrive.setX(),
             m_robotDrive));
 
-       new JoystickButton(m_driverController, XboxController.Button.kStart.value)
+    new JoystickButton(m_driverController, XboxController.Button.kY.value)
         .whileTrue(new InstantCommand(
             () -> m_robotDrive.resetGyro(),
-            m_robotDrive));      
+            m_robotDrive));
+    // AlgaeIntake
+    m_operatorController.rightTrigger(0.1).whileTrue(new InstantCommand(() -> m_algaeIntake.intake(0.2)))
+        .onFalse(new InstantCommand(() -> m_algaeIntake.stop()));
+    m_operatorController.rightBumper().whileTrue(new InstantCommand(() -> m_algaeIntake.eject()))
+        .onFalse(new InstantCommand(() -> m_algaeIntake.stop()));
+    // Climber
+    m_operatorController.a().whileTrue(new InstantCommand(() -> m_climber.climb(m_operatorController.getLeftY())))
+        .onFalse(new InstantCommand(() -> m_climber.stopClimb()));
+    // Elevator
+    m_operatorController.x().whileTrue(new InstantCommand(() -> m_elevator.move(m_operatorController.getLeftY())))
+        .onFalse(new InstantCommand(() -> m_elevator.stop()));
+    // CoralIntake
+    m_operatorController.leftTrigger(0.1).whileTrue(new InstantCommand(() -> m_coralIntake.intake(0.2)))
+        .onFalse(new InstantCommand(() -> m_algaeIntake.stop()));
+    m_operatorController.leftBumper().whileTrue(new InstantCommand(() -> m_algaeIntake.eject()))
+        .onFalse(new InstantCommand(() -> m_algaeIntake.stop()));
+    // Wrist
+    m_operatorController.b().whileTrue(new InstantCommand(() -> m_wrist.moveUp(0.1)))
+        .onFalse(new InstantCommand(() -> m_wrist.stop()));
+    m_operatorController.y().whileTrue(new InstantCommand(() -> m_wrist.moveDown(-0.1)))
+        .onFalse(new InstantCommand(() -> m_wrist.stop()));
+
   }
 
-  /**  pass the autonomous command to the main {@link Robot} class.
+  /**
+   * pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
    */

src/main/java/frc/robot/subsystems/AlgaeIntake.java

@@ -0,0 +1,36 @@
+package frc.robot.subsystems;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+
+public class AlgaeIntake extends SubsystemBase {
+    private final SparkMax intakeMotor1;
+    private final SparkMax intakeMotor2;
+
+    public AlgaeIntake() {
+        intakeMotor1 = new SparkMax(17, MotorType.kBrushless); // CAN ID 17
+        intakeMotor2 = new SparkMax(27, MotorType.kBrushless); // CAN ID 27
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+    }
+
+    public void intake(double speed) {
+        intakeMotor1.set(speed); // Set motor 1 to intake
+        intakeMotor2.set(-speed); // Set motor 2 to intake in opposite direction
+    }
+
+    public void eject() {
+        intakeMotor1.set(-1.0); // Set motor 1 to eject
+        intakeMotor2.set(1.0); // Set motor 2 to eject in opposite direction
+    }
+
+    public void stop() {
+        intakeMotor1.stopMotor(); // Stop motor 1
+        intakeMotor2.stopMotor(); // Stop motor 2
+    }
+}

src/main/java/frc/robot/subsystems/Climber.java

@@ -0,0 +1,31 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Climber extends SubsystemBase {
+    private final SparkMax climberMotor;
+
+    public Climber() {
+        climberMotor = new SparkMax(Constants.ClimberConstants.climberMotor, MotorType.kBrushless); // CAN ID 13
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+    }
+
+    public void climb(double speed) {
+        climberMotor.set(speed); // Set motor to full speed up
+    }
+
+
+    public void stopClimb() {
+        climberMotor.stopMotor(); // Stop the motor
+    }
+}

src/main/java/frc/robot/subsystems/CoralIntake.java

@@ -0,0 +1,33 @@
+package frc.robot.subsystems;
+
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+public class CoralIntake extends SubsystemBase {
+    private final SparkMax intakeMotor;
+
+    public CoralIntake() {
+        intakeMotor = new SparkMax(Constants.CoralIntakeConstants.intakeMotor, MotorType.kBrushless); // CAN ID 15
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+    }
+
+    public void intake(double speed) {
+        intakeMotor.set(speed); // Set motor to intake
+    }
+
+    public void eject() {
+        intakeMotor.set(-1.0); // Set motor to eject
+    }
+
+    public void stop() {
+        intakeMotor.stopMotor(); // Stop the motor
+    }
+}