fleshed out subsystems

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

@@ -5,38 +5,118 @@
 package frc.robot;
 
 import frc.robot.constants.OIConstants;
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.ClimberPivot;
+import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Indexer;
+import frc.robot.subsystems.Manipulator;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
-  private DriveSubsystem drivetrain;
+	private Arm arm;
 
-  private CommandXboxController driver;
+	private ClimberPivot climberPivot;
 
-  public RobotContainer() {
-    drivetrain = new DriveSubsystem();
+	private ClimberRollers climberRollers;
 
-    driver = new CommandXboxController(OIConstants.kDriverControllerPort);
-    
-    configureButtonBindings();
-  }
+	private DriveSubsystem drivetrain;
 
-  private void configureButtonBindings() {
-    drivetrain.setDefaultCommand(
-        drivetrain.drive(
-            driver::getLeftY, 
-            driver::getLeftX, 
-            driver::getRightX, 
-            () -> true
-        )
-    );
+	private Elevator elevator;
 
-    driver.start().whileTrue(drivetrain.setXCommand());
-  }
+	private Indexer indexer;
 
-  public Command getAutonomousCommand() {
-    return new PrintCommand("NO AUTO DEFINED");
-  }
+	private Manipulator manipulator;
+
+	private CommandXboxController driver;
+	private CommandXboxController operator;
+
+	public RobotContainer() {
+		arm = new Arm();
+
+		climberPivot = new ClimberPivot();
+
+		climberRollers = new ClimberRollers();
+
+		drivetrain = new DriveSubsystem();
+
+		elevator = new Elevator();
+
+		indexer = new Indexer();
+
+		manipulator = new Manipulator();
+
+		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
+		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
+
+		configureButtonBindings();
+	}
+
+	private void configureButtonBindings() {
+		arm.setDefaultCommand(
+			arm.goToSetpoint(0, 1)
+		);
+
+		climberPivot.setDefaultCommand(
+			climberPivot.goToAngle(0)
+		);
+
+		climberRollers.setDefaultCommand(
+			climberRollers.runRoller(0)
+		);
+
+		drivetrain.setDefaultCommand(
+			drivetrain.drive(
+				driver::getLeftY,
+				driver::getLeftX,
+				driver::getRightX,
+				() -> true
+			)
+		);
+
+		elevator.setDefaultCommand(
+			elevator.goToSetpoint(0, 1)
+		);
+
+		indexer.setDefaultCommand(
+			indexer.runIndexer(0)
+		);
+
+		manipulator.setDefaultCommand(
+			manipulator.runManipulator(0)
+		);
+
+		//Driver inputs
+		driver.start().whileTrue(
+			drivetrain.setXCommand()
+		);
+
+		driver.rightTrigger().whileTrue(
+			manipulator.runManipulator(1)
+		);
+
+		//Operator inputs
+		operator.povUp().onTrue(
+			elevator.goToSetpoint(0, 0)
+		);
+
+		operator.povRight().onTrue(
+			elevator.goToSetpoint(0, 0)
+		);
+
+		operator.povLeft().onTrue(
+			elevator.goToSetpoint(0, 0)
+		);
+
+		operator.povDown().onTrue(
+			elevator.goToSetpoint(0, 0)
+		);
+	}
+
+	public Command getAutonomousCommand() {
+		return new PrintCommand("NO AUTO DEFINED");
+	}
 }

