diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2485c20..07a6ddc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,32 +4,73 @@ package frc.robot; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 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.OIConstants; +import frc.robot.constants.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.Elevator; public class RobotContainer { private Elevator elevator; + private CommandXboxController driver; + public RobotContainer() { elevator = new Elevator(); + driver = new CommandXboxController(OIConstants.kDriverUSB); + configureBindings(); configureShuffleboard(); } private void configureBindings() { elevator.setDefaultCommand( - elevator.debugSetpoints() + elevator.assistedManualControl(driver::getRightY) ); + + driver.a().onTrue(elevator.goToPosition(ElevatorPositions.kBottom, 3)); + driver.b().onTrue(elevator.goToPosition(ElevatorPositions.kLowMid, 3)); + driver.x().onTrue(elevator.goToPosition(ElevatorPositions.kHighMid, 3)); + driver.y().onTrue(elevator.goToPosition(ElevatorPositions.kTop, 3)); } private void configureShuffleboard() { ShuffleboardTab tab = Shuffleboard.getTab("MyTab"); - tab.add("Elevator Subsystem", elevator); + tab.addDouble("Elevator Encoder Distance", elevator::getEncoderDistance) + .withPosition(0, 0) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView); + + tab.addDouble("Elevator Encoder Rate", elevator::getEncoderRate) + .withPosition(2, 1) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView); + + tab.addDouble("Elevator Bottom Setpoint", ElevatorPositions.kBottom::getPositionValue) + .withPosition(0, 1) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView); + + tab.addDouble("Elevator Low-Mid Setpoint", ElevatorPositions.kLowMid::getPositionValue) + .withPosition(2, 1) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView); + + tab.addDouble("Elevator High-Mid Setpoint", ElevatorPositions.kHighMid::getPositionValue) + .withPosition(0, 2) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView); + + tab.addDouble("Elevator Top Setpoint", ElevatorPositions.kTop::getPositionValue) + .withPosition(2, 2) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kTextView); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 4f4298e..66b4e79 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -1,13 +1,33 @@ package frc.robot.constants; public interface ElevatorConstants { + public enum ElevatorPositions { + kBottom(0), + kLowMid(1), + kHighMid(2), + kTop(3); + + private double position; + + private ElevatorPositions(double position) { + this.position = position; + } + + public double getPositionValue() { + return position; + } + + public void setPositionValue(double newPosition) { + position = newPosition; + } + } public static final int kMotor1CANID = 0; public static final int kMotor2CANID = 1; public static final int kEncoderChannelADIO = 0; public static final int kEncoderChannelBDIO = 1; - public static final double kEncoderDistancePerPulse = 0; + public static final double kEncoderDistancePerPulse = 0.0000000001; //Avoids UncleanStatusException distancPerPulse must not be 0 public static final double kPPositional = 0; public static final double kIPositional = 0; @@ -24,8 +44,5 @@ public interface ElevatorConstants { public static final double kFFG = 0; public static final double kFFV = 0; - public static final double kDefaultBottomPositionalSetpoint = 0; - public static final double kDefaultLowMidPositionalSetpoint = 1; - public static final double kDefaultHighMidPositionalSetpoint = 2; - public static final double kDefaultTopPositionalSetpoint = 3; + public static final double kElevatorMaxVelocity = 0; } diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java new file mode 100644 index 0000000..54d25ac --- /dev/null +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -0,0 +1,5 @@ +package frc.robot.constants; + +public class OIConstants { + public static final int kDriverUSB = 0; +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 3773c1b..87d1be2 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import edu.wpi.first.math.controller.ElevatorFeedforward; @@ -7,11 +9,11 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.ElevatorConstants.ElevatorPositions; public class Elevator extends SubsystemBase { private WPI_VictorSPX motor1; @@ -24,16 +26,14 @@ public class Elevator extends SubsystemBase { private ElevatorFeedforward eff; - private double currentBottomPositionalSetpoint; - private double currentLowMidPositionalSetpoint; - private double currentHighMidPositionalSetpoint; - private double currentTopPositionalSetpoint; - public Elevator() { motor1 = new WPI_VictorSPX(ElevatorConstants.kMotor1CANID); motor2 = new WPI_VictorSPX(ElevatorConstants.kMotor2CANID); + motor2.follow(motor1); + encoder = new Encoder(ElevatorConstants.kEncoderChannelADIO, ElevatorConstants.kEncoderChannelBDIO); + encoder.setDistancePerPulse(ElevatorConstants.kEncoderDistancePerPulse); positionalController = new PIDController( ElevatorConstants.kPPositional, @@ -52,20 +52,47 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kFFG, ElevatorConstants.kFFV ); + } - currentBottomPositionalSetpoint = ElevatorConstants.kDefaultBottomPositionalSetpoint; - currentLowMidPositionalSetpoint = ElevatorConstants.kDefaultLowMidPositionalSetpoint; - currentHighMidPositionalSetpoint = ElevatorConstants.kDefaultHighMidPositionalSetpoint; - currentTopPositionalSetpoint = ElevatorConstants.kDefaultTopPositionalSetpoint; + public double getEncoderDistance() { + return encoder.getDistance(); + } + + public double getEncoderRate() { + return encoder.getRate(); + } + + public Command goToPosition(ElevatorPositions position, double timeout) { + return run(() -> { + double voltsOut = positionalController.calculate( + encoder.getDistance(), + position.getPositionValue() + ) + eff.calculate(0); + + motor1.setVoltage(voltsOut); + }).until(positionalController::atSetpoint).withTimeout(timeout); + } + + public Command assistedManualControl(DoubleSupplier speed) { + return run(() -> { + double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity; + + double voltsOut = velocityController.calculate( + encoder.getRate(), + realSpeedTarget + ) + eff.calculate(realSpeedTarget); + + motor1.setVoltage(voltsOut); + }); } public Command debugSetpoints() { return new RepeatCommand( runOnce(() -> { - System.out.println("Current Bottom Positional Setpoint: " + currentBottomPositionalSetpoint); - System.out.println("Current Low-Mid Positional Setpoint: " + currentLowMidPositionalSetpoint); - System.out.println("Current High-Mid Positional Setpoint: " + currentHighMidPositionalSetpoint); - System.out.println("Current Top Positional Setpoint: " + currentTopPositionalSetpoint); + System.out.println("Current Bottom Positional Setpoint: " + ElevatorPositions.kBottom.getPositionValue()); + System.out.println("Current Low-Mid Positional Setpoint: " + ElevatorPositions.kLowMid.getPositionValue()); + System.out.println("Current High-Mid Positional Setpoint: " + ElevatorPositions.kHighMid.getPositionValue()); + System.out.println("Current Top Positional Setpoint: " + ElevatorPositions.kTop.getPositionValue()); System.out.println(""); }).andThen(new WaitCommand(5)) ); @@ -77,30 +104,26 @@ public class Elevator extends SubsystemBase { builder.addDoubleProperty( ".currentBottomPositionalSetpoint", - () -> currentBottomPositionalSetpoint, - (d) -> { - System.out.println("Current Bottom Positional Setpoint Changed!"); - System.out.println("Old Value: " + currentBottomPositionalSetpoint + " New Value: " + d); - currentBottomPositionalSetpoint = d; - } + () -> ElevatorPositions.kBottom.getPositionValue(), + (d) -> ElevatorPositions.kBottom.setPositionValue(d) ); builder.addDoubleProperty( ".currentLowMidPositionalSetpoint", - () -> currentLowMidPositionalSetpoint, - (d) -> currentLowMidPositionalSetpoint = d + () -> ElevatorPositions.kLowMid.getPositionValue(), + (d) -> ElevatorPositions.kLowMid.setPositionValue(d) ); builder.addDoubleProperty( ".currentHighMidPositionalSetpoint", - () -> currentHighMidPositionalSetpoint, - (d) -> currentHighMidPositionalSetpoint = d + () -> ElevatorPositions.kHighMid.getPositionValue(), + (d) -> ElevatorPositions.kHighMid.setPositionValue(d) ); builder.addDoubleProperty( ".currentTopPositionalSetpoint", - () -> currentTopPositionalSetpoint, - (d) -> currentTopPositionalSetpoint = d + () -> ElevatorPositions.kTop.getPositionValue(), + (d) -> ElevatorPositions.kTop.setPositionValue(d) ); } }