diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 079e51d6..46821aba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -176,7 +176,7 @@ public void putShuffleboardCommands() { } if (Constants.RAMP_DEBUG) { SmartShuffleboard.put("Ramp", "myTargetPos", 0); - SmartShuffleboard.putCommand("Ramp", "SetRamp", CommandUtil.logged(new RampMove(ramp, () -> SmartShuffleboard.getDouble("Ramp", "myTargetPos", 0)))); + SmartShuffleboard.putCommand("Ramp", "SetRamp", CommandUtil.logged(new RampMove(ramp, lightStrip, () -> SmartShuffleboard.getDouble("Ramp", "myTargetPos", 0)))); // SmartShuffleboard.putCommand("Ramp", "SetArmPID400", CommandUtil.logged(new RampMove(ramp, 15 ))); // SmartShuffleboard.putCommand("Ramp", "SetArmPID500", CommandUtil.logged(new RampMove(ramp, 500))); SmartShuffleboard.putCommand("Ramp", "ResetRamp", CommandUtil.logged(new ResetRamp(ramp, lightStrip))); @@ -236,12 +236,12 @@ private void configureBindings() { // Set up to shoot Speaker CLOSE - Y controller.y().onTrue(CommandUtil.parallel("Setup Speaker Shot (CLOSE)", - new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_SPEAKER_CLOSE), + new RampMove(ramp, lightStrip ,() -> GameConstants.RAMP_POS_SHOOT_SPEAKER_CLOSE), new ShootSpeaker(shooter, drivetrain, lightStrip))); // Set up to shoot Speaker AWAY - X controller.x().onTrue(CommandUtil.parallel("Setup Speaker Shot (AWAY)", - new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_SPEAKER_AWAY), + new RampMove(ramp, lightStrip ,() -> GameConstants.RAMP_POS_SHOOT_SPEAKER_AWAY), new ShootSpeaker(shooter, drivetrain, lightStrip))); // Cancell all - B @@ -252,14 +252,14 @@ private void configureBindings() { new TimedFeeder(feeder, lightStrip, Constants.TIMED_FEEDER_EXIT), new WaitCommand(GameConstants.SHOOTER_TIME_BEFORE_STOPPING), new StopShooter(shooter), - new RampMove(ramp, () -> GameConstants.RAMP_POS_STOW))); + new RampMove(ramp, lightStrip ,() -> GameConstants.RAMP_POS_STOW))); //Driver Shoot joyRightButton2.onTrue(CommandUtil.sequence("Driver Shoot", new TimedFeeder(feeder,lightStrip, Constants.TIMED_FEEDER_EXIT), new WaitCommand(GameConstants.SHOOTER_TIME_BEFORE_STOPPING), new StopShooter(shooter), - new RampMove(ramp, () -> GameConstants.RAMP_POS_STOW)) + new RampMove(ramp, lightStrip, () -> GameConstants.RAMP_POS_STOW)) ); // start intaking a note diff --git a/src/main/java/frc/robot/commands/ramp/RampMove.java b/src/main/java/frc/robot/commands/ramp/RampMove.java index fbb9db15..6854b450 100644 --- a/src/main/java/frc/robot/commands/ramp/RampMove.java +++ b/src/main/java/frc/robot/commands/ramp/RampMove.java @@ -1,33 +1,51 @@ package frc.robot.commands.ramp; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.subsystems.LightStrip; import frc.robot.subsystems.Ramp; +import frc.robot.utils.TimeoutCounter; import java.util.function.DoubleSupplier; -public class RampMove extends Command{ - +public class RampMove extends Command { + private final Ramp ramp; private final DoubleSupplier encoderValue; + private final TimeoutCounter timeoutCounter; + private final Timer timer = new Timer(); - public RampMove(Ramp ramp, DoubleSupplier encoderValue) { + public RampMove(Ramp ramp, LightStrip lightStrip, DoubleSupplier encoderValue) { this.ramp = ramp; this.encoderValue = encoderValue; + this.timeoutCounter = new TimeoutCounter("RampMove", lightStrip); addRequirements(this.ramp); } @Override public void initialize() { + timer.restart(); ramp.setRampPos(encoderValue.getAsDouble()); } @Override public void execute() { + ramp.updateFF(); + } + + @Override + public void end(boolean interrupted) { + timer.stop(); } @Override public boolean isFinished() { + if (timer.hasElapsed(Constants.RAMP_MOVE_TIMEOUT)){ + timeoutCounter.increaseTimeoutCount(); return true; + } + return Math.abs(ramp.getRampPos() - ramp.getDesiredPosition()) < Constants.RAMP_MOVE_THRESHOLD; } } diff --git a/src/main/java/frc/robot/commands/ramp/RampMoveAndWait.java b/src/main/java/frc/robot/commands/ramp/RampMoveAndWait.java index 68c4cf17..78fd4f6c 100644 --- a/src/main/java/frc/robot/commands/ramp/RampMoveAndWait.java +++ b/src/main/java/frc/robot/commands/ramp/RampMoveAndWait.java @@ -43,6 +43,7 @@ public void initialize() { @Override public void execute() { + ramp.updateFF(); } @Override diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 81b3a374..2c071f6b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -21,6 +21,9 @@ public class GameConstants { public static final boolean ENABLE_VISION = true; public static final boolean LED_DEBUG = false; public static final boolean RELY_COLOR_SENSOR = false; + public static final boolean LOG_LIMIT_SWITCHES = true; + public static final boolean DRIVE_CURRENT_DEBUG = false; + //AMP public static final double SHOOT_AMP_MOTOR_SPEED = 0.2;//0.2 @@ -154,5 +157,7 @@ public class GameConstants { public static final double RAMP_ELIM_FF_THRESHOLD = 0.075; public static final int LIGHTSTRIP_PORT = 7; public static final long GYRO_THREAD_RATE_MS = 10; + public static final double RAMP_MOVE_THRESHOLD = 0.001; + public static final double RAMP_MOVE_TIMEOUT = 5; } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 215a99b8..282314aa 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -118,7 +118,11 @@ public void periodic() { SmartShuffleboard.put("Climber", "Left Encoder", climberLeft.getEncoder().getPosition()); SmartShuffleboard.put("Climber", "Right Encoder", climberRight.getEncoder().getPosition()); + Logger.logBoolean(baseLogName + "climber/engaged",ratchetEngaged, Constants.ENABLE_LOGGING); + } + if (Constants.LOG_LIMIT_SWITCHES){ + Logger.logBoolean(baseLogName + "climber/isRevLeftLmt",isLeftReverseLimitSwitchPressed(), Constants.ENABLE_LOGGING); + Logger.logBoolean(baseLogName + "climber/isRevRightLmt",isRightReverseLimitSwitchPressed(), Constants.ENABLE_LOGGING); } - Logger.logBoolean(baseLogName + "climber/engaged",ratchetEngaged, Constants.ENABLE_LOGGING); } } diff --git a/src/main/java/frc/robot/subsystems/Deployer.java b/src/main/java/frc/robot/subsystems/Deployer.java index b3618841..4b37b258 100644 --- a/src/main/java/frc/robot/subsystems/Deployer.java +++ b/src/main/java/frc/robot/subsystems/Deployer.java @@ -70,15 +70,15 @@ public void periodic() { SmartShuffleboard.put("Deployer", "encoder", getEncoder()); SmartShuffleboard.put("Deployer", "Fwd Limt", isDeployerForwardLimitSwitchClosed()); SmartShuffleboard.put("Deployer", "Rev Limit", isDeployerReverseLimitSwitchClosed()); + SmartShuffleboard.put("Driver", "Is Intake Deployer Raised?", isDeployerForwardLimitSwitchClosed()) + .withPosition(0, 2) + .withSize(2, 2); } - - /* - SmartShuffleboard.put("Driver", "Is Intake Deployer Raised?", isDeployerForwardLimitSwitchClosed()) - .withPosition(0, 2) - .withSize(2, 2); - */ - Logger.logBoolean(baseLogName + "FWD LMT", isDeployerForwardLimitSwitchClosed(), Constants.ENABLE_LOGGING); - Logger.logBoolean(baseLogName + "REV LMT", isDeployerForwardLimitSwitchClosed(), Constants.ENABLE_LOGGING); + if (Constants.LOG_LIMIT_SWITCHES) { + Logger.logBoolean(baseLogName + "FWD LMT", isDeployerForwardLimitSwitchClosed(), Constants.ENABLE_LOGGING); + Logger.logBoolean(baseLogName + "REV LMT", isDeployerForwardLimitSwitchClosed(), Constants.ENABLE_LOGGING); + } + } //Spin deployer motor diff --git a/src/main/java/frc/robot/subsystems/Feeder.java b/src/main/java/frc/robot/subsystems/Feeder.java index b6361882..f3c2298b 100644 --- a/src/main/java/frc/robot/subsystems/Feeder.java +++ b/src/main/java/frc/robot/subsystems/Feeder.java @@ -77,14 +77,12 @@ public void periodic() { SmartShuffleboard.put("Feeder", "Color Sensor", "Certainty", matchedColor.confidence); SmartShuffleboard.put("Feeder", "Piece Seen Incoming", pieceSeen(true)); SmartShuffleboard.put("Feeder", "Piece Seen Reverse", pieceSeen(false)); + SmartShuffleboard.put("Driver", "Has Game Piece?", pieceSeen(false)) + .withPosition(0, 0) + .withSize(2, 2); + Logger.logDouble(baseLogName + "FeederMotorSpeed",getFeederMotorSpeed(),Constants.ENABLE_LOGGING); } - /* - SmartShuffleboard.put("Driver", "Has Game Piece?", pieceSeen(false)) - .withPosition(0, 0) - .withSize(2, 2); - */ - Logger.logDouble(baseLogName + "FeederMotorSpeed",getFeederMotorSpeed(),Constants.ENABLE_LOGGING); } public boolean getForwardSwitchTripped() { diff --git a/src/main/java/frc/robot/subsystems/LightStrip.java b/src/main/java/frc/robot/subsystems/LightStrip.java index 31e475bc..485c7922 100644 --- a/src/main/java/frc/robot/subsystems/LightStrip.java +++ b/src/main/java/frc/robot/subsystems/LightStrip.java @@ -1,52 +1,58 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.utils.BlinkinPattern; -import frc.robot.utils.logging.Logger; +import frc.robot.utils.smartshuffleboard.SmartShuffleboard; -import java.util.HashMap; -import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicReference; import java.util.function.BooleanSupplier; public class LightStrip extends SubsystemBase { - private static final String baseLogName = "/robot/lightstrip/"; private final Spark colorSensorPort; - private BlinkinPattern pattern = BlinkinPattern.WHITE; - private final Map predicateLightEvents = new HashMap<>(); - + private final AtomicReference pattern = new AtomicReference<>(BlinkinPattern.BLACK); + private final ConcurrentHashMap predicateLightEvents = new ConcurrentHashMap<>(); + private final AtomicBoolean running = new AtomicBoolean(true); public LightStrip(int port) { this.colorSensorPort = new Spark(port); - } - - @Override - public void periodic() { - predicateLightEvents.keySet() - .stream() - .filter(BooleanSupplier::getAsBoolean) - .findFirst() - .ifPresent(c -> { - setPattern(predicateLightEvents.get(c)); - predicateLightEvents.remove(c); - Logger.logInteger(baseLogName + "predicateSize", predicateLightEvents.size(), Constants.ENABLE_LOGGING); + new Thread(() -> { + while (running.get()){ + for (BooleanSupplier supplier : predicateLightEvents.keySet()){ + if (supplier.getAsBoolean()){ + setPattern(predicateLightEvents.get(supplier)); + predicateLightEvents.remove(supplier); + break; + } } - ); + try { + Thread.sleep(40); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } + }).start(); } public void setPattern(BlinkinPattern pattern) { - this.pattern = pattern; + this.pattern.set(pattern); colorSensorPort.set(pattern.getPwm()); - SmartDashboard.putString("PATTERN",pattern.toString()); - Logger.logDouble(baseLogName + "pwmSignal", pattern.getPwm(), Constants.ENABLE_LOGGING); - Logger.logString(baseLogName + "pwmName", pattern.toString(), Constants.ENABLE_LOGGING); + if (Constants.LED_DEBUG){ + SmartShuffleboard.put("Lightstrip", "pwm", pattern.getPwm()); + SmartShuffleboard.put("Lightstrip", "pattern", pattern.toString()); + } } public BlinkinPattern getPattern() { - return pattern; + return pattern.get(); } public void scheduleOnTrue(BooleanSupplier callable, BlinkinPattern pattern) { predicateLightEvents.put(callable, pattern); } + + public void setRunning(boolean running) { + this.running.set(running); + } } diff --git a/src/main/java/frc/robot/subsystems/Ramp.java b/src/main/java/frc/robot/subsystems/Ramp.java index 6659cf1a..0222b375 100644 --- a/src/main/java/frc/robot/subsystems/Ramp.java +++ b/src/main/java/frc/robot/subsystems/Ramp.java @@ -17,6 +17,8 @@ public class Ramp extends SubsystemBase { private final NeoPidMotor neoPidMotor; private double rampPos = Constants.RAMP_POS; private final InterpolatingDoubleTreeMap rampAngleMap = new InterpolatingDoubleTreeMap(); + private double neoModerFF = Constants.RAMP_PID_FAR_FF; + public Ramp() { neoPidMotor = new NeoPidMotor(Constants.RAMP_ID); configureMotor(); @@ -32,48 +34,33 @@ private void configureMotor() { neoPidMotor.setSmartMotionAllowedClosedLoopError(Constants.RAMP_ERROR_RANGE); neoPidMotor.setMaxAccel(Constants.RAMP_MAX_RPM_ACCELERATION); neoPidMotor.getPidController().setP(Constants.RAMP_PID_P); - neoPidMotor.getPidController().setFF(Constants.RAMP_PID_FAR_FF); + neoPidMotor.getPidController().setFF(neoModerFF); } public void periodic() { - if (Math.abs(getRampPos() - getDesiredPosition()) <= Constants.RAMP_ELIM_FF_THRESHOLD){ - neoPidMotor.getPidController().setFF(NeoPidMotor.DEFAULT_FF); - }else{ - neoPidMotor.getPidController().setFF(Constants.RAMP_PID_FAR_FF); - } if (Constants.RAMP_DEBUG){ SmartShuffleboard.put("Ramp", "Encoder Value", getRampPos()); SmartShuffleboard.put("Ramp", "Desired pos", rampPos); SmartShuffleboard.put("Ramp", "Reverse Switch Tripped", getReversedSwitchState()); SmartShuffleboard.put("Ramp", "Forward Switch Tripped", getForwardSwitchState()); -// double pidP = SmartShuffleboard.getDouble("Ramp", "PID P", neoPidMotor.getPidController().getP()); -// double pidI = SmartShuffleboard.getDouble("Ramp", "PID I", neoPidMotor.getPidController().getI()); -// double pidD = SmartShuffleboard.getDouble("Ramp", "PID D", neoPidMotor.getPidController().getD()); -// double pidFF = SmartShuffleboard.getDouble("Ramp", "PID FF", neoPidMotor.getPidController().getFF()); -// if (pidP != neoPidMotor.getPidController().getP()) neoPidMotor.getPidController().setP(pidP); -// if (pidI != neoPidMotor.getPidController().getI()) neoPidMotor.getPidController().setI(pidI); -// if (pidD != neoPidMotor.getPidController().getD()) neoPidMotor.getPidController().setD(pidD); -// if (pidFF != neoPidMotor.getPidController().getFF()) neoPidMotor.getPidController().setFF(pidFF); + SmartShuffleboard.put("Driver", "Speaker Close", isShootCloseAngle()) + .withPosition(9, 0) + .withSize(1, 1); + SmartShuffleboard.put("Driver", "Speaker Away", isShootAwayAngle()) + .withPosition(9, 1) + .withSize(1, 1); + SmartShuffleboard.put("Driver", "Amp", isShootAmpAngle()) + .withPosition(8, 1) + .withSize(1, 1); SmartShuffleboard.put("Ramp", "Forward Switch Tripped", getForwardSwitchState()); SmartShuffleboard.put("Ramp", "Forward Switch Tripped", getForwardSwitchState()); + Logger.logDouble(baseLogName + "EncoderValue", getRampPos(), Constants.ENABLE_LOGGING); + Logger.logDouble(baseLogName + "DesiredPos", rampPos, Constants.ENABLE_LOGGING); + } + if (Constants.LOG_LIMIT_SWITCHES){ + Logger.logBoolean(baseLogName + "FWD LMT", getForwardSwitchState(), Constants.ENABLE_LOGGING); + Logger.logBoolean(baseLogName + "REV LMT", getReversedSwitchState(), Constants.ENABLE_LOGGING); } - - Logger.logDouble(baseLogName + "EncoderValue", getRampPos(), Constants.ENABLE_LOGGING); - Logger.logDouble(baseLogName + "DesiredPos", rampPos, Constants.ENABLE_LOGGING); - Logger.logBoolean(baseLogName + "FWD LMT", getForwardSwitchState(), Constants.ENABLE_LOGGING); - Logger.logBoolean(baseLogName + "REV LMT", getReversedSwitchState(), Constants.ENABLE_LOGGING); - - /* - SmartShuffleboard.put("Driver", "Speaker Close", isShootCloseAngle()) - .withPosition(9, 0) - .withSize(1, 1); - SmartShuffleboard.put("Driver", "Speaker Away", isShootAwayAngle()) - .withPosition(9, 1) - .withSize(1, 1); - SmartShuffleboard.put("Driver", "Amp", isShootAmpAngle()) - .withPosition(8, 1) - .withSize(1, 1); - */ } public void setRampPos(double targetPosition) { @@ -150,4 +137,22 @@ public void setAngle(Rotation2d angleFromGround) { public double calcPose(Pose2d pose2d, Alignable alignable) { return rampAngleMap.get(pose2d.getTranslation().getDistance(new Translation2d(alignable.getX(), alignable.getY()))); } + + public void setFF(double feedForward) { + neoPidMotor.getPidController().setFF(feedForward); + neoModerFF = feedForward; + } + public double getFF(){ + return neoModerFF; + } + + public void updateFF() { + if (Math.abs(getRampPos() - getDesiredPosition()) <= Constants.RAMP_ELIM_FF_THRESHOLD) { + if (getFF() != NeoPidMotor.DEFAULT_FF) { + setFF(NeoPidMotor.DEFAULT_FF); + } + } else if (getFF() != Constants.RAMP_PID_FAR_FF) { + setFF(Constants.RAMP_PID_FAR_FF); + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index fa0569f2..ab878a1d 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; @@ -61,23 +60,22 @@ public class SwerveDrivetrain extends SubsystemBase { @Override public void periodic() { - if (Constants.RAMP_DEBUG) { - SmartDashboard.putNumber("DELTA X", Units.Inch.convertFrom(getPose().getTranslation().getDistance(new Translation2d(Alignable.SPEAKER.getX(), Alignable.SPEAKER.getY())),Units.Meter)); + if (Constants.DRIVE_CURRENT_DEBUG){ + frontLeftDriveCurrent = ((CANSparkMax)frontLeft.getSwerveMotor().getDriveMotor()).getOutputCurrent(); + frontRightDriveCurrent = ((CANSparkMax)frontRight.getSwerveMotor().getDriveMotor()).getOutputCurrent(); + backLeftDriveCurrent = ((CANSparkMax)backLeft.getSwerveMotor().getDriveMotor()).getOutputCurrent(); + backRightDriveCurrent = ((CANSparkMax)backRight.getSwerveMotor().getDriveMotor()).getOutputCurrent(); + frontLeftSteerCurrent = ((CANSparkMax)frontLeft.getSwerveMotor().getSteerMotor()).getOutputCurrent(); + frontRightSteerCurrent = ((CANSparkMax)frontRight.getSwerveMotor().getSteerMotor()).getOutputCurrent(); + backLeftSteerCurrent = ((CANSparkMax)backLeft.getSwerveMotor().getSteerMotor()).getOutputCurrent(); + backRightSteerCurrent = ((CANSparkMax)backRight.getSwerveMotor().getSteerMotor()).getOutputCurrent(); + totalSteerCurrent = frontLeftSteerCurrent + frontRightSteerCurrent + backRightSteerCurrent + backLeftSteerCurrent; + totalDriveCurrent = frontLeftDriveCurrent + frontRightDriveCurrent + backLeftDriveCurrent + backRightDriveCurrent; + totalCurrent = totalSteerCurrent + totalDriveCurrent; + Logger.logDouble("/Robot/DriveCurrent", totalDriveCurrent, Constants.ENABLE_LOGGING); + Logger.logDouble("/Robot/SteerCurrent", totalSteerCurrent, Constants.ENABLE_LOGGING); + Logger.logDouble("/Robot/TotalCurrent", totalCurrent, Constants.ENABLE_LOGGING); } - frontLeftDriveCurrent = ((CANSparkMax)frontLeft.getSwerveMotor().getDriveMotor()).getOutputCurrent(); - frontRightDriveCurrent = ((CANSparkMax)frontRight.getSwerveMotor().getDriveMotor()).getOutputCurrent(); - backLeftDriveCurrent = ((CANSparkMax)backLeft.getSwerveMotor().getDriveMotor()).getOutputCurrent(); - backRightDriveCurrent = ((CANSparkMax)backRight.getSwerveMotor().getDriveMotor()).getOutputCurrent(); - frontLeftSteerCurrent = ((CANSparkMax)frontLeft.getSwerveMotor().getSteerMotor()).getOutputCurrent(); - frontRightSteerCurrent = ((CANSparkMax)frontRight.getSwerveMotor().getSteerMotor()).getOutputCurrent(); - backLeftSteerCurrent = ((CANSparkMax)backLeft.getSwerveMotor().getSteerMotor()).getOutputCurrent(); - backRightSteerCurrent = ((CANSparkMax)backRight.getSwerveMotor().getSteerMotor()).getOutputCurrent(); - totalSteerCurrent = frontLeftSteerCurrent + frontRightSteerCurrent + backRightSteerCurrent + backLeftSteerCurrent; - totalDriveCurrent = frontLeftDriveCurrent + frontRightDriveCurrent + backLeftDriveCurrent + backRightDriveCurrent; - totalCurrent = totalSteerCurrent + totalDriveCurrent; - Logger.logDouble("/Robot/DriveCurrent", totalDriveCurrent, Constants.ENABLE_LOGGING); - Logger.logDouble("/Robot/SteerCurrent", totalSteerCurrent, Constants.ENABLE_LOGGING); - Logger.logDouble("/Robot/TotalCurrent", totalCurrent, Constants.ENABLE_LOGGING); if (Constants.PATHPLANNER_DEBUG){ SmartShuffleboard.putCommand("PathPlanner","Plan To Podium", PathPlannerUtils.autoFromPath(PathPlannerUtils.createManualPath(getPose(),new Pose2d(2.5,4,new Rotation2d(Math.PI)),0))); SmartShuffleboard.putCommand("PathPlanner","Plan To PodiumV2", PathPlannerUtils.pathToPose(new Pose2d(2.5,4,new Rotation2d(Math.PI)),0)); @@ -91,7 +89,9 @@ public void periodic() { SmartShuffleboard.put("DriveTrain", "Total Drive Current", totalDriveCurrent); SmartShuffleboard.put("DriveTrain", "Total Steer Current", totalSteerCurrent); SmartShuffleboard.put("DriveTrain", "TOTAL Current", totalCurrent); + SmartShuffleboard.put("GYRO", "Gyro Angle", gyro.getGyroValue()); } + if (Constants.ENABLE_VISION){ poseEstimator.updatePositionWithVis(gyro.getGyroValue());