diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 633cb83..e3807c6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -142,27 +142,36 @@ public static final class SwerveDriveConfig { } /** - * Configurations for the elevator. + * Configurations for the coraller. */ - public static final class ElevatorConfig { + public static final class CorallerConfig { + // Angler + public static final double kAnglerP = 0.0; + public static final double kAnglerI = 0.0; + public static final double kAnglerD = 0.0; + public static final double kAnglerEncoderPositionConversionFactor = 1; + public static final double kAnglerEncoderVelocityConversionFactor = kAnglerEncoderPositionConversionFactor / 60.0; + + // Elevator public static final double kElevatorP = 0.0; public static final double kElevatorI = 0.0; public static final double kElevatorD = 0.0; public static final double kElevatorEncoderPositionConversionFactor = 1; - public static final double kElevatorEncoderVelocityConversionFactor = kElevatorEncoderPositionConversionFactor - / 60.0; + public static final double kElevatorEncoderVelocityConversionFactor = kElevatorEncoderPositionConversionFactor / 60.0; public static final double kRobotElevatorStowHeightInches = 48; - } - /** - * Configurations for the coraller. - */ - public static final class CorallerConfig { - public static final double kCorallerP = 0.0; - public static final double kCorallerI = 0.0; - public static final double kCorallerD = 0.0; - public static final double kCorallerEncoderPositionConversionFactor = 1; - public static final double kCorallerEncoderVelocityConversionFactor = kCorallerEncoderPositionConversionFactor - / 60.0; + // Coraller Configuration + public static final double STOW_ANGLE = 0; + public static final double STOW_HEIGHT = 0; + public static final double L1_ANGLE = 0; + public static final double L1_HEIGHT = 0; + public static final double L2_ANGLE = 0; + public static final double L2_HEIGHT = 0; + public static final double L3_ANGLE = 0; + public static final double L3_HEIGHT = 0; + public static final double L4_ANGLE = 0; + public static final double L4_HEIGHT = 0; + public static final double RECEIVE_ANGLE = 0; + public static final double RECEIVE_HEIGHT = 0; } } diff --git a/src/main/java/frc/robot/subsystems/coraller/Angler.java b/src/main/java/frc/robot/subsystems/coraller/Angler.java index 2bc870b..d0c7cea 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Angler.java +++ b/src/main/java/frc/robot/subsystems/coraller/Angler.java @@ -23,7 +23,7 @@ public class Angler { public Angler() { var anglerClosedLoopConfig = new ClosedLoopConfig() - .pid(CorallerConfig.kCorallerP, CorallerConfig.kCorallerI, CorallerConfig.kCorallerD, + .pid(CorallerConfig.kAnglerP, CorallerConfig.kAnglerI, CorallerConfig.kAnglerD, ClosedLoopSlot.kSlot0); var anglerConfig = new SparkMaxConfig() .idleMode(IdleMode.kBrake) diff --git a/src/main/java/frc/robot/subsystems/coraller/Coraller.java b/src/main/java/frc/robot/subsystems/coraller/Coraller.java index 71a589e..9a8eb11 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Coraller.java +++ b/src/main/java/frc/robot/subsystems/coraller/Coraller.java @@ -3,16 +3,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ElevatorConfig; +import frc.robot.Constants.CorallerConfig; public class Coraller extends SubsystemBase { public enum Configuration { - STOW(CorallerConstants.STOW_HEIGHT, CorallerConstants.STOW_ANGLE), - L1(CorallerConstants.L1_HEIGHT, CorallerConstants.L1_ANGLE), - L2(CorallerConstants.L2_HEIGHT, CorallerConstants.L2_ANGLE), - L3(CorallerConstants.L3_HEIGHT, CorallerConstants.L3_ANGLE), - L4(CorallerConstants.L4_HEIGHT, CorallerConstants.L4_ANGLE), - RECEIVE(CorallerConstants.RECEIVE_HEIGHT, CorallerConstants.RECEIVE_ANGLE); + STOW(CorallerConfig.STOW_HEIGHT, CorallerConfig.STOW_ANGLE), + L1(CorallerConfig.L1_HEIGHT, CorallerConfig.L1_ANGLE), + L2(CorallerConfig.L2_HEIGHT, CorallerConfig.L2_ANGLE), + L3(CorallerConfig.L3_HEIGHT, CorallerConfig.L3_ANGLE), + L4(CorallerConfig.L4_HEIGHT, CorallerConfig.L4_ANGLE), + RECEIVE(CorallerConfig.RECEIVE_HEIGHT, CorallerConfig.RECEIVE_ANGLE); private final double elevatorPosition; private final double anglerPosition; @@ -28,7 +28,7 @@ private Configuration(double elevatorPosition, double anglerPosition) { private final Intake intake; public Coraller() { - elevator = new Elevator(ElevatorConfig.kRobotElevatorStowHeightInches); + elevator = new Elevator(CorallerConfig.kRobotElevatorStowHeightInches); angler = new Angler(); intake = new Intake(); } diff --git a/src/main/java/frc/robot/subsystems/coraller/CorallerConstants.java b/src/main/java/frc/robot/subsystems/coraller/CorallerConstants.java deleted file mode 100644 index 85c782e..0000000 --- a/src/main/java/frc/robot/subsystems/coraller/CorallerConstants.java +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.coraller; - -/** Add your docs here. */ -public class CorallerConstants { - public static final double STOW_ANGLE = 0; - public static final double STOW_HEIGHT = 0; - - public static final double L1_ANGLE = 0; - public static final double L1_HEIGHT = 0; - - public static final double L2_ANGLE = 0; - public static final double L2_HEIGHT = 0; - - public static final double L3_ANGLE = 0; - public static final double L3_HEIGHT = 0; - - public static final double L4_ANGLE = 0; - public static final double L4_HEIGHT = 0; - - public static final double RECEIVE_ANGLE = 0; - public static final double RECEIVE_HEIGHT = 0; -} diff --git a/src/main/java/frc/robot/subsystems/coraller/Elevator.java b/src/main/java/frc/robot/subsystems/coraller/Elevator.java index 4071ffc..289db50 100644 --- a/src/main/java/frc/robot/subsystems/coraller/Elevator.java +++ b/src/main/java/frc/robot/subsystems/coraller/Elevator.java @@ -13,7 +13,7 @@ import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.RobotMap; -import frc.robot.Constants.ElevatorConfig; +import frc.robot.Constants.CorallerConfig; public class Elevator { private final SparkMax motor; @@ -25,10 +25,10 @@ public class Elevator { public Elevator(double base) { var elevatorEncoderConfig = new EncoderConfig() - .positionConversionFactor(ElevatorConfig.kElevatorEncoderPositionConversionFactor) - .velocityConversionFactor(ElevatorConfig.kElevatorEncoderVelocityConversionFactor); + .positionConversionFactor(CorallerConfig.kElevatorEncoderPositionConversionFactor) + .velocityConversionFactor(CorallerConfig.kElevatorEncoderVelocityConversionFactor); var elevatorClosedLoopConfig = new ClosedLoopConfig() - .pid(ElevatorConfig.kElevatorP, ElevatorConfig.kElevatorI, ElevatorConfig.kElevatorD, ClosedLoopSlot.kSlot0); + .pid(CorallerConfig.kElevatorP, CorallerConfig.kElevatorI, CorallerConfig.kElevatorD, ClosedLoopSlot.kSlot0); var elevatorConfig = new SparkMaxConfig() .idleMode(IdleMode.kBrake) .smartCurrentLimit(45)