From edcab41c2e6543c1c353e5d4719dc9aca4bd7025 Mon Sep 17 00:00:00 2001 From: VincentShao32 Date: Mon, 29 Jan 2024 17:51:29 -0800 Subject: [PATCH] documentation updates --- .../java/frc/robot/commands/SetPivot.java | 2 +- .../java/frc/robot/commands/SmartIntake.java | 2 -- .../frc/robot/subsystems/IntakeShooter.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 25 +++++++++++++++++-- 4 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/SetPivot.java b/src/main/java/frc/robot/commands/SetPivot.java index 084b14a..a109fdc 100644 --- a/src/main/java/frc/robot/commands/SetPivot.java +++ b/src/main/java/frc/robot/commands/SetPivot.java @@ -33,7 +33,7 @@ public void execute() { s_Pivot.setVoltage(voltage); } - @Override + @Override public boolean isFinished() { return Math.abs(s_Pivot.getSetPoint() - s_Pivot.getCANcoderPosition()) < 45; } diff --git a/src/main/java/frc/robot/commands/SmartIntake.java b/src/main/java/frc/robot/commands/SmartIntake.java index 723aa85..4763456 100644 --- a/src/main/java/frc/robot/commands/SmartIntake.java +++ b/src/main/java/frc/robot/commands/SmartIntake.java @@ -6,12 +6,10 @@ public class SmartIntake extends Command{ private final IntakeShooter s_IntakeShooter; - private boolean finished; public SmartIntake(){ s_IntakeShooter = IntakeShooter.getInstance(); addRequirements(s_IntakeShooter); - finished = false; } @Override diff --git a/src/main/java/frc/robot/subsystems/IntakeShooter.java b/src/main/java/frc/robot/subsystems/IntakeShooter.java index c166205..4e973d5 100644 --- a/src/main/java/frc/robot/subsystems/IntakeShooter.java +++ b/src/main/java/frc/robot/subsystems/IntakeShooter.java @@ -100,7 +100,7 @@ public void configFollowerMotor(){ // mLeaderShooter.getPIDController().setI(Constants.SwerveConstants.driveKI); // mLeaderShooter.getPIDController().setD(Constants.SwerveConstants.driveKD); } - + public boolean hasNote(){ return m_intakeSensor.getProximity() >= Constants.noteIntakeDistance; } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 21a990d..cad15b4 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -43,22 +43,40 @@ public Pivot() { configCANcoder(); } + /** + * Sets the desired state for the pivot + * @param state Desired state + */ public void setState(PivotState state) { curState = state; } + /** + * Gets the current position measured by the CANcoder + * @return Current position measured by CANcoder + */ public double getCANcoderPosition() { return pivotCANcoder.getPosition().getValueAsDouble(); } - + /** + * Gets the current set point of the pivot. + * @return Current set point in CANcoder values. + */ public double getSetPoint() { return curState.pos; } + /** + * Sets the voltage of the pivot motor + * @param Desired voltage. + */ public void setVoltage(double voltage) { mPivotMotor.setVoltage(voltage); } + /** + * Configures the pivot motor with current limit, idle mode, and PID values + */ private void configMotor() { mPivotMotor.setSmartCurrentLimit(Constants.pivotPeakCurrentLimit); mPivotMotor.setIdleMode(IdleMode.kBrake); @@ -67,7 +85,10 @@ private void configMotor() { mPivotMotor.getPIDController().setI(Constants.hardwarePIDs.pivotkI); mPivotMotor.getPIDController().setD(Constants.hardwarePIDs.pivotkD); } - + + /** + * Configures the pivot CANcoder with sensor direction and absolute range. + */ private void configCANcoder(){ /* Swerve CANCoder Configuration */ // CANcoder is always initialized to absolute position on boot in Phoenix 6 - https://www.chiefdelphi.com/t/what-kind-of-encoders-are-built-into-the-kraken-motors/447253/7