From d1453f7e299f00921a4250150783bbf8fcf9977e Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Thu, 21 Mar 2024 11:09:04 -0400 Subject: [PATCH 01/12] multi-thread led strip (needs to be tested. Might improve command scheduler loop overrun) --- .../java/frc/robot/subsystems/LightStrip.java | 30 +++++++------------ .../java/frc/robot/utils/BlinkinPattern.java | 12 ++++++++ 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LightStrip.java b/src/main/java/frc/robot/subsystems/LightStrip.java index 31e475bc..f1b8bb4e 100644 --- a/src/main/java/frc/robot/subsystems/LightStrip.java +++ b/src/main/java/frc/robot/subsystems/LightStrip.java @@ -1,49 +1,39 @@ 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 java.util.HashMap; import java.util.Map; +import java.util.concurrent.Executors; +import java.util.concurrent.TimeUnit; 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 BlinkinPattern pattern = BlinkinPattern.BLACK; private final Map predicateLightEvents = new HashMap<>(); - public LightStrip(int port) { this.colorSensorPort = new Spark(port); - } - - @Override - public void periodic() { - predicateLightEvents.keySet() + Executors.newScheduledThreadPool(1).scheduleAtFixedRate(() -> 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); - } - ); + setPattern(predicateLightEvents.get(c)); + predicateLightEvents.remove(c); + } + ),0,40, TimeUnit.MILLISECONDS); } - public void setPattern(BlinkinPattern pattern) { + public synchronized void setPattern(BlinkinPattern pattern) { this.pattern = 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); } - public BlinkinPattern getPattern() { + public synchronized BlinkinPattern getPattern() { return pattern; } public void scheduleOnTrue(BooleanSupplier callable, BlinkinPattern pattern) { diff --git a/src/main/java/frc/robot/utils/BlinkinPattern.java b/src/main/java/frc/robot/utils/BlinkinPattern.java index 21ffb26c..9bfddc14 100644 --- a/src/main/java/frc/robot/utils/BlinkinPattern.java +++ b/src/main/java/frc/robot/utils/BlinkinPattern.java @@ -142,4 +142,16 @@ public BlinkinPattern previous(){ } return values()[values().length - 1]; } + + /** + * @param pwm pwm single that represents pattern + * @return the corresponding pattern or BLACK if that pwm value is invalid + */ + public static BlinkinPattern of(double pwm){ + double pos = (pwm - start)/increment ; + if (pos == (int) pos){ + return values()[(int) pos]; + } + return BLACK; + } } From 326bc8acd227949703996e55f0a8b8dfb3772114 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Fri, 22 Mar 2024 10:16:46 -0400 Subject: [PATCH 02/12] moved shuffleboard stuff into debug --- .../java/frc/robot/subsystems/Deployer.java | 8 ++++---- .../java/frc/robot/subsystems/Feeder.java | 6 +++--- src/main/java/frc/robot/subsystems/Ramp.java | 19 ++++++++++--------- .../robot/subsystems/SwerveDrivetrain.java | 6 ++---- 4 files changed, 19 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Deployer.java b/src/main/java/frc/robot/subsystems/Deployer.java index e769cd23..ca9ce64e 100644 --- a/src/main/java/frc/robot/subsystems/Deployer.java +++ b/src/main/java/frc/robot/subsystems/Deployer.java @@ -64,11 +64,11 @@ 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); } diff --git a/src/main/java/frc/robot/subsystems/Feeder.java b/src/main/java/frc/robot/subsystems/Feeder.java index ae6397c1..9a00503c 100644 --- a/src/main/java/frc/robot/subsystems/Feeder.java +++ b/src/main/java/frc/robot/subsystems/Feeder.java @@ -72,11 +72,11 @@ 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); } - SmartShuffleboard.put("Driver", "Has Game Piece?", pieceSeen(false)) - .withPosition(0, 0) - .withSize(2, 2); Logger.logDouble(baseLogName + "FeederMotorSpeed",getFeederMotorSpeed(),Constants.ENABLE_LOGGING); } diff --git a/src/main/java/frc/robot/subsystems/Ramp.java b/src/main/java/frc/robot/subsystems/Ramp.java index 954fe958..c1d7119e 100644 --- a/src/main/java/frc/robot/subsystems/Ramp.java +++ b/src/main/java/frc/robot/subsystems/Ramp.java @@ -28,6 +28,15 @@ public void periodic() { SmartShuffleboard.put("Ramp", "Desired pos", rampPos); SmartShuffleboard.put("Ramp", "Reverse Switch Tripped", getReversedSwitchState()); SmartShuffleboard.put("Ramp", "Forward Switch Tripped", getForwardSwitchState()); + 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); } Logger.logDouble(baseLogName + "EncoderValue", getRampPos(), Constants.ENABLE_LOGGING); @@ -35,15 +44,7 @@ public void periodic() { 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) { diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index 5fbaf66d..d84610f7 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -22,8 +22,8 @@ import frc.robot.utils.PathPlannerUtils; import frc.robot.utils.diag.DiagSparkMaxAbsEncoder; import frc.robot.utils.diag.DiagSparkMaxEncoder; -import frc.robot.utils.smartshuffleboard.SmartShuffleboard; import frc.robot.utils.logging.Logger; +import frc.robot.utils.smartshuffleboard.SmartShuffleboard; public class SwerveDrivetrain extends SubsystemBase { @@ -96,6 +96,7 @@ 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", gyroValue); } @@ -105,9 +106,6 @@ public void periodic() { }else { poseEstimator.updatePosition(gyroValue); } - if (Constants.SWERVE_DEBUG) { - SmartShuffleboard.put("GYRO", "Gyro Angle", gyroValue); - } } public SwerveDrivetrain(SwerveIdConfig frontLeftConfig, SwerveIdConfig frontRightConfig, SwerveIdConfig backLeftConfig, SwerveIdConfig backRightConfig, From 43cd54201569a254578b9342c725002ecc85a874 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Fri, 22 Mar 2024 10:27:19 -0400 Subject: [PATCH 03/12] made limit only check for piece while running MoveToGamepiece command --- .../frc/robot/commands/MoveToGamepiece.java | 28 ++++++++------- .../java/frc/robot/subsystems/Vision.java | 35 +++---------------- 2 files changed, 19 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/commands/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index ca222887..bafdd95d 100644 --- a/src/main/java/frc/robot/commands/MoveToGamepiece.java +++ b/src/main/java/frc/robot/commands/MoveToGamepiece.java @@ -9,44 +9,46 @@ import frc.robot.subsystems.Vision; public class MoveToGamepiece extends Command { - private SwerveDrivetrain drivetrain; - private Vision vision; + private final SwerveDrivetrain drivetrain; + private final Vision vision; private double startTime; private final PIDController turningPIDController; private final PIDController movingPIDController; - private ChassisSpeeds driveStates; - private double ychange; - private double xchange; + private double yChange; + private double pieceNotSeenCounter = 0; public MoveToGamepiece(SwerveDrivetrain drivetrain, Vision vision) { this.drivetrain = drivetrain; this.vision = vision; - addRequirements(drivetrain); turningPIDController = new PIDController(Constants.MOVE_TO_GAMEPIECE_TURNING_P, 0, Constants.MOVE_TO_GAMEPIECE_TURNING_D); movingPIDController = new PIDController(Constants.MOVE_TO_GAMEPIECE_MOVING_P, 0, 0); - + addRequirements(drivetrain); } @Override public void initialize() { startTime = Timer.getFPGATimestamp(); + pieceNotSeenCounter = 0; } @Override public void execute() { - ychange = vision.getPieceOffestAngleY() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_Y; - xchange = vision.getPieceOffestAngleX() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_X; - if (vision.isPieceSeen() && (ychange > Constants.MOVE_TO_GAMEPIECE_THRESHOLD)) { - driveStates = new ChassisSpeeds(movingPIDController.calculate(ychange), 0, turningPIDController.calculate(xchange)); - drivetrain.drive(driveStates); + yChange = vision.getPieceOffestAngleY() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_Y; + double xChange = vision.getPieceOffestAngleX() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_X; + if (yChange == 0){ + pieceNotSeenCounter++; + } + if (pieceNotSeenCounter < Constants.LIMELIGHT_PIECE_NOT_SEEN_COUNT && yChange > Constants.MOVE_TO_GAMEPIECE_THRESHOLD){ + ChassisSpeeds driveStates = new ChassisSpeeds(movingPIDController.calculate(yChange), 0, turningPIDController.calculate(xChange)); + drivetrain.drive(driveStates); } } @Override public boolean isFinished() { - return ((Timer.getFPGATimestamp() - startTime > Constants.MOVE_TO_GAMEPIECE_TIMEOUT) || (ychange <= Constants.MOVE_TO_GAMEPIECE_THRESHOLD) || !vision.isPieceSeen()); + return ((Timer.getFPGATimestamp() - startTime > Constants.MOVE_TO_GAMEPIECE_TIMEOUT) || (yChange <= Constants.MOVE_TO_GAMEPIECE_THRESHOLD) || pieceNotSeenCounter >= Constants.LIMELIGHT_PIECE_NOT_SEEN_COUNT); } @Override diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 07b32d6f..a199a8c7 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -4,8 +4,6 @@ package frc.robot.subsystems; -import java.util.Map; - import edu.wpi.first.cscore.HttpCamera; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -19,16 +17,14 @@ import frc.robot.utils.diag.DiagLimelight; import frc.robot.utils.smartshuffleboard.SmartShuffleboard; +import java.util.Map; + public class Vision extends SubsystemBase { /** Creates a new Limelight. */ private final NetworkTable table; private final NetworkTableEntry tv; private final NetworkTableEntry tx; private final NetworkTableEntry ty; - private int noPieceSeenCounter; - private boolean pieceSeen; - private double x; - private double y; public Vision() { @@ -44,45 +40,22 @@ public Vision() { dashboardTab.add("Limelight feed", limelightFeed).withSize(6,4).withPosition(2, 0).withProperties(Map.of("Show Crosshair", false, "Show Controls", false)); } - /** - * - * @return whether or not the piece has not been seen for a number of cycles - */ - public boolean isPieceSeen() { - return pieceSeen; - } - /** * @return The piece's x offset angle in degrees and 0.0 if the piece isn't seen */ public double getPieceOffestAngleX() { - return x; + return tx.getDouble(0); } /** * @return The piece's y offset angle in degrees and 0.0 if the piece isn't seen */ public double getPieceOffestAngleY() { - return y; + return ty.getDouble(0); } @Override public void periodic() { - if(tv.getDouble(0) == 0) { - noPieceSeenCounter++; - if(noPieceSeenCounter >= Constants.LIMELIGHT_PIECE_NOT_SEEN_COUNT) { - x = 0.0; - y = 0.0; - pieceSeen = false; - } - } - else { - noPieceSeenCounter = 0; - x = tx.getDouble(0.0); - y = ty.getDouble(0.0); - pieceSeen = true; - } - if(Constants.VISION_DEBUG) { SmartShuffleboard.put("Vision", "tv", tv.getDouble(-1)); SmartShuffleboard.put("Vision", "tx", tx.getDouble(0.0)); From 3e4b61a87dc1ea52033afdcc62fc6fbf30ebc6d1 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Fri, 22 Mar 2024 11:03:57 -0400 Subject: [PATCH 04/12] stop logging joystick --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3d1e9884..11e40bce 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -38,7 +38,7 @@ public class Robot extends TimedRobot { public void robotInit() { if (Constants.ENABLE_LOGGING) { DataLogManager.start(); - DriverStation.startDataLog(DataLogManager.getLog(), true); + DriverStation.startDataLog(DataLogManager.getLog(), false); CommandScheduler.getInstance().onCommandInterrupt(command -> Logger.logInterruption(command.getName(), true)); } diagnostics = new Diagnostics(); From e421a7be211f45af299321bab90420a99a2f59a3 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sat, 23 Mar 2024 20:32:56 -0400 Subject: [PATCH 05/12] deleted empty file --- .../java/frc/robot/commands/sequences/TurnToGampieceGroup.java | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/sequences/TurnToGampieceGroup.java diff --git a/src/main/java/frc/robot/commands/sequences/TurnToGampieceGroup.java b/src/main/java/frc/robot/commands/sequences/TurnToGampieceGroup.java deleted file mode 100644 index e69de29b..00000000 From ccc1dd70d6b2ceed22acd6b5101916c22ca7918e Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 24 Mar 2024 18:44:03 -0400 Subject: [PATCH 06/12] use atomic reference instead of synchronized block. Also got rid of unneeded logging variable --- src/main/java/frc/robot/subsystems/LightStrip.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LightStrip.java b/src/main/java/frc/robot/subsystems/LightStrip.java index f1b8bb4e..8831a8bc 100644 --- a/src/main/java/frc/robot/subsystems/LightStrip.java +++ b/src/main/java/frc/robot/subsystems/LightStrip.java @@ -8,12 +8,12 @@ import java.util.Map; import java.util.concurrent.Executors; import java.util.concurrent.TimeUnit; +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.BLACK; + private final AtomicReference pattern = new AtomicReference<>(BlinkinPattern.BLACK); private final Map predicateLightEvents = new HashMap<>(); public LightStrip(int port) { this.colorSensorPort = new Spark(port); @@ -28,13 +28,13 @@ public LightStrip(int port) { ),0,40, TimeUnit.MILLISECONDS); } - public synchronized void setPattern(BlinkinPattern pattern) { - this.pattern = pattern; + public void setPattern(BlinkinPattern pattern) { + this.pattern.set(pattern); colorSensorPort.set(pattern.getPwm()); } - public synchronized BlinkinPattern getPattern() { - return pattern; + public BlinkinPattern getPattern() { + return pattern.get(); } public void scheduleOnTrue(BooleanSupplier callable, BlinkinPattern pattern) { predicateLightEvents.put(callable, pattern); From 1b251228b64b47083268f326093fa04a5281ecfa Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 24 Mar 2024 19:15:02 -0400 Subject: [PATCH 07/12] comment fixes --- .../java/frc/robot/subsystems/Feeder.java | 2 +- .../java/frc/robot/subsystems/LightStrip.java | 39 +++++++++++++------ src/main/java/frc/robot/subsystems/Ramp.java | 17 +++++--- .../robot/subsystems/SwerveDrivetrain.java | 2 +- 4 files changed, 42 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Feeder.java b/src/main/java/frc/robot/subsystems/Feeder.java index c8c55312..f3c2298b 100644 --- a/src/main/java/frc/robot/subsystems/Feeder.java +++ b/src/main/java/frc/robot/subsystems/Feeder.java @@ -80,9 +80,9 @@ public void periodic() { SmartShuffleboard.put("Driver", "Has Game Piece?", pieceSeen(false)) .withPosition(0, 0) .withSize(2, 2); + Logger.logDouble(baseLogName + "FeederMotorSpeed",getFeederMotorSpeed(),Constants.ENABLE_LOGGING); } - 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 8831a8bc..57338fa5 100644 --- a/src/main/java/frc/robot/subsystems/LightStrip.java +++ b/src/main/java/frc/robot/subsystems/LightStrip.java @@ -2,12 +2,13 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; import frc.robot.utils.BlinkinPattern; +import frc.robot.utils.smartshuffleboard.SmartShuffleboard; import java.util.HashMap; import java.util.Map; -import java.util.concurrent.Executors; -import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.atomic.AtomicReference; import java.util.function.BooleanSupplier; @@ -15,22 +16,34 @@ public class LightStrip extends SubsystemBase { private final Spark colorSensorPort; private final AtomicReference pattern = new AtomicReference<>(BlinkinPattern.BLACK); private final Map predicateLightEvents = new HashMap<>(); + private final AtomicBoolean running = new AtomicBoolean(true); public LightStrip(int port) { this.colorSensorPort = new Spark(port); - Executors.newScheduledThreadPool(1).scheduleAtFixedRate(() -> predicateLightEvents.keySet() - .stream() - .filter(BooleanSupplier::getAsBoolean) - .findFirst() - .ifPresent(c -> { - setPattern(predicateLightEvents.get(c)); - predicateLightEvents.remove(c); - } - ),0,40, TimeUnit.MILLISECONDS); + 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.set(pattern); colorSensorPort.set(pattern.getPwm()); + if (Constants.LED_DEBUG){ + SmartShuffleboard.put("Lightstrip", "pwm", pattern.getPwm()); + SmartShuffleboard.put("Lightstrip", "pattern", pattern.toString()); + } } public BlinkinPattern getPattern() { @@ -39,4 +52,8 @@ public BlinkinPattern getPattern() { 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 36f11584..217ea2be 100644 --- a/src/main/java/frc/robot/subsystems/Ramp.java +++ b/src/main/java/frc/robot/subsystems/Ramp.java @@ -12,6 +12,7 @@ public class Ramp extends SubsystemBase { private final String baseLogName = "/robot/ramp/"; private final NeoPidMotor neoPidMotor; private double rampPos = Constants.RAMP_POS; + private double neoModerFF = Constants.RAMP_PID_FAR_FF; public Ramp() { neoPidMotor = new NeoPidMotor(Constants.RAMP_ID); @@ -24,14 +25,18 @@ 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 (neoModerFF != NeoPidMotor.DEFAULT_FF){ + neoModerFF = NeoPidMotor.DEFAULT_FF; + neoPidMotor.getPidController().setFF(neoModerFF); + } + } else if (neoModerFF != Constants.RAMP_PID_FAR_FF){ + neoModerFF = Constants.RAMP_PID_FAR_FF; + neoPidMotor.getPidController().setFF(neoModerFF); } if (Constants.RAMP_DEBUG){ SmartShuffleboard.put("Ramp", "Encoder Value", getRampPos()); @@ -47,9 +52,11 @@ public void periodic() { 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 + "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); diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index d84610f7..180f7863 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -69,6 +69,7 @@ private double getGyro() { @Override public void periodic() { + gyroValue = getGyro(); frontLeftDriveCurrent = ((CANSparkMax)frontLeft.getSwerveMotor().getDriveMotor()).getOutputCurrent(); frontRightDriveCurrent = ((CANSparkMax)frontRight.getSwerveMotor().getDriveMotor()).getOutputCurrent(); backLeftDriveCurrent = ((CANSparkMax)backLeft.getSwerveMotor().getDriveMotor()).getOutputCurrent(); @@ -100,7 +101,6 @@ public void periodic() { } - gyroValue = getGyro(); if (Constants.ENABLE_VISION){ poseEstimator.updatePositionWithVis(gyroValue); }else { From b830a9cbc24f1b45f06e66b1ea6dd46710ad8234 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 24 Mar 2024 19:37:53 -0400 Subject: [PATCH 08/12] I think I fixed the logic --- .../java/frc/robot/commands/MoveToGamepiece.java | 16 ++++++++++++---- src/main/java/frc/robot/subsystems/Vision.java | 3 +++ 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index bafdd95d..cb1427ae 100644 --- a/src/main/java/frc/robot/commands/MoveToGamepiece.java +++ b/src/main/java/frc/robot/commands/MoveToGamepiece.java @@ -16,7 +16,8 @@ public class MoveToGamepiece extends Command { private final PIDController movingPIDController; private double yChange; private double pieceNotSeenCounter = 0; - + private double gamePieceX; + private double gamePieceY; public MoveToGamepiece(SwerveDrivetrain drivetrain, Vision vision) { @@ -35,11 +36,18 @@ public void initialize() { @Override public void execute() { - yChange = vision.getPieceOffestAngleY() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_Y; - double xChange = vision.getPieceOffestAngleX() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_X; - if (yChange == 0){ + if (vision.getTv() == 0){ pieceNotSeenCounter++; + if (pieceNotSeenCounter >= Constants.LIMELIGHT_PIECE_NOT_SEEN_COUNT){ + return; + } + } else { + pieceNotSeenCounter = 0; + gamePieceX = vision.getPieceOffestAngleX(); + gamePieceY = vision.getPieceOffestAngleY(); } + yChange = gamePieceY - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_Y; + double xChange = gamePieceX - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_X; if (pieceNotSeenCounter < Constants.LIMELIGHT_PIECE_NOT_SEEN_COUNT && yChange > Constants.MOVE_TO_GAMEPIECE_THRESHOLD){ ChassisSpeeds driveStates = new ChassisSpeeds(movingPIDController.calculate(yChange), 0, turningPIDController.calculate(xChange)); drivetrain.drive(driveStates); diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index a199a8c7..4db55e70 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -53,6 +53,9 @@ public double getPieceOffestAngleX() { public double getPieceOffestAngleY() { return ty.getDouble(0); } + public double getTv(){ + return tv.getDouble(0); + } @Override public void periodic() { From 4754c03e19813f5f122e2f6a92605e0abf054284 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Sun, 24 Mar 2024 23:32:50 -0400 Subject: [PATCH 09/12] comment fixes --- src/main/java/frc/robot/RobotContainer.java | 10 ++--- .../frc/robot/commands/MoveToGamepiece.java | 2 +- .../frc/robot/commands/ramp/RampFollow.java | 1 + .../frc/robot/commands/ramp/RampMove.java | 24 ++++++++++-- .../robot/commands/ramp/RampMoveAndWait.java | 1 + .../frc/robot/constants/GameConstants.java | 5 +++ .../java/frc/robot/subsystems/Climber.java | 6 ++- .../java/frc/robot/subsystems/Deployer.java | 6 ++- .../java/frc/robot/subsystems/LightStrip.java | 5 +-- src/main/java/frc/robot/subsystems/Ramp.java | 38 +++++++++++-------- .../robot/subsystems/SwerveDrivetrain.java | 30 ++++++++------- .../java/frc/robot/subsystems/Vision.java | 2 +- 12 files changed, 85 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1ae00a10..26dd0ae5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -178,7 +178,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))); @@ -238,12 +238,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 @@ -254,14 +254,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/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index cb1427ae..25c6ce53 100644 --- a/src/main/java/frc/robot/commands/MoveToGamepiece.java +++ b/src/main/java/frc/robot/commands/MoveToGamepiece.java @@ -36,7 +36,7 @@ public void initialize() { @Override public void execute() { - if (vision.getTv() == 0){ + if (vision.isPieceSeen() == 0){ pieceNotSeenCounter++; if (pieceNotSeenCounter >= Constants.LIMELIGHT_PIECE_NOT_SEEN_COUNT){ return; diff --git a/src/main/java/frc/robot/commands/ramp/RampFollow.java b/src/main/java/frc/robot/commands/ramp/RampFollow.java index f4fef8c2..bfc14330 100644 --- a/src/main/java/frc/robot/commands/ramp/RampFollow.java +++ b/src/main/java/frc/robot/commands/ramp/RampFollow.java @@ -46,6 +46,7 @@ public void execute() { } ramp.setAngle(targetAngle); } + ramp.updateFF(); } @Override 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 2c0b985d..870a5dca 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 @@ -153,5 +156,7 @@ public class GameConstants { public static final double RAMP_PID_FAR_FF = 0.00031; public static final double RAMP_ELIM_FF_THRESHOLD = 0.075; public static final int LIGHTSTRIP_PORT = 7; + 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 053b763c..4b37b258 100644 --- a/src/main/java/frc/robot/subsystems/Deployer.java +++ b/src/main/java/frc/robot/subsystems/Deployer.java @@ -74,9 +74,11 @@ public void periodic() { .withPosition(0, 2) .withSize(2, 2); } + if (Constants.LOG_LIMIT_SWITCHES) { + Logger.logBoolean(baseLogName + "FWD LMT", isDeployerForwardLimitSwitchClosed(), Constants.ENABLE_LOGGING); + Logger.logBoolean(baseLogName + "REV LMT", isDeployerForwardLimitSwitchClosed(), Constants.ENABLE_LOGGING); + } - 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/LightStrip.java b/src/main/java/frc/robot/subsystems/LightStrip.java index 57338fa5..485c7922 100644 --- a/src/main/java/frc/robot/subsystems/LightStrip.java +++ b/src/main/java/frc/robot/subsystems/LightStrip.java @@ -6,8 +6,7 @@ import frc.robot.utils.BlinkinPattern; 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; @@ -15,7 +14,7 @@ public class LightStrip extends SubsystemBase { private final Spark colorSensorPort; private final AtomicReference pattern = new AtomicReference<>(BlinkinPattern.BLACK); - private final Map predicateLightEvents = new HashMap<>(); + private final ConcurrentHashMap predicateLightEvents = new ConcurrentHashMap<>(); private final AtomicBoolean running = new AtomicBoolean(true); public LightStrip(int port) { this.colorSensorPort = new Spark(port); diff --git a/src/main/java/frc/robot/subsystems/Ramp.java b/src/main/java/frc/robot/subsystems/Ramp.java index 217ea2be..b9b9310f 100644 --- a/src/main/java/frc/robot/subsystems/Ramp.java +++ b/src/main/java/frc/robot/subsystems/Ramp.java @@ -29,15 +29,6 @@ private void configureMotor() { } public void periodic() { - if (Math.abs(getRampPos() - getDesiredPosition()) <= Constants.RAMP_ELIM_FF_THRESHOLD){ - if (neoModerFF != NeoPidMotor.DEFAULT_FF){ - neoModerFF = NeoPidMotor.DEFAULT_FF; - neoPidMotor.getPidController().setFF(neoModerFF); - } - } else if (neoModerFF != Constants.RAMP_PID_FAR_FF){ - neoModerFF = Constants.RAMP_PID_FAR_FF; - neoPidMotor.getPidController().setFF(neoModerFF); - } if (Constants.RAMP_DEBUG){ SmartShuffleboard.put("Ramp", "Encoder Value", getRampPos()); SmartShuffleboard.put("Ramp", "Desired pos", rampPos); @@ -55,13 +46,12 @@ public void periodic() { 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 + "DesiredPos", rampPos, Constants.ENABLE_LOGGING); - Logger.logBoolean(baseLogName + "FWD LMT", getForwardSwitchState(), Constants.ENABLE_LOGGING); - Logger.logBoolean(baseLogName + "REV LMT", getReversedSwitchState(), Constants.ENABLE_LOGGING); - - } public void setRampPos(double targetPosition) { @@ -134,4 +124,22 @@ public static double angleToEncoder(double angle){ public void setAngle(Rotation2d angleFromGround) { setRampPos(angleToEncoder(angleFromGround.getDegrees())); } + + 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 180f7863..0859aba7 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -70,20 +70,22 @@ private double getGyro() { @Override public void periodic() { gyroValue = getGyro(); - 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.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); + } 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)); diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 4db55e70..19372094 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -53,7 +53,7 @@ public double getPieceOffestAngleX() { public double getPieceOffestAngleY() { return ty.getDouble(0); } - public double getTv(){ + public double isPieceSeen(){ return tv.getDouble(0); } From 8b93d8726e02f88da7df870e75baa18e978e3636 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Mon, 25 Mar 2024 17:09:57 -0400 Subject: [PATCH 10/12] piece seen is now boolean --- src/main/java/frc/robot/commands/MoveToGamepiece.java | 2 +- src/main/java/frc/robot/subsystems/Vision.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index 25c6ce53..3a40715d 100644 --- a/src/main/java/frc/robot/commands/MoveToGamepiece.java +++ b/src/main/java/frc/robot/commands/MoveToGamepiece.java @@ -36,7 +36,7 @@ public void initialize() { @Override public void execute() { - if (vision.isPieceSeen() == 0){ + if (!vision.isPieceSeen()){ pieceNotSeenCounter++; if (pieceNotSeenCounter >= Constants.LIMELIGHT_PIECE_NOT_SEEN_COUNT){ return; diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 19372094..24c6771c 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -53,8 +53,8 @@ public double getPieceOffestAngleX() { public double getPieceOffestAngleY() { return ty.getDouble(0); } - public double isPieceSeen(){ - return tv.getDouble(0); + public boolean isPieceSeen(){ + return tv.getDouble(0) != 0; } @Override From 1210a5df6aff4698c53aea8aaafef42da356613c Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Wed, 27 Mar 2024 22:48:36 -0400 Subject: [PATCH 11/12] merge conflict --- src/main/java/frc/robot/subsystems/SwerveDrivetrain.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index 1180360a..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; @@ -90,7 +89,7 @@ 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", gyroValue); + SmartShuffleboard.put("GYRO", "Gyro Angle", gyro.getGyroValue()); } From 296fd668aaa0e4e399011b222f62feebdc4719c7 Mon Sep 17 00:00:00 2001 From: goatfanboi23 Date: Wed, 27 Mar 2024 22:51:31 -0400 Subject: [PATCH 12/12] merge conflict --- .../{ramp/RampFollow.java => sequences/TurnToGampieceGroup.java} | 0 src/main/java/frc/robot/utils/BlinkinPattern.java | 1 - 2 files changed, 1 deletion(-) rename src/main/java/frc/robot/commands/{ramp/RampFollow.java => sequences/TurnToGampieceGroup.java} (100%) diff --git a/src/main/java/frc/robot/commands/ramp/RampFollow.java b/src/main/java/frc/robot/commands/sequences/TurnToGampieceGroup.java similarity index 100% rename from src/main/java/frc/robot/commands/ramp/RampFollow.java rename to src/main/java/frc/robot/commands/sequences/TurnToGampieceGroup.java diff --git a/src/main/java/frc/robot/utils/BlinkinPattern.java b/src/main/java/frc/robot/utils/BlinkinPattern.java index 9bfddc14..e59af853 100644 --- a/src/main/java/frc/robot/utils/BlinkinPattern.java +++ b/src/main/java/frc/robot/utils/BlinkinPattern.java @@ -142,7 +142,6 @@ public BlinkinPattern previous(){ } return values()[values().length - 1]; } - /** * @param pwm pwm single that represents pattern * @return the corresponding pattern or BLACK if that pwm value is invalid