From dff4d0e04f6ffd527dfd0fe373e7fe7294d63153 Mon Sep 17 00:00:00 2001 From: NoahLacks63 Date: Tue, 21 Jan 2025 04:18:36 +0000 Subject: [PATCH] added spark configs to all subsystems, fixed a few formatting inconsistencies, added a TODO --- src/main/java/frc/robot/constants/ArmConstants.java | 2 +- .../java/frc/robot/constants/ClimberPivotConstants.java | 4 ++++ .../frc/robot/constants/ClimberRollersConstants.java | 4 ++++ .../java/frc/robot/constants/ManipulatorConstants.java | 4 ++++ src/main/java/frc/robot/subsystems/Arm.java | 6 +++++- src/main/java/frc/robot/subsystems/ClimberPivot.java | 8 ++++++++ src/main/java/frc/robot/subsystems/ClimberRollers.java | 9 +++++++++ src/main/java/frc/robot/subsystems/Manipulator.java | 8 ++++++++ 8 files changed, 43 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 48be67a..f4cf608 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -69,7 +69,7 @@ public class ArmConstants { static { canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - canCoderConfig.MagnetSensor.MagnetOffset = 0.0; + canCoderConfig.MagnetSensor.MagnetOffset = kMagnetOffset; // TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0 //canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; diff --git a/src/main/java/frc/robot/constants/ClimberPivotConstants.java b/src/main/java/frc/robot/constants/ClimberPivotConstants.java index b83b040..a0d59f4 100644 --- a/src/main/java/frc/robot/constants/ClimberPivotConstants.java +++ b/src/main/java/frc/robot/constants/ClimberPivotConstants.java @@ -1,5 +1,7 @@ package frc.robot.constants; +import com.revrobotics.spark.config.SparkMaxConfig; + public class ClimberPivotConstants { public static final int kPivotMotorID = 0; @@ -12,4 +14,6 @@ public class ClimberPivotConstants { public static final double kPIDControllerD = 0; public static final double kClimberClimbPosition = 0; + + public static final SparkMaxConfig motorConfig = new SparkMaxConfig(); } diff --git a/src/main/java/frc/robot/constants/ClimberRollersConstants.java b/src/main/java/frc/robot/constants/ClimberRollersConstants.java index 60b7404..7acd7a6 100644 --- a/src/main/java/frc/robot/constants/ClimberRollersConstants.java +++ b/src/main/java/frc/robot/constants/ClimberRollersConstants.java @@ -1,5 +1,9 @@ package frc.robot.constants; +import com.revrobotics.spark.config.SparkMaxConfig; + public class ClimberRollersConstants { public static final int kRollerMotorID = 0; + + public static final SparkMaxConfig motorConfig = new SparkMaxConfig(); } diff --git a/src/main/java/frc/robot/constants/ManipulatorConstants.java b/src/main/java/frc/robot/constants/ManipulatorConstants.java index 6bdb595..0cea272 100644 --- a/src/main/java/frc/robot/constants/ManipulatorConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorConstants.java @@ -1,7 +1,11 @@ package frc.robot.constants; +import com.revrobotics.spark.config.SparkMaxConfig; + public class ManipulatorConstants { public static final int kManipulatorMotorID = 0; public static final int kCoralBeamBreakID = 0; public static final int kAlgaeBeamBreakID = 0; + + public static final SparkMaxConfig motorConfig = new SparkMaxConfig(); } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 943e43c..76b275d 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -32,7 +32,11 @@ public class Arm extends SubsystemBase { MotorType.kBrushless ); - armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + armMotor.configure( + ArmConstants.motorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); positionController = new PIDController( ArmConstants.kPositionalP, diff --git a/src/main/java/frc/robot/subsystems/ClimberPivot.java b/src/main/java/frc/robot/subsystems/ClimberPivot.java index f57a5b7..636f18a 100644 --- a/src/main/java/frc/robot/subsystems/ClimberPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimberPivot.java @@ -2,6 +2,8 @@ package frc.robot.subsystems; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; @@ -22,6 +24,12 @@ public class ClimberPivot extends SubsystemBase { MotorType.kBrushless ); + pivotMotor.configure( + ClimberPivotConstants.motorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + neoEncoder = pivotMotor.getEncoder(); cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID); diff --git a/src/main/java/frc/robot/subsystems/ClimberRollers.java b/src/main/java/frc/robot/subsystems/ClimberRollers.java index 5ecdba6..3a24b99 100644 --- a/src/main/java/frc/robot/subsystems/ClimberRollers.java +++ b/src/main/java/frc/robot/subsystems/ClimberRollers.java @@ -1,12 +1,15 @@ package frc.robot.subsystems; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ClimberRollersConstants; +//TODO Figure out a way to detect if we're at the top of the cage public class ClimberRollers extends SubsystemBase { private SparkMax rollerMotor; @@ -15,6 +18,12 @@ public class ClimberRollers extends SubsystemBase { ClimberRollersConstants.kRollerMotorID, MotorType.kBrushless ); + + rollerMotor.configure( + ClimberRollersConstants.motorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); } /** diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index 7916cb9..9217965 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -3,6 +3,8 @@ package frc.robot.subsystems; import java.util.function.DoubleSupplier; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; @@ -22,6 +24,12 @@ public class Manipulator extends SubsystemBase { MotorType.kBrushless ); + manipulatorMotor.configure( + ManipulatorConstants.motorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + coralBeamBreak = new DigitalInput(ManipulatorConstants.kCoralBeamBreakID); algaeBeamBreak = new DigitalInput(ManipulatorConstants.kAlgaeBeamBreakID); }