From 31855facad58262c6e076b7c1e7b272c75835ef6 Mon Sep 17 00:00:00 2001 From: Ammelll Date: Thu, 30 Nov 2023 18:49:21 -0800 Subject: [PATCH] Moving update entries to periodic --- src/main/java/frc/robot/Robot.java | 3 +++ src/main/java/frc/robot/subsystems/Swerve.java | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1d8f296..e12d01a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.autos.Auto; import frc.robot.commands.arm.MoveToPos; +import frc.robot.commands.swerve.Detector; import frc.robot.commands.testing.AutoAlignPennTester; /** @@ -37,6 +38,7 @@ public class Robot extends TimedRobot { private static LinkedHashMap autoMap = new LinkedHashMap(); private static LinkedHashMap tempMap = new LinkedHashMap(); private static SendableChooser chooser; + private Detector detector = new Detector(); /** * This function is run when the robot is first started up and should be used for any @@ -71,6 +73,7 @@ public void robotPeriodic() { // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. + detector.updateEntries(); CommandScheduler.getInstance().run(); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 0e81407..9bea129 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -36,14 +36,12 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.*; -import frc.robot.commands.swerve.Detector; public class Swerve extends SubsystemBase { // public SwerveDriveOdometry swerveOdometry; public SwerveDrivePoseEstimator estimator; public SwerveModule[] mSwerveMods; public Pigeon2 gyro; - public Detector detector; public boolean isUsingCones = true; private SwerveAutoBuilder autoBuilder = null; @@ -217,7 +215,6 @@ public double getRotationRadians(){ public void periodic(){ // swerveOdometry.update(getYaw(), getModulePositions()); estimator.update(getYaw(), getModulePositions()); - detector.updateEntries(); // for(SwerveModule mod : mSwerveMods){ // SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Cancoder", mod.getCanCoder().getDegrees()); // SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Integrated", mod.getPosition().angle.getDegrees());