diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2193af5..8dbef29 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,6 +1,7 @@ package frc.robot; import com.revrobotics.spark.config.SparkMaxConfig; + import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; @@ -280,16 +281,61 @@ 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 SparkMaxConfig climberConfig = new SparkMaxConfig(); + 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 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 kLightG = 0; + + public static final double setEncoderValue = 0; + public static final double initialEncoderValue = 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 { - climberConfig + climberSetConfig .idleMode(IdleMode.kBrake) .smartCurrentLimit(30) - .voltageCompensation(12) - ; + .voltageCompensation(12); + // climberSetConfig.encoder + // .positionConversionFactor(kEncoderConversionFactor); + climberSetConfig.closedLoop + .feedbackSensor((FeedbackSensor.kPrimaryEncoder)) + .p(kSetP) + .i(kSetP) + .d(kSetD); } } @@ -297,4 +343,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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5f932c9..e3e9158 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,12 +4,13 @@ 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; 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 +27,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 +77,19 @@ private void configureBindings() { driverController.x().onTrue(swerve.runOpenTestDrive()); driverController.x().onFalse(swerve.runStopDrive()); driverController.b().onTrue(swerve.runUpdateControlConstants()); + + // 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()); + + 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 new file mode 100644 index 0000000..b787189 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.climber; + +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; + +public class Climber extends SubsystemBase{ + + private ClimberIO climber; + + public Climber(ClimberIO climberIO) { + Preferences.initDouble("volts",0); + Preferences.initDouble("kG", 0); + Preferences.initDouble("P",0.1); + this.climber = climberIO; + } + + 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)); + } + + public Command runUp() { + return runOnce( () -> climber.setVolts(Preferences.getDouble("volts",0))); + } + + public Command runDown() { + return runOnce(() -> climber.setVolts(-Preferences.getDouble("volts",0))); + } + + public Command stop() { + 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 new file mode 100644 index 0000000..2e37e15 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public static class ClimberIOInputs { + } + + public default void setClimberVolts(boolean low, boolean high) {} + + 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) {} + + 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 new file mode 100644 index 0000000..0d0af56 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -0,0 +1,160 @@ +package frc.robot.subsystems.climber; + +import frc.robot.Constants.ClimberConstants; + +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; +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 boolean prevSet; + private boolean prevUp; + private boolean prevDown; + + private boolean up; + private boolean down; + + 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); + + prevSet = true; + prevUp = false; + prevDown = false; + + up = false; + down = false; + + 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 + public void climberInputs(boolean set, boolean up, boolean down) { + if(set && !prevSet && !up && !down) { + isLocked = false; + pid.setReference( + ClimberConstants.setEncoderValue, + ControlType.kPosition, + ClosedLoopSlot.kSlot0, + ClimberConstants.kSetG, + ArbFFUnits.kVoltage + ); + } + if(up && !this.up) { + isLocked = false; + climberMotor.setVoltage(ClimberConstants.upVolts); + } + if(!up && down && !this.down) { + isLocked = false; + climberMotor.setVoltage(ClimberConstants.downVolts); + } + 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; + prevDown = down; + } + + @Override + 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 + ); + } + + 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(); + } + + @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); + } +}