From be97c25a66a0575b55ebaad9f93a963944fd95d0 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Thu, 15 Feb 2024 22:22:11 -0500 Subject: [PATCH 1/2] singleton robotContainer --- src/main/java/frc/robot/Robot.java | 13 ++++++----- src/main/java/frc/robot/RobotContainer.java | 22 ++++++++++++++++++- src/main/java/frc/robot/climber/Climber.java | 10 +++------ .../java/frc/robot/climber/ClimberSide.java | 11 ++++------ 4 files changed, 36 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cd681a3..c44e370 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,6 +2,8 @@ package frc.robot; +import static frc.robot.RobotContainer.getRobotContainer; + import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; @@ -11,13 +13,14 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; - private RobotContainer m_robotContainer; + + // private RobotContainer m_robotContainer; @Override public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); + getRobotContainer(); // Starts recording to data log // https://docs.wpilib.org/en/stable/docs/software/telemetry/datalog.html#logging-joystick-data @@ -28,7 +31,7 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - m_robotContainer.periodic(); + getRobotContainer().periodic(); } @Override @@ -39,7 +42,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + m_autonomousCommand = getRobotContainer().getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); @@ -55,7 +58,7 @@ public void teleopInit() { m_autonomousCommand.cancel(); } - m_robotContainer.teleopInit(); + getRobotContainer().teleopInit(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 702149b..5b341df 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,12 +31,21 @@ public class RobotContainer { + private static RobotContainer INSTANCE = null; + + public static RobotContainer getRobotContainer() { + if (INSTANCE == null) { + INSTANCE = new RobotContainer(); + } + return INSTANCE; + } + private final PowerDistribution m_PowerDistribution = new PowerDistribution(1, ModuleType.kRev); private final Drivetrain m_swerve = new Drivetrain(); private final Arm m_arm = new Arm(); private final Intake m_intake = new Intake(); - private final Climber m_climber = new Climber(m_PowerDistribution); + private final Climber m_climber = new Climber(); private final EventLoop m_loop = new EventLoop(); private Joystick m_driver = new Joystick(OIConstants.kDriverControllerPort); @@ -218,4 +227,15 @@ private int getSelectedAutonomousMode() { } return -1; // failure of the physical switch } + + /** + * Gets the current drawn from the Power Distribution Hub by a CAN motor controller, assuming that + * (PDH port number + 10) = CAN ID + * + * @param CANBusPort The CAN ID of the motor controller + * @return Current in Amps on the PDH channel corresponding to the motor channel + */ + public double getPDHCurrent(int CANBusPort) { + return m_PowerDistribution.getCurrent(CANBusPort - 10); + } } diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index cd0764f..33cb79b 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -1,6 +1,5 @@ package frc.robot.climber; -import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.Command; @@ -11,15 +10,12 @@ public class Climber extends SubsystemBase { private ClimberSide[] m_actuators = new ClimberSide[2]; - private final PowerDistribution m_pdp; - DifferentialDrive m_differentialDrive; - public Climber(PowerDistribution pdp) { + public Climber() { - this.m_pdp = pdp; - m_actuators[0] = new ClimberSide("Left", ClimberConstants.kLeftMotorPort, m_pdp); - m_actuators[1] = new ClimberSide("Right", ClimberConstants.kRightMotorPort, m_pdp); + m_actuators[0] = new ClimberSide("Left", ClimberConstants.kLeftMotorPort); + m_actuators[1] = new ClimberSide("Right", ClimberConstants.kRightMotorPort); m_differentialDrive = new DifferentialDrive(m_actuators[0]::setPower, m_actuators[1]::setPower); } diff --git a/src/main/java/frc/robot/climber/ClimberSide.java b/src/main/java/frc/robot/climber/ClimberSide.java index f971303..9875801 100644 --- a/src/main/java/frc/robot/climber/ClimberSide.java +++ b/src/main/java/frc/robot/climber/ClimberSide.java @@ -1,5 +1,7 @@ package frc.robot.climber; +import static frc.robot.RobotContainer.getRobotContainer; + import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -10,7 +12,6 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.ClimberConstants.CalibrationState; @@ -36,15 +37,11 @@ public class ClimberSide { private CalibrationState m_calibrationState = CalibrationState.UNCALIBRATED; - private final PowerDistribution m_pdp; - - public ClimberSide(String climberName, int climberMotorChannel, PowerDistribution pdp) { + public ClimberSide(String climberName, int climberMotorChannel) { this.climberName = climberName; this.motorChannel = climberMotorChannel; - this.m_pdp = pdp; - m_climberMover = new CANSparkMax(motorChannel, MotorType.kBrushless); m_climberMover.restoreFactoryDefaults(); @@ -150,7 +147,7 @@ private double getSparkMaxCurrent() { } private double getPdhCurrent() { - return m_pdp.getCurrent(motorChannel - 10); + return getRobotContainer().getPDHCurrent(motorChannel); } /** From 2a14e9227262b7fd5de05215ff556af3d540e9ba Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Thu, 15 Feb 2024 22:25:57 -0500 Subject: [PATCH 2/2] method naming --- src/main/java/frc/robot/Robot.java | 2 -- src/main/java/frc/robot/climber/ClimberSide.java | 15 +++++++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c44e370..cbb96a1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,8 +14,6 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; - // private RobotContainer m_robotContainer; - @Override public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our diff --git a/src/main/java/frc/robot/climber/ClimberSide.java b/src/main/java/frc/robot/climber/ClimberSide.java index 9875801..80a9dc3 100644 --- a/src/main/java/frc/robot/climber/ClimberSide.java +++ b/src/main/java/frc/robot/climber/ClimberSide.java @@ -99,7 +99,7 @@ public void resetEncoder() { } public boolean getCurrentSenseState() { - return m_debouncer.calculate(getSparkMaxCurrent() > ClimberConstants.kMotorCurrentHardStop); + return m_debouncer.calculate(getOutputCurrent() > ClimberConstants.kMotorCurrentHardStop); } private boolean getUpperSoftLimitSwtichState() { @@ -140,13 +140,16 @@ private double getPosition() { } /** - * @return Current in Amps, output of linear filter + * @return Current in Amps, output of motor controller */ - private double getSparkMaxCurrent() { + private double getOutputCurrent() { return m_filter.calculate(m_climberMover.getOutputCurrent()); } - private double getPdhCurrent() { + /** + * @return Current in Amps, input to motor controller + */ + private double getInputCurrent() { return getRobotContainer().getPDHCurrent(motorChannel); } @@ -163,8 +166,8 @@ public void periodic() { getName() + "MotorRotations", getPosition() / ClimberConstants.kPositionConversionFactor); SmartDashboard.putBoolean(getName() + "UpperSoftLimitState", getUpperSoftLimitSwtichState()); SmartDashboard.putBoolean(getName() + "LowerSoftLimitState", getLowerSoftLimitSwtichState()); - SmartDashboard.putNumber(getName() + "SparkMaxCurrent", addPolarity(getSparkMaxCurrent())); - SmartDashboard.putNumber(getName() + "PDHCurrent", addPolarity(getPdhCurrent())); + SmartDashboard.putNumber(getName() + "OutputCurrent", addPolarity(getOutputCurrent())); + SmartDashboard.putNumber(getName() + "InputCurrent", addPolarity(getInputCurrent())); SmartDashboard.putString(getName() + "CalibrationState", getCalibrationState().name()); }