diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1987d58..02b6f7b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -120,7 +120,7 @@ public final class Constants { public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0, 0.0453, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0); public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); public static final double SHOOTER_TURRET_MIN = -1.0; @@ -130,7 +130,7 @@ public final class Constants { } public static final class ClimberConstants { - public static final int CLIMBER_SPARK_ID = 10; + public static final int CLIMBER_SPARK_ID = -10; } public static final class LevelerConstants { @@ -138,7 +138,7 @@ public final class Constants { } public static final class StorageConstants { - public static final int STORAGE_CAN_ID = -1; + public static final int STORAGE_CAN_ID = 11; public static final double STORAGE_PARTIAL_BALL = 2; public static final double STORAGE_FULL_BALL = 7; public static final double STORAGE_SPEED = 0.5; @@ -159,7 +159,7 @@ public final class Constants { /* PID Gains */ public static final double STORAGE_MIN_OUTPUT = -1.0; - public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains STORAGE_GAINS = new Gains(1, 0.0, 0.0, 0.0, 0, 1.0); } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 76b8ed1..dbb29f6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -41,7 +41,6 @@ import frc4388.robot.commands.StorageIntake; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; -import frc4388.robot.commands.StorageIntakeGroup; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -105,7 +104,7 @@ public class RobotContainer { // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // runs the turret with joystick - m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); + m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); // moves the drum not m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); // drives climber with input from triggers on the opperator controller @@ -114,6 +113,8 @@ public class RobotContainer { m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); + //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } /** @@ -149,15 +150,18 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + /* Operator Buttons */ // shoots until released new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false); + .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false); + .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -171,7 +175,9 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) + //.whileHeld(new TrackTarget(m_robotShooter,m_robotShooterAim)) .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); //Prepares storage for intaking new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) @@ -182,10 +188,12 @@ public class RobotContainer { .whileHeld(new StorageOutake(m_robotStorage)); new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)); + .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.2))) + .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity()))); + .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.2))) + .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); } /** diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java index cbe43cf..bbbe656 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/HoldTarget.java @@ -49,7 +49,8 @@ public class HoldTarget extends CommandBase { @Override public void initialize() { //Vision Processing Mode - LimeLight.limeOn(); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); } @@ -88,8 +89,7 @@ public class HoldTarget extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - //Drive Camera Mode - LimeLight.limeOff(); + } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java index 7d23c65..10eb073 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java @@ -28,7 +28,7 @@ public class ShootFireGroup extends ParallelRaceGroup { new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())), new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), new HoldTarget(m_shooter, m_shooterAim), - new StorageRun(m_storage) + new StoragePositionPID(m_storage) ); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index e84813c..a5e0b5a 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Shooter; @@ -35,6 +36,8 @@ public class ShooterVelocityControlPID extends CommandBase { public void execute() { m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); m_shooter.runAngleAdjustPID(m_shooter.addFireAngle()); + SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); + SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); } // Called once the command ends or is interrupted. @@ -49,9 +52,11 @@ public class ShooterVelocityControlPID extends CommandBase { double upperBound = m_targetVel + 300; double lowerBound = m_targetVel - 300; if (m_actualVel < upperBound && m_actualVel > lowerBound){ + SmartDashboard.putBoolean("ShooterVelocityPID Finished", true); return true; } else{ + SmartDashboard.putBoolean("ShooterVelocityPID Finished", false); return false; } } diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java index b382a40..69de480 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntake.java +++ b/src/main/java/frc4388/robot/commands/StorageIntake.java @@ -37,7 +37,7 @@ public class StorageIntake extends CommandBase { @Override public void execute() { - if (m_storage.getBeam(0)){ + if (!m_storage.getBeam(0)){ m_storage.runStorage(StorageConstants.STORAGE_SPEED); intook = true; } @@ -54,7 +54,7 @@ public class StorageIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (!m_storage.getBeam(0) && intook){ + if (m_storage.getBeam(0) && intook){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/StoragePositionPID.java index 1270d3c..9536e24 100644 --- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java +++ b/src/main/java/frc4388/robot/commands/StoragePositionPID.java @@ -8,30 +8,31 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Storage; - public class StoragePositionPID extends CommandBase { - Storage m_storage; + public Storage m_storage; + double startPos; /** * Creates a new StoragePositionPID. */ - public StoragePositionPID(Storage storage) { - m_storage = storage; + public StoragePositionPID(Storage subsystem) { + m_storage = subsystem; addRequirements(m_storage); } // Called when the command is initially scheduled. @Override public void initialize() { + startPos = m_storage.getEncoderPos(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL)); + System.err.println("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooiujgxzxfghjkiujsdasdgioiedsdjkl"); + m_storage.runStoragePositionPID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL); } // Called once the command ends or is interrupted. @@ -42,6 +43,10 @@ public class StoragePositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { + if (startPos + StorageConstants.STORAGE_FULL_BALL == m_storage.getEncoderPos()) + { + return true; + } return false; } } diff --git a/src/main/java/frc4388/robot/commands/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/StoragePrepAim.java index 7af0252..2782c7c 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepAim.java +++ b/src/main/java/frc4388/robot/commands/StoragePrepAim.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Storage; @@ -29,7 +30,7 @@ public class StoragePrepAim extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_storage.getBeam(1) == false){ + if (m_storage.getBeam(1)){ m_storage.runStorage(StorageConstants.STORAGE_SPEED); } else{ @@ -45,9 +46,11 @@ public class StoragePrepAim extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_storage.getBeam(1)){ + if (!m_storage.getBeam(1)){ + SmartDashboard.putBoolean("StoragePrepAim Finished", true); return true; } + SmartDashboard.putBoolean("StoragePrepAim Finished", false); return false; } } diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java index 3a94353..643c858 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java @@ -36,7 +36,8 @@ public class StoragePrepIntake extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (!m_storage.getBeam(0) && System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed + System.err.println(m_storage.getBeam(0)); + if (m_storage.getBeam(0)){//&& System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed m_storage.runStorage(-StorageConstants.STORAGE_SPEED); } else{ @@ -52,7 +53,7 @@ public class StoragePrepIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_storage.getBeam(0)){ + if (!m_storage.getBeam(0)){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index e7c3c08..5db2669 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -51,7 +51,8 @@ public class TrackTarget extends CommandBase { @Override public void initialize() { // Vision Processing Mode - LimeLight.limeOn(); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); } // Called every time the scheduler runs while the command is scheduled. @@ -93,17 +94,18 @@ public class TrackTarget extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - // Drive Camera Mode - LimeLight.limeOff(); + } // Returns true when the command should end. @Override public boolean isFinished() { - if (xAngle < 1 && xAngle > -1) + if (xAngle < 1 && xAngle > -1 && target == 1) { + SmartDashboard.putBoolean("TrackTarget Finished", true); return true; } + SmartDashboard.putBoolean("TrackTarget Finished", false); return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fca1e7b..3a87531 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -110,7 +110,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted); m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted); m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted); - m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted); + // m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted); /* Config Open Loop Ramp so we don't make sudden output changes */ m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java index 70c53ae..d8e39a4 100644 --- a/src/main/java/frc4388/robot/subsystems/LimeLight.java +++ b/src/main/java/frc4388/robot/subsystems/LimeLight.java @@ -18,12 +18,12 @@ public class LimeLight extends SubsystemBase { } - public static void limeOff(){ + public void limeOff(){ NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); } - public static void limeOn(){ + public void limeOn(){ NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 2180447..c8bb07d 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -13,9 +13,11 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANDigitalInput; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.ControlType; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Joystick; @@ -48,6 +50,7 @@ public class Shooter extends SubsystemBase { public boolean velReached; public double m_fireVel; public double m_fireAngle; + CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; /* * Creates a new Shooter subsystem. @@ -81,6 +84,13 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); + + + + m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodUpLimit.enableLimitSwitch(true); + m_hoodDownLimit.enableLimitSwitch(true); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index b6ffef2..beeccda 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -7,10 +7,12 @@ package frc4388.robot.subsystems; +import com.revrobotics.CANDigitalInput; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.ControlType; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -22,6 +24,7 @@ import frc4388.robot.Constants.ShooterConstants; public class ShooterAim extends SubsystemBase { public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; + CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit; // Configure PID Controllers CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); @@ -32,10 +35,15 @@ public class ShooterAim extends SubsystemBase { public ShooterAim() { resetGyroShooterRotate(); m_shooterRotateMotor.setIdleMode(IdleMode.kBrake); + + m_shooterRightLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_shooterLeftLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_shooterRightLimit.enableLimitSwitch(true); + m_shooterLeftLimit.enableLimitSwitch(true); } public void runShooterWithInput(double input) { - m_shooterRotateMotor.set(input); + m_shooterRotateMotor.set(input/3); } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 74b2484..ec258c5 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -20,12 +20,13 @@ import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.command.WaitUntilCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { - private CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); private DigitalInput[] m_beamSensors = new DigitalInput[6]; CANPIDController m_storagePIDController = m_storageMotor.getPIDController(); @@ -69,6 +70,11 @@ public class Storage extends SubsystemBase { m_encoder.setPosition(0); } + public void testBeams(){ + SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get()); + SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get()); + } + /* Storage PID Control */ public void runStoragePositionPID(double targetPos){ // Set PID Coefficients @@ -79,6 +85,8 @@ public class Storage extends SubsystemBase { m_storagePIDController.setFF(storageGains.m_kF); m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput); + SmartDashboard.putNumber("Storage Position PID Target", targetPos); + SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); m_storagePIDController.setReference(targetPos, ControlType.kPosition); }