src/main/java/frc/robot/constants/ArmConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class ArmConstants {
+    public static final int kArmMotorID = 0;
+    public static final int kCANcoderID = 0;
+}

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -1,7 +1,7 @@
 package frc.robot.constants;
 
 public class ElevatorConstants {
-    public static final int kMotor1ID = 0;
+    public static final int kElevatorMotorID = 0;
 
     public static final double kPIDControllerP = 0;
     public static final double kPIDControllerI = 0;

src/main/java/frc/robot/constants/IDs

@@ -0,0 +1,29 @@
+CAN
+----------------------------
+ARM
+kArmMotorID = 0
+kCANcoderID = 0
+
+CLIMBER
+kPivotMotorID = 0
+kRollerMotorID = 0
+
+DRIVEBASE
+kFrontLeftDrivingCanId = 11
+kRearLeftDrivingCanId = 13
+kFrontRightDrivingCanId = 15
+kRearRightDrivingCanId = 17
+kFrontLeftTurningCanId = 10
+kRearLeftTurningCanId = 12
+kFrontRightTurningCanId = 14
+kRearRightTurningCanId = 16
+
+ELEVATOR
+kElevatorMotorID = 0
+
+MANIPULATOR
+kManipulatorMotorID = 0
+
+PWM
+----------------------------
+

src/main/java/frc/robot/constants/IndexerConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class IndexerConstants {
+    public static final int kIndexerMotorID = 0;
+    public static final int kIndexerBeamBreakID = 0;
+}

src/main/java/frc/robot/constants/ManipulatorConstants.java

@@ -0,0 +1,7 @@
+package frc.robot.constants;
+
+public class ManipulatorConstants {
+    public static final int kManipulatorMotorID = 0;
+    public static final int kCoralBeamBreakID = 0;
+    public static final int kAlgaeBeamBreakID = 0;
+}

src/main/java/frc/robot/constants/OIConstants.java

@@ -2,6 +2,7 @@ package frc.robot.constants;
 
 public class OIConstants {
     public static final int kDriverControllerPort = 0;
+    public static final int kOperatorControllerPort = 1;
     
     public static final double kDriveDeadband = 0.05;
 }

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

@@ -0,0 +1,43 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ArmConstants;
+
+public class Arm extends SubsystemBase {
+    private SparkMax ArmMotor;
+
+    private CANcoder canCoder;
+
+    private PIDController pidController;
+    private ArmFeedforward feedForward;
+
+    public Arm() {
+        ArmMotor = new SparkMax(
+            ArmConstants.kArmMotorID, 
+            MotorType.kBrushless
+        );
+
+        canCoder = new CANcoder(ArmConstants.kCANcoderID);
+    }
+
+    public Command goToSetpoint(double setpoint, double timeout) {
+        return run(() -> {
+            double voltsOut = pidController.calculate(
+                canCoder.getPosition().getValueAsDouble(), 
+                setpoint
+            ) + feedForward.calculate(
+                canCoder.getPosition().getValueAsDouble(), 
+                canCoder.getVelocity().getValueAsDouble()
+            );
+
+            ArmMotor.setVoltage(voltsOut);
+        }).until(pidController::atSetpoint).withTimeout(timeout);
+    }
+}

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

@@ -24,7 +24,6 @@ public class ClimberPivot extends SubsystemBase {
             ClimberPivotConstants.kPivotMotorID, 
             MotorType.kBrushless
         );
-        pivotMotor.setInverted(true);
 
         neoEncoder = pivotMotor.getAbsoluteEncoder();
 

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

@@ -15,7 +15,6 @@ public class ClimberRollers extends SubsystemBase {
             ClimberRollersConstants.kRollerMotorID, 
             MotorType.kBrushless
         );
-        rollerMotor.setInverted(false);
     }
 
     public Command runRoller(double speed) {

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

@@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ElevatorConstants;
 
 public class Elevator extends SubsystemBase {
-    private SparkMax motor1;
+    private SparkMax elevatorMotor1;
 
     private AbsoluteEncoder encoder;
 
@@ -20,12 +20,12 @@ public class Elevator extends SubsystemBase {
     private ElevatorFeedforward feedForward;
     
     public Elevator() {
-        motor1 = new SparkMax(
-            ElevatorConstants.kMotor1ID, 
+        elevatorMotor1 = new SparkMax(
+            ElevatorConstants.kElevatorMotorID, 
             MotorType.kBrushless
         );
 
-        encoder = motor1.getAbsoluteEncoder();
+        encoder = elevatorMotor1.getAbsoluteEncoder();
 
         pidController = new PIDController(
             ElevatorConstants.kPIDControllerP, 
@@ -42,19 +42,18 @@ public class Elevator extends SubsystemBase {
 
     public Command runElevator(double speed) {
         return run(() -> {
-            motor1.set(speed);
+            elevatorMotor1.set(speed);
         });
     }
     
-    public Command goToSetpoint(double setpoint) {
+    public Command goToSetpoint(double setpoint, double timeout) {
         return run(() -> {
-            motor1.set(feedForward.calculate(
-                encoder.getVelocity(), 
-                pidController.calculate(
-                    encoder.getPosition(), 
-                    setpoint
-                )
-            ));
-        });
+            double voltsOut = pidController.calculate(
+                encoder.getPosition(),
+                setpoint
+            ) + feedForward.calculate(0);
+
+            elevatorMotor1.setVoltage(voltsOut);
+        }).until(pidController::atSetpoint).withTimeout(timeout);
     }
 }

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

@@ -0,0 +1,37 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IndexerConstants;
+
+public class Indexer extends SubsystemBase {
+    private SparkMax indexerMotor;
+
+    private DigitalInput indexerBeamBreak;
+
+    public Indexer() {
+        indexerMotor = new SparkMax(
+            IndexerConstants.kIndexerMotorID, 
+            MotorType.kBrushless
+        );
+
+        indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakID);
+    }
+
+    public Command runIndexer(double speed) {
+        return run(() -> {
+            indexerMotor.set(speed);
+        });
+    }
+
+    public Command indexCoral(double speed) {
+        return run(() -> {
+            indexerMotor.set(speed);
+        })
+        .until(indexerBeamBreak::get);
+    }
+}

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

@@ -0,0 +1,46 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ManipulatorConstants;
+
+public class Manipulator extends SubsystemBase {
+    private SparkMax manipulatorMotor;
+
+    private DigitalInput coralBeamBreak;
+    private DigitalInput algaeBeamBreak;
+
+    public Manipulator() {
+        manipulatorMotor = new SparkMax(
+            ManipulatorConstants.kManipulatorMotorID, 
+            MotorType.kBrushless
+        );
+
+        coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID);
+        algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID);
+    }
+
+    public Command runManipulator(double speed) {
+        return run(() -> {
+            manipulatorMotor.set(speed);
+        });
+    }
+
+    public Command runUntilCollected(double speed, boolean coral) {
+        return run(() -> {
+            manipulatorMotor.set(coral ? speed : speed * -1);
+        })
+        .until(() -> coralBeamBreak.get() || algaeBeamBreak.get());
+    }
+
+    public Command indexCoral(double speed) {
+        return run(() -> {
+            manipulatorMotor.set(speed);
+        })
+        .until(coralBeamBreak::get);
+    }
+}