From 83dc96cdef2357f7c78112eda83734337ceae790 Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Thu, 26 Oct 2023 01:14:43 -0700 Subject: [PATCH] Led Bugfix + Cleanup --- src/main/java/frc/team3128/Constants.java | 2 +- src/main/java/frc/team3128/Robot.java | 26 ++++++++----------- .../frc/team3128/autonomous/AutoPrograms.java | 16 +++++++----- .../frc/team3128/autonomous/Trajectories.java | 4 +++ .../frc/team3128/commands/CmdAutoBalance.java | 4 ++- 5 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 2bca0dd..d4d0439 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -363,7 +363,7 @@ public static class ManipulatorConstants{ public static final double STALL_POWER_CUBE = 0.1; - public static final double CURRENT_THRESHOLD_CONE = 30; + public static final double CURRENT_THRESHOLD_CONE = 35; public static final double CURRENT_THRESHOLD_CUBE = 25; } diff --git a/src/main/java/frc/team3128/Robot.java b/src/main/java/frc/team3128/Robot.java index 46ff8c4..57aadac 100644 --- a/src/main/java/frc/team3128/Robot.java +++ b/src/main/java/frc/team3128/Robot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.autonomous.AutoPrograms; +import frc.team3128.commands.CmdManager; import frc.team3128.subsystems.Leds; import frc.team3128.subsystems.Swerve; @@ -23,10 +24,8 @@ public class Robot extends TimedRobot { public static Robot instance; public static RobotContainer m_robotContainer = new RobotContainer(); - private Command m_autonomousCommand; public static AutoPrograms autoPrograms = new AutoPrograms(); - public Timer timer; - public Timer xlockTimer; + public Timer xLockTimer = new Timer(); public double startTime; public static synchronized Robot getInstance() { @@ -38,6 +37,7 @@ public static synchronized Robot getInstance() { @Override public void robotInit(){ + m_robotContainer.init(); LiveWindow.disableAllTelemetry(); } @@ -48,38 +48,34 @@ public void robotPeriodic(){ @Override public void autonomousInit() { - m_robotContainer.init(); - timer = new Timer(); - m_autonomousCommand = autoPrograms.getAutonomousCommand(); - Leds.getInstance().defaultColor = Colors.AUTO; + xLockTimer.restart(); + Command m_autonomousCommand = autoPrograms.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); - timer.start(); } } @Override public void autonomousPeriodic() { CommandScheduler.getInstance().run(); - if (timer.hasElapsed(14.75)) { + if (xLockTimer.hasElapsed(14.75)) { new RunCommand(()->Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); } } @Override public void teleopInit() { - Leds.getInstance().defaultColor = Colors.CHUTE; - m_robotContainer.init(); - xlockTimer = new Timer(); - xlockTimer.start(); + Leds.getInstance().defaultColor = CmdManager.SINGLE_STATION ? Colors.CHUTE : Colors.SHELF; + Leds.getInstance().resetLeds(); + xLockTimer.restart(); CommandScheduler.getInstance().cancelAll(); } @Override public void teleopPeriodic() { CommandScheduler.getInstance().run(); - if (xlockTimer.hasElapsed(134.75)) { - new RunCommand(()-> Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); + if (xLockTimer.hasElapsed(134.75)) { + // new RunCommand(()-> Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); } } diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index c4e9767..b2946e8 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -23,7 +23,7 @@ public AutoPrograms() { } private void initAutoSelector() { - String[] autoStrings = new String[] { + final String[] autoStrings = new String[] { //Blue Autos //Cable "cable_1Cone+1Cube","cable_1Cone+2Cube", @@ -39,20 +39,24 @@ private void initAutoSelector() { public Command getAutonomousCommand() { String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); + final Command autoCommand; if (selectedAutoName == null) { - return score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()); + autoCommand = score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()); } - if (selectedAutoName == "scuffedClimb") { - return sequence( + else if (selectedAutoName == "scuffedClimb") { + autoCommand = sequence( score(Position.HIGH_CONE, true).beforeStarting(Trajectories.resetAuto()), new CmdAutoBalance(false) ); } - selectedAutoName = (DriverStation.getAlliance() == Alliance.Red) ? "r_" : "b_" + selectedAutoName; + else { + selectedAutoName = (DriverStation.getAlliance() == Alliance.Red) ? "r_" : "b_" + selectedAutoName; + autoCommand = Trajectories.get(selectedAutoName); + } - return Trajectories.get(selectedAutoName); + return autoCommand.beforeStarting(Trajectories.resetAuto()); } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index b6d68d0..184ad27 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -18,10 +18,12 @@ import static frc.team3128.Constants.AutoConstants.*; import static frc.team3128.Constants.SwerveConstants.*; +import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.PositionConstants.Position; import static frc.team3128.commands.CmdManager.*; import frc.team3128.commands.CmdAutoBalance; +import frc.team3128.subsystems.Leds; import frc.team3128.subsystems.Manipulator; import frc.team3128.subsystems.Swerve; @@ -102,6 +104,8 @@ public static CommandBase get(String name) { public static CommandBase resetAuto() { return sequence( + runOnce(()-> Leds.getInstance().defaultColor = Colors.AUTO), + resetLeds(), resetGyro(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), resetAll(), diff --git a/src/main/java/frc/team3128/commands/CmdAutoBalance.java b/src/main/java/frc/team3128/commands/CmdAutoBalance.java index 1af2845..db4f2a9 100644 --- a/src/main/java/frc/team3128/commands/CmdAutoBalance.java +++ b/src/main/java/frc/team3128/commands/CmdAutoBalance.java @@ -52,8 +52,10 @@ public void execute() { if (advAngle > RAMP_THRESHOLD) onRamp = true; if (Math.abs(advAngle) < ANGLE_THRESHOLD && onRamp) { - Manipulator.getInstance().outtake(); swerve.xlock(); + if (direction == 1) { + Manipulator.getInstance().set(-1); + } return; }