From 9e2cf6769d7b296577a37cb9d890cad5758bb309 Mon Sep 17 00:00:00 2001 From: arnav Date: Mon, 10 Feb 2025 17:18:12 -0500 Subject: [PATCH 01/12] Initializing Climber Class --- .../java/frc/robot/subsystems/climber/Climber.java | 9 +++++++++ .../java/frc/robot/subsystems/climber/ClimberIO.java | 10 ++++++++++ 2 files changed, 19 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/climber/Climber.java create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIO.java diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..d72e493 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,9 @@ +package frc.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.littletonrobotics.junction.Logger; + +public class Climber extends SubsystemBase{ + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..ced3c0b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public static class ClimberIOInputs {} + + +} From 2263755bd440693e7f358480761d68cbce3999d1 Mon Sep 17 00:00:00 2001 From: arnav Date: Mon, 24 Feb 2025 17:19:36 -0500 Subject: [PATCH 02/12] Initializing Climber --- src/main/java/frc/robot/subsystems/climber/ClimberIO.java | 5 ++++- .../java/frc/robot/subsystems/climber/ClimberIOSpark.java | 5 +++++ 2 files changed, 9 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index ced3c0b..0905d09 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -2,9 +2,12 @@ import org.littletonrobotics.junction.AutoLog; -public interface ClimberIO { +public interface ClimberIO{ @AutoLog public static class ClimberIOInputs {} + + public default void updateInputs(ClimberIOInputs inputs) {} + public default void updateClimberPosition() {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java new file mode 100644 index 0000000..dfbe8e5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.climber; + +public class ClimberIOSpark implements ClimberIO { + +} From 900a4c1ec378b962013e07acc2ea04ac5897a59a Mon Sep 17 00:00:00 2001 From: arnav Date: Mon, 24 Feb 2025 17:57:32 -0500 Subject: [PATCH 03/12] Added outline of Climber --- .../frc/robot/subsystems/climber/Climber.java | 22 ++++++++++++++++--- .../robot/subsystems/climber/ClimberIO.java | 17 +++++++++++--- .../subsystems/climber/ClimberPosition.java | 22 +++++++++++++++++++ 3 files changed, 55 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberPosition.java diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index d72e493..930b966 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,9 +1,25 @@ package frc.robot.subsystems.climber; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; - public class Climber extends SubsystemBase{ - + private ClimberIO climberIO; + + public Climber(ClimberIO climberIO) { + this.climberIO = climberIO; + } + + public Command toggleClimber(ClimberPosition climberPosition) { + return runOnce(null); + } + + public Command runVolts(double volts) { + return runOnce(() -> climberIO.setVolts(volts)); + } + + @Override + public void periodic() { + + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 0905d09..c1430e0 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -2,12 +2,23 @@ import org.littletonrobotics.junction.AutoLog; -public interface ClimberIO{ +public interface ClimberIO { @AutoLog - public static class ClimberIOInputs {} + public static class ClimberIOInputs { + public ClimberPosition desiredPosition = ClimberPosition.HOME; + + public double climberHeight = 0; + public double climberVelocity = 0; + + public double elevatorAppliedVolts = 0; + public double elevatorCurrentAmps = 0; + } public default void updateInputs(ClimberIOInputs inputs) {} - public default void updateClimberPosition() {} + public default void setClimberPosition(ClimberPosition climberPosition) {} + public default void setVolts(double volts) {} + + public default void updateControlConstants() {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberPosition.java b/src/main/java/frc/robot/subsystems/climber/ClimberPosition.java new file mode 100644 index 0000000..2596fef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberPosition.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.climber; + +public enum ClimberPosition { + HOME("Home", 0.0), + L1("L1", 0.0), + L2("L2", 0.0), + L3("L3", 0.0), + L4("L4", 0.0); + + public final String name; + public final double height; + + private ClimberPosition(String name, double height) { + this.name = name; + this.height = height; + } + + @Override + public String toString() { + return name; + } +} From 317a42baacb3a3860198155ddea20f3d6c8ef031 Mon Sep 17 00:00:00 2001 From: arnav Date: Wed, 26 Feb 2025 16:51:04 -0500 Subject: [PATCH 04/12] programmed climber --- src/main/java/frc/robot/Constants.java | 10 +++++-- src/main/java/frc/robot/RobotContainer.java | 8 ++++++ .../frc/robot/subsystems/climber/Climber.java | 13 ++++----- .../robot/subsystems/climber/ClimberIO.java | 6 ++-- .../subsystems/climber/ClimberIOSpark.java | 28 +++++++++++++++++++ 5 files changed, 54 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2193af5..cb7136e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -282,14 +282,20 @@ public static class ManipulatorConstants { // CAN ID range 21-24 public static class ClimberConstants { // CAN ID range 25-29 public static final int kClimberMotorCANID = 25; + public static final double kEncoderConversionFactor = 2*Math.PI; + public static final double climberLowVolts = 1; // Temp + public static final double climberHighVolts = 5; // Temp + public static final SparkMaxConfig climberConfig = new SparkMaxConfig(); static { climberConfig .idleMode(IdleMode.kBrake) .smartCurrentLimit(30) - .voltageCompensation(12) - ; + .voltageCompensation(12); + climberConfig.absoluteEncoder + .positionConversionFactor(kEncoderConversionFactor) + .velocityConversionFactor(kEncoderConversionFactor); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5f932c9..edb9a2b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.OperatorConstants; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberIOSpark; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorIO; import frc.robot.subsystems.elevator.ElevatorIOSpark; @@ -26,11 +28,15 @@ public class RobotContainer { private SwerveDrive swerve; private Elevator elevator; + private Climber climber; + public RobotContainer() { Preferences.removeAll(); driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); auxController = new CommandXboxController(OperatorConstants.kAuxControllerPort); + climber = new Climber(new ClimberIOSpark()); + switch(Constants.currentMode) { case REAL: case SIM: // no sim classes for now @@ -72,6 +78,8 @@ private void configureBindings() { driverController.x().onTrue(swerve.runOpenTestDrive()); driverController.x().onFalse(swerve.runStopDrive()); driverController.b().onTrue(swerve.runUpdateControlConstants()); + + climber.setDefaultCommand(climber.runClimber(auxController.a()::getAsBoolean, auxController.b()::getAsBoolean)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 930b966..9d8ce20 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,23 +1,22 @@ package frc.robot.subsystems.climber; +import java.util.function.BooleanSupplier; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase{ + private ClimberIO climberIO; public Climber(ClimberIO climberIO) { this.climberIO = climberIO; } - public Command toggleClimber(ClimberPosition climberPosition) { - return runOnce(null); + public Command runClimber(BooleanSupplier low, BooleanSupplier high) { + return runOnce(() -> climberIO.setClimberVolts(low.getAsBoolean(), high.getAsBoolean())); } - - public Command runVolts(double volts) { - return runOnce(() -> climberIO.setVolts(volts)); - } - + @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index c1430e0..3ab1384 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -18,7 +18,9 @@ public default void updateInputs(ClimberIOInputs inputs) {} public default void setClimberPosition(ClimberPosition climberPosition) {} - public default void setVolts(double volts) {} - + public default void setClimberVolts(boolean low, boolean high) {} + public default void updateControlConstants() {} + + public default void setToggle(boolean toggle) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index dfbe8e5..806f6b6 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -1,5 +1,33 @@ package frc.robot.subsystems.climber; +import frc.robot.Constants.ClimberConstants; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; + + public class ClimberIOSpark implements ClimberIO { + private SparkMax climberMotor; + public ClimberIOSpark() { + climberMotor = new SparkMax(ClimberConstants.kClimberMotorCANID, MotorType.kBrushless); + climberMotor.setCANTimeout(0); + + climberMotor.configure(ClimberConstants.climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void setClimberVolts(boolean low, boolean high) { + if(low) { + climberMotor.setVoltage(ClimberConstants.climberLowVolts); + } + else if(high) { + climberMotor.setVoltage(ClimberConstants.climberHighVolts); + } + else{ + climberMotor.setVoltage(0); + } + } + } From 1d1fffa2608e6dcc63f980b635d74ee86a850713 Mon Sep 17 00:00:00 2001 From: arnav Date: Wed, 26 Feb 2025 16:54:11 -0500 Subject: [PATCH 05/12] cleaning up climber --- src/main/java/frc/robot/Constants.java | 7 +++--- .../robot/subsystems/climber/ClimberIO.java | 20 ++++++++--------- .../subsystems/climber/ClimberIOSpark.java | 3 ++- .../subsystems/climber/ClimberPosition.java | 22 ------------------- 4 files changed, 15 insertions(+), 37 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberPosition.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cb7136e..acea806 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -282,7 +282,6 @@ public static class ManipulatorConstants { // CAN ID range 21-24 public static class ClimberConstants { // CAN ID range 25-29 public static final int kClimberMotorCANID = 25; - public static final double kEncoderConversionFactor = 2*Math.PI; public static final double climberLowVolts = 1; // Temp public static final double climberHighVolts = 5; // Temp @@ -293,9 +292,9 @@ public static class ClimberConstants { // CAN ID range 25-29 .idleMode(IdleMode.kBrake) .smartCurrentLimit(30) .voltageCompensation(12); - climberConfig.absoluteEncoder - .positionConversionFactor(kEncoderConversionFactor) - .velocityConversionFactor(kEncoderConversionFactor); + // climberConfig.absoluteEncoder + // .positionConversionFactor(kEncoderConversionFactor) + // .velocityConversionFactor(kEncoderConversionFactor); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 3ab1384..260e575 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -5,22 +5,22 @@ public interface ClimberIO { @AutoLog public static class ClimberIOInputs { - public ClimberPosition desiredPosition = ClimberPosition.HOME; + // public ClimberPosition desiredPosition = ClimberPosition.HOME; - public double climberHeight = 0; - public double climberVelocity = 0; + // public double climberHeight = 0; + // public double climberVelocity = 0; - public double elevatorAppliedVolts = 0; - public double elevatorCurrentAmps = 0; + // public double elevatorAppliedVolts = 0; + // public double elevatorCurrentAmps = 0; } - public default void updateInputs(ClimberIOInputs inputs) {} + // public default void updateInputs(ClimberIOInputs inputs) {} - public default void setClimberPosition(ClimberPosition climberPosition) {} + // public default void setClimberPosition(ClimberPosition climberPosition) {} public default void setClimberVolts(boolean low, boolean high) {} - - public default void updateControlConstants() {} - public default void setToggle(boolean toggle) {} + // public default void updateControlConstants() {} + + // public default void setToggle(boolean toggle) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index 806f6b6..059136e 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -10,7 +10,7 @@ public class ClimberIOSpark implements ClimberIO { private SparkMax climberMotor; - + public ClimberIOSpark() { climberMotor = new SparkMax(ClimberConstants.kClimberMotorCANID, MotorType.kBrushless); climberMotor.setCANTimeout(0); @@ -18,6 +18,7 @@ public ClimberIOSpark() { climberMotor.configure(ClimberConstants.climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } + @Override public void setClimberVolts(boolean low, boolean high) { if(low) { climberMotor.setVoltage(ClimberConstants.climberLowVolts); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberPosition.java b/src/main/java/frc/robot/subsystems/climber/ClimberPosition.java deleted file mode 100644 index 2596fef..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberPosition.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.subsystems.climber; - -public enum ClimberPosition { - HOME("Home", 0.0), - L1("L1", 0.0), - L2("L2", 0.0), - L3("L3", 0.0), - L4("L4", 0.0); - - public final String name; - public final double height; - - private ClimberPosition(String name, double height) { - this.name = name; - this.height = height; - } - - @Override - public String toString() { - return name; - } -} From 16cd12f2c5b9ae8508bde22b47b0d262543baa33 Mon Sep 17 00:00:00 2001 From: 178Laptop Date: Fri, 28 Feb 2025 18:43:09 -0500 Subject: [PATCH 06/12] use pid in spark --- src/main/java/frc/robot/Constants.java | 43 ++++++++-- .../robot/subsystems/climber/ClimberIO.java | 4 + .../subsystems/climber/ClimberIOSpark.java | 79 ++++++++++++++++--- 3 files changed, 110 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index acea806..d5b8c9d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,6 +1,9 @@ package frc.robot; import com.revrobotics.spark.config.SparkMaxConfig; + +import static edu.wpi.first.units.Units.Kilo; + import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; @@ -282,19 +285,44 @@ public static class ManipulatorConstants { // CAN ID range 21-24 public static class ClimberConstants { // CAN ID range 25-29 public static final int kClimberMotorCANID = 25; - public static final double climberLowVolts = 1; // Temp - public static final double climberHighVolts = 5; // Temp + // public static final double climberLowVolts = 1; // Temp + // public static final double climberHighVolts = 5; // Temp + + public static final double upVolts = 0; + public static final double downVolts = 0; + + public static final SparkMaxConfig climberSetConfig = new SparkMaxConfig(); + + public static final double kEncoderConversionFactor = 1 * 2 * Math.PI; - public static final SparkMaxConfig climberConfig = new SparkMaxConfig(); + public static final double kSetP = 0; + public static final double kSetI = 0; + public static final double kSetD = 0; + + public static final double kSetG = 0; + public static final double kClimbG = 0; + public static final double kHeheG = 0; + + public static final double verticalEncoderValue = 0; + public static final double setEncoderValue = 0; + + public static final double maxDownVel = 0; + public static final double maxUpVel = 0; static { - climberConfig + climberSetConfig .idleMode(IdleMode.kBrake) .smartCurrentLimit(30) .voltageCompensation(12); - // climberConfig.absoluteEncoder - // .positionConversionFactor(kEncoderConversionFactor) - // .velocityConversionFactor(kEncoderConversionFactor); + climberSetConfig.encoder + .positionConversionFactor(kEncoderConversionFactor); + climberSetConfig.closedLoop + .feedbackSensor((FeedbackSensor.kPrimaryEncoder)) + .p(kSetP) + .i(kSetP) + .d(kSetD) + .outputRange(-1, 1); + } } @@ -302,4 +330,5 @@ public static class OperatorConstants { public static final int kDriverControllerPort = 0; public static final int kAuxControllerPort = 1; } + } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 260e575..a6129cd 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -23,4 +23,8 @@ public default void setClimberVolts(boolean low, boolean high) {} // public default void updateControlConstants() {} // public default void setToggle(boolean toggle) {} + + public default void climberInputs(boolean set, boolean up, boolean down){} + + public default void updateKG() {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index 059136e..d06bc26 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -2,32 +2,93 @@ import frc.robot.Constants.ClimberConstants; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkRelativeEncoder; + +import static edu.wpi.first.units.Units.Volts; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; import com.revrobotics.spark.SparkLowLevel.MotorType; public class ClimberIOSpark implements ClimberIO { private SparkMax climberMotor; + private RelativeEncoder encoder; + private SparkClosedLoopController pid; + // private SparkClosedLoopController motorPID; + + private boolean prevSet; + private boolean prevUp; + private boolean prevDown; + private boolean isSet; + + private double kG; public ClimberIOSpark() { climberMotor = new SparkMax(ClimberConstants.kClimberMotorCANID, MotorType.kBrushless); climberMotor.setCANTimeout(0); + encoder = climberMotor.getEncoder(); + pid = climberMotor.getClosedLoopController(); + climberMotor.configure(ClimberConstants.climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + pid.setReference(ClimberConstants.setEncoderValue, ControlType.kPosition); - climberMotor.configure(ClimberConstants.climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + prevSet = true; + prevUp = false; + prevDown = false; + isSet = true; + + kG = ClimberConstants.kSetG; } + // @Override + // public void setClimberVolts(boolean low, boolean high) { + // if(low) { + // climberMotor.setVoltage(ClimberConstants.climberLowVolts); + // } + // else if(high) { + // climberMotor.setVoltage(ClimberConstants.climberHighVolts); + // } + // else{ + // climberMotor.setVoltage(0); + // } + // } + + @Override + public void climberInputs(boolean set, boolean up, boolean down) { + if(set && !prevSet && !up && !down) { + pid.setReference(ClimberConstants.setEncoderValue, ControlType.kPosition); + isSet = true; + } + if(up && !prevUp) { + climberMotor.setVoltage(ClimberConstants.upVolts); + } + if((!up && prevUp && down) || (down && !prevDown && !up)) { + climberMotor.setVoltage(ClimberConstants.downVolts); + } + if(!up && !down && !isSet) { + pid.setReference(encoder.getPosition(), ControlType.kPosition, ClosedLoopSlot.kSlot0, kG, ArbFFUnits.kVoltage); + } + prevSet = set; + prevUp = up; + prevDown = down; + } + @Override - public void setClimberVolts(boolean low, boolean high) { - if(low) { - climberMotor.setVoltage(ClimberConstants.climberLowVolts); - } - else if(high) { - climberMotor.setVoltage(ClimberConstants.climberHighVolts); + public void updateKG() { + if(encoder.getPosition() > ClimberConstants.verticalEncoderValue) { + kG = ClimberConstants.kSetG; + } + else if(encoder.getVelocity() > ClimberConstants.maxUpVel) { + kG = ClimberConstants.kClimbG; } - else{ - climberMotor.setVoltage(0); + else if(encoder.getVelocity() < ClimberConstants.maxDownVel) { + kG = ClimberConstants.kHeheG; } } From 17e241e4fa104a3905860cc015f27b88b8cd627e Mon Sep 17 00:00:00 2001 From: 178Laptop Date: Sat, 1 Mar 2025 11:16:50 -0500 Subject: [PATCH 07/12] turkey --- src/main/java/frc/robot/Constants.java | 38 +++++++--- src/main/java/frc/robot/RobotContainer.java | 7 +- .../frc/robot/subsystems/climber/Climber.java | 19 +++-- .../robot/subsystems/climber/ClimberIO.java | 4 +- .../subsystems/climber/ClimberIOSpark.java | 76 +++++++++++++++---- 5 files changed, 111 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d5b8c9d..fb325fc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -283,6 +283,23 @@ public static class ManipulatorConstants { // CAN ID range 21-24 } public static class ClimberConstants { // CAN ID range 25-29 + + /* + * To Find: + * initialEncoderValue + * setEncoderValue + * minPos + * maxPos + * kSetG + * kSetP + * kLightG + * downVolts + * upVolts + * kClimbG + * minVel + * maxVel + */ + public static final int kClimberMotorCANID = 25; // public static final double climberLowVolts = 1; // Temp @@ -293,7 +310,7 @@ public static class ClimberConstants { // CAN ID range 25-29 public static final SparkMaxConfig climberSetConfig = new SparkMaxConfig(); - public static final double kEncoderConversionFactor = 1 * 2 * Math.PI; + // public static final double kEncoderConversionFactor = 1 * 2 * Math.PI; public static final double kSetP = 0; public static final double kSetI = 0; @@ -301,28 +318,29 @@ public static class ClimberConstants { // CAN ID range 25-29 public static final double kSetG = 0; public static final double kClimbG = 0; - public static final double kHeheG = 0; + public static final double kLightG = 0; - public static final double verticalEncoderValue = 0; public static final double setEncoderValue = 0; + public static final double initialEncoderValue = 0; - public static final double maxDownVel = 0; - public static final double maxUpVel = 0; + public static final double minPos = 0; + public static final double maxPos = 0; + + public static final double maxVel = 0; + public static final double minVel = 0; static { climberSetConfig .idleMode(IdleMode.kBrake) .smartCurrentLimit(30) .voltageCompensation(12); - climberSetConfig.encoder - .positionConversionFactor(kEncoderConversionFactor); + // climberSetConfig.encoder + // .positionConversionFactor(kEncoderConversionFactor); climberSetConfig.closedLoop .feedbackSensor((FeedbackSensor.kPrimaryEncoder)) .p(kSetP) .i(kSetP) - .d(kSetD) - .outputRange(-1, 1); - + .d(kSetD); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index edb9a2b..a9145e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,12 @@ private void configureBindings() { driverController.x().onFalse(swerve.runStopDrive()); driverController.b().onTrue(swerve.runUpdateControlConstants()); - climber.setDefaultCommand(climber.runClimber(auxController.a()::getAsBoolean, auxController.b()::getAsBoolean)); + climber.setDefaultCommand(climber.runClimber( + auxController.b()::getAsBoolean, + auxController.x()::getAsBoolean, + auxController.y()::getAsBoolean, + auxController.a()::getAsBoolean + )); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 9d8ce20..ddbfd70 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,24 +1,33 @@ package frc.robot.subsystems.climber; import java.util.function.BooleanSupplier; +import java.util.prefs.Preferences; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase{ - private ClimberIO climberIO; + private ClimberIO climber; public Climber(ClimberIO climberIO) { - this.climberIO = climberIO; + this.climber = climberIO; } - public Command runClimber(BooleanSupplier low, BooleanSupplier high) { - return runOnce(() -> climberIO.setClimberVolts(low.getAsBoolean(), high.getAsBoolean())); + public Command runClimber(BooleanSupplier set, BooleanSupplier up, BooleanSupplier down, BooleanSupplier key) { + // return runOnce(() -> climberIO.setClimberVolts(low.getAsBoolean(), high.getAsBoolean())); + boolean bKey = key.getAsBoolean(); + boolean bSet = set.getAsBoolean() && bKey; + boolean bUp = up.getAsBoolean() && bKey; + boolean bDown = down.getAsBoolean() && bKey; + + return run(() -> climber.climberInputs(bSet, bUp, bDown)); } @Override public void periodic() { - + climber.update(); + SmartDashboard.putNumber("encoder position", climber.getPos()); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index a6129cd..8045858 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -26,5 +26,7 @@ public default void setClimberVolts(boolean low, boolean high) {} public default void climberInputs(boolean set, boolean up, boolean down){} - public default void updateKG() {} + public default void update() {} + + public default double getPos() {return 0;} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index d06bc26..5ef95ad 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -26,9 +26,12 @@ public class ClimberIOSpark implements ClimberIO { private boolean prevSet; private boolean prevUp; private boolean prevDown; - private boolean isSet; + + private boolean up; + private boolean down; private double kG; + private boolean isLocked; public ClimberIOSpark() { climberMotor = new SparkMax(ClimberConstants.kClimberMotorCANID, MotorType.kBrushless); @@ -36,14 +39,18 @@ public ClimberIOSpark() { encoder = climberMotor.getEncoder(); pid = climberMotor.getClosedLoopController(); climberMotor.configure(ClimberConstants.climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - pid.setReference(ClimberConstants.setEncoderValue, ControlType.kPosition); + pid.setReference(ClimberConstants.initialEncoderValue, ControlType.kPosition, ClosedLoopSlot.kSlot0, ClimberConstants.kSetG, ArbFFUnits.kVoltage); prevSet = true; prevUp = false; prevDown = false; - isSet = true; - kG = ClimberConstants.kSetG; + up = false; + down = false; + + kG = ClimberConstants.kClimbG; + isLocked = true; + } // @Override @@ -62,17 +69,32 @@ public ClimberIOSpark() { @Override public void climberInputs(boolean set, boolean up, boolean down) { if(set && !prevSet && !up && !down) { - pid.setReference(ClimberConstants.setEncoderValue, ControlType.kPosition); - isSet = true; + isLocked = false; + pid.setReference( + ClimberConstants.setEncoderValue, + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + ClimberConstants.kSetG, + ArbFFUnits.kVoltage + ); } - if(up && !prevUp) { + if(up && !this.up) { + isLocked = false; climberMotor.setVoltage(ClimberConstants.upVolts); } - if((!up && prevUp && down) || (down && !prevDown && !up)) { + if(!up && down && !this.down) { + isLocked = false; climberMotor.setVoltage(ClimberConstants.downVolts); } - if(!up && !down && !isSet) { - pid.setReference(encoder.getPosition(), ControlType.kPosition, ClosedLoopSlot.kSlot0, kG, ArbFFUnits.kVoltage); + if(!this.up && ((!up && prevUp) || (!up && !down && prevDown))) { + isLocked = true; + pid.setReference( + encoder.getPosition(), + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + kG, + ArbFFUnits.kVoltage + ); } prevSet = set; prevUp = up; @@ -80,16 +102,38 @@ public void climberInputs(boolean set, boolean up, boolean down) { } @Override - public void updateKG() { - if(encoder.getPosition() > ClimberConstants.verticalEncoderValue) { - kG = ClimberConstants.kSetG; + public void update() { + if(encoder.getPosition() > ClimberConstants.maxPos) { + up = true; + pid.setReference( + encoder.getPosition(), + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + ClimberConstants.kSetG, + ArbFFUnits.kVoltage + ); + } + if(encoder.getPosition() < ClimberConstants.minPos) { + down = true; + pid.setReference( + encoder.getPosition(), + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + kG, + ArbFFUnits.kVoltage + ); } - else if(encoder.getVelocity() > ClimberConstants.maxUpVel) { + if(isLocked && (encoder.getVelocity() > ClimberConstants.maxVel)) { kG = ClimberConstants.kClimbG; } - else if(encoder.getVelocity() < ClimberConstants.maxDownVel) { - kG = ClimberConstants.kHeheG; + if(isLocked && (encoder.getVelocity() < ClimberConstants.minVel)) { + kG = ClimberConstants.kLightG; } } + @Override + public double getPos() { + return encoder.getPosition(); + } + } From de9fc0a1ef8eb5233c76eb0a1ecd8f649594dae4 Mon Sep 17 00:00:00 2001 From: arnav Date: Sat, 1 Mar 2025 11:23:39 -0500 Subject: [PATCH 08/12] climber-turkey --- src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index 5ef95ad..a27d356 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -135,5 +135,4 @@ public void update() { public double getPos() { return encoder.getPosition(); } - } From 37f218e6bd2ce07e63c59e6fa042f5003214a3cc Mon Sep 17 00:00:00 2001 From: arnav Date: Sat, 1 Mar 2025 11:31:59 -0500 Subject: [PATCH 09/12] cleaning up code --- src/main/java/frc/robot/Constants.java | 2 -- .../frc/robot/subsystems/climber/Climber.java | 1 - .../frc/robot/subsystems/climber/ClimberIO.java | 15 --------------- .../subsystems/climber/ClimberIOSpark.java | 17 ----------------- 4 files changed, 35 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fb325fc..fff74c5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,8 +2,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; -import static edu.wpi.first.units.Units.Kilo; - import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index ddbfd70..5d1bf00 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.climber; import java.util.function.BooleanSupplier; -import java.util.prefs.Preferences; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 8045858..8d5b6da 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -5,25 +5,10 @@ public interface ClimberIO { @AutoLog public static class ClimberIOInputs { - // public ClimberPosition desiredPosition = ClimberPosition.HOME; - - // public double climberHeight = 0; - // public double climberVelocity = 0; - - // public double elevatorAppliedVolts = 0; - // public double elevatorCurrentAmps = 0; } - // public default void updateInputs(ClimberIOInputs inputs) {} - - // public default void setClimberPosition(ClimberPosition climberPosition) {} - public default void setClimberVolts(boolean low, boolean high) {} - // public default void updateControlConstants() {} - - // public default void setToggle(boolean toggle) {} - public default void climberInputs(boolean set, boolean up, boolean down){} public default void update() {} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index a27d356..340c15d 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -5,9 +5,6 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkRelativeEncoder; - -import static edu.wpi.first.units.Units.Volts; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; @@ -21,7 +18,6 @@ public class ClimberIOSpark implements ClimberIO { private SparkMax climberMotor; private RelativeEncoder encoder; private SparkClosedLoopController pid; - // private SparkClosedLoopController motorPID; private boolean prevSet; private boolean prevUp; @@ -52,19 +48,6 @@ public ClimberIOSpark() { isLocked = true; } - - // @Override - // public void setClimberVolts(boolean low, boolean high) { - // if(low) { - // climberMotor.setVoltage(ClimberConstants.climberLowVolts); - // } - // else if(high) { - // climberMotor.setVoltage(ClimberConstants.climberHighVolts); - // } - // else{ - // climberMotor.setVoltage(0); - // } - // } @Override public void climberInputs(boolean set, boolean up, boolean down) { From d3021ce056681df7cf9ad5fb3d1a6af8a013b787 Mon Sep 17 00:00:00 2001 From: arnav Date: Sat, 1 Mar 2025 12:05:00 -0500 Subject: [PATCH 10/12] adding testing code --- src/main/java/frc/robot/RobotContainer.java | 16 +++++++++------- .../frc/robot/subsystems/climber/Climber.java | 12 ++++++++++++ .../frc/robot/subsystems/climber/ClimberIO.java | 2 ++ .../robot/subsystems/climber/ClimberIOSpark.java | 9 ++++++++- 4 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9145e3..90610e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,6 @@ package frc.robot; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -79,12 +78,15 @@ private void configureBindings() { driverController.x().onFalse(swerve.runStopDrive()); driverController.b().onTrue(swerve.runUpdateControlConstants()); - climber.setDefaultCommand(climber.runClimber( - auxController.b()::getAsBoolean, - auxController.x()::getAsBoolean, - auxController.y()::getAsBoolean, - auxController.a()::getAsBoolean - )); + // climber.setDefaultCommand(climber.runClimber( + // auxController.b()::getAsBoolean, + // auxController.a()::getAsBoolean + // )); + + auxController.a().onTrue(climber.runUp()); + auxController.b().onTrue(climber.runDown()); + auxController.a().onFalse(climber.stop()); + auxController.b().onFalse(climber.stop()); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 5d1bf00..fa46328 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -23,6 +23,18 @@ public Command runClimber(BooleanSupplier set, BooleanSupplier up, BooleanSuppli return run(() -> climber.climberInputs(bSet, bUp, bDown)); } + + public Command runUp() { + return runOnce( () -> climber.setVolts(0.1)); + } + + public Command runDown() { + return runOnce(() -> climber.setVolts(-0.1)); + } + + public Command stop() { + return runOnce(() -> climber.setVolts(0)); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 8d5b6da..fea6666 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -14,4 +14,6 @@ public default void climberInputs(boolean set, boolean up, boolean down){} public default void update() {} public default double getPos() {return 0;} + + public default void setVolts(double volts) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index 340c15d..c50c47b 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -46,7 +46,6 @@ public ClimberIOSpark() { kG = ClimberConstants.kClimbG; isLocked = true; - } @Override @@ -96,6 +95,7 @@ public void update() { ArbFFUnits.kVoltage ); } + if(encoder.getPosition() < ClimberConstants.minPos) { down = true; pid.setReference( @@ -106,14 +106,21 @@ public void update() { ArbFFUnits.kVoltage ); } + if(isLocked && (encoder.getVelocity() > ClimberConstants.maxVel)) { kG = ClimberConstants.kClimbG; } + if(isLocked && (encoder.getVelocity() < ClimberConstants.minVel)) { kG = ClimberConstants.kLightG; } } + @Override + public void setVolts(double volts) { + climberMotor.setVoltage(volts); + } + @Override public double getPos() { return encoder.getPosition(); From 9e5d8937f69c82c7d73e1a427b615de67db332da Mon Sep 17 00:00:00 2001 From: arnav Date: Sat, 1 Mar 2025 12:14:53 -0500 Subject: [PATCH 11/12] adding preferences --- src/main/java/frc/robot/Constants.java | 3 --- src/main/java/frc/robot/subsystems/climber/Climber.java | 8 +++++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fff74c5..7f6ea86 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -300,9 +300,6 @@ public static class ClimberConstants { // CAN ID range 25-29 public static final int kClimberMotorCANID = 25; - // public static final double climberLowVolts = 1; // Temp - // public static final double climberHighVolts = 5; // Temp - public static final double upVolts = 0; public static final double downVolts = 0; diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index fa46328..951d81a 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -2,6 +2,7 @@ import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -11,6 +12,7 @@ public class Climber extends SubsystemBase{ private ClimberIO climber; public Climber(ClimberIO climberIO) { + Preferences.initDouble("volts",0.1); this.climber = climberIO; } @@ -25,15 +27,15 @@ public Command runClimber(BooleanSupplier set, BooleanSupplier up, BooleanSuppli } public Command runUp() { - return runOnce( () -> climber.setVolts(0.1)); + return runOnce( () -> climber.setVolts(Preferences.getDouble("volts",0.1))); } public Command runDown() { - return runOnce(() -> climber.setVolts(-0.1)); + return runOnce(() -> climber.setVolts(-Preferences.getDouble("volts",0.1))); } public Command stop() { - return runOnce(() -> climber.setVolts(0)); + return runOnce(() -> climber.setVolts(Preferences.getDouble("volts", 0))); } @Override From a4bc26d2cb06bb24c6d3b6d95570531d89032c49 Mon Sep 17 00:00:00 2001 From: 178Laptop Date: Sat, 1 Mar 2025 13:42:40 -0500 Subject: [PATCH 12/12] switching branch --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 3 ++ .../frc/robot/subsystems/climber/Climber.java | 19 ++++++++-- .../robot/subsystems/climber/ClimberIO.java | 4 ++ .../subsystems/climber/ClimberIOSpark.java | 38 +++++++++++++++++-- 5 files changed, 58 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7f6ea86..8dbef29 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -297,7 +297,7 @@ public static class ClimberConstants { // CAN ID range 25-29 * minVel * maxVel */ - + public static final int kClimberMotorCANID = 25; public static final double upVolts = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 90610e4..e3e9158 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -87,6 +87,9 @@ private void configureBindings() { auxController.b().onTrue(climber.runDown()); auxController.a().onFalse(climber.stop()); auxController.b().onFalse(climber.stop()); + + auxController.x().onTrue(climber.gravity()); + auxController.x().onFalse(climber.stop()); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 951d81a..b787189 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -12,7 +12,9 @@ public class Climber extends SubsystemBase{ private ClimberIO climber; public Climber(ClimberIO climberIO) { - Preferences.initDouble("volts",0.1); + Preferences.initDouble("volts",0); + Preferences.initDouble("kG", 0); + Preferences.initDouble("P",0.1); this.climber = climberIO; } @@ -27,20 +29,29 @@ public Command runClimber(BooleanSupplier set, BooleanSupplier up, BooleanSuppli } public Command runUp() { - return runOnce( () -> climber.setVolts(Preferences.getDouble("volts",0.1))); + return runOnce( () -> climber.setVolts(Preferences.getDouble("volts",0))); } public Command runDown() { - return runOnce(() -> climber.setVolts(-Preferences.getDouble("volts",0.1))); + return runOnce(() -> climber.setVolts(-Preferences.getDouble("volts",0))); } public Command stop() { - return runOnce(() -> climber.setVolts(Preferences.getDouble("volts", 0))); + return runOnce(() -> climber.setVolts(0)); + } + + public Command gravity() { + return runOnce(() -> climber.setVolts(Preferences.getDouble("kG",0))); + } + + public Command pid() { + return runOnce(() -> climber.setReference(Preferences.getDouble("P", 0))); } @Override public void periodic() { climber.update(); SmartDashboard.putNumber("encoder position", climber.getPos()); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index fea6666..2e37e15 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -16,4 +16,8 @@ public default void update() {} public default double getPos() {return 0;} public default void setVolts(double volts) {} + + public default void setPos(double encoderValue, double kG) {} + + public default void setReference(double p) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index c50c47b..0d0af56 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -5,7 +5,9 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; - +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -29,13 +31,15 @@ public class ClimberIOSpark implements ClimberIO { private double kG; private boolean isLocked; + public SparkMaxConfig climberSetConfig; + public ClimberIOSpark() { climberMotor = new SparkMax(ClimberConstants.kClimberMotorCANID, MotorType.kBrushless); climberMotor.setCANTimeout(0); encoder = climberMotor.getEncoder(); pid = climberMotor.getClosedLoopController(); - climberMotor.configure(ClimberConstants.climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - pid.setReference(ClimberConstants.initialEncoderValue, ControlType.kPosition, ClosedLoopSlot.kSlot0, ClimberConstants.kSetG, ArbFFUnits.kVoltage); + // climberMotor.configure(ClimberConstants.climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // pid.setReference(ClimberConstants.initialEncoderValue, ControlType.kPosition, ClosedLoopSlot.kSlot0, ClimberConstants.kSetG, ArbFFUnits.kVoltage); prevSet = true; prevUp = false; @@ -46,6 +50,21 @@ public ClimberIOSpark() { kG = ClimberConstants.kClimbG; isLocked = true; + + climberSetConfig = new SparkMaxConfig(); + climberSetConfig + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(30) + .voltageCompensation(12); + // climberSetConfig.encoder + // .positionConversionFactor(kEncoderConversionFactor); + climberSetConfig.closedLoop + .feedbackSensor((FeedbackSensor.kPrimaryEncoder)) + .p(0) + .i(0) + .d(0); + + climberMotor.configure(climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @Override @@ -125,4 +144,17 @@ public void setVolts(double volts) { public double getPos() { return encoder.getPosition(); } + + @Override + public void setPos(double volts, double kG) { + pid.setReference(volts, ControlType.kPosition, ClosedLoopSlot.kSlot0, kG, ArbFFUnits.kVoltage); + } + + @Override + public void setReference(double p) { + climberSetConfig.closedLoop + .p(p); + climberMotor.configure(climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + pid.setReference(0,ControlType.kPosition, ClosedLoopSlot.kSlot0, 0, ArbFFUnits.kVoltage); + } }