Led Bugfix + Cleanup

Team3128 · Oct 26, 2023 · 83dc96c · 83dc96c
1 parent b70f4d6
commit 83dc96c
Show file tree

Hide file tree

Showing 5 changed files with 29 additions and 23 deletions.
diff --git 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
@@ -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
@@ -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());
     }
 }
diff --git 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
@@ -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;
         }