Fixed Sim latency issue (#42)

* Fixed Sim latency issue * Fix Spotless check
FRC-Team-620 · Jan 17, 2024 · 88de3aa · 88de3aa
1 parent 6a91b5e
commit 88de3aa
Show file tree

Hide file tree

Showing 3 changed files with 242 additions and 250 deletions.
diff --git a/src/main/java/org/jmhsrobotics/frc2024/Robot.java b/src/main/java/org/jmhsrobotics/frc2024/Robot.java
@@ -16,97 +16,103 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class Robot extends TimedRobot {
-  private Command autonomousCommand;
-
-  private RobotContainer m_robotContainer;
-
-  @Override
-  public void robotInit() {
-    m_robotContainer = new RobotContainer();
-    SmartDashboard.putData("IntakeCommand", new IntakeCommand(this.m_robotContainer.getDriveSubsystem(), 5));
-
-  }
-  @Override
-  public void robotPeriodic() {
-    CommandScheduler.getInstance().run();
-  }
-
-  @Override
-  public void disabledInit() {
-  }
-
-  @Override
-  public void disabledPeriodic() {
-  }
-
-  @Override
-  public void disabledExit() {
-  }
-
-  @Override
-  public void autonomousInit() {
-    autonomousCommand = m_robotContainer.getAutonomousCommand();
-
-    if (autonomousCommand != null) {
-      autonomousCommand.schedule();
-    }
-
-    List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(
-        new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0)),
-        new Pose2d(3.0, 1.0, Rotation2d.fromDegrees(0)),
-        new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(0)));
-
-    // Create the path using the bezier points created above
-    PathPlannerPath path = new PathPlannerPath(
-        bezierPoints,
-        new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), // The constraints for this path. If using a
-                                                                 // differential drivetrain, the angular constraints
-                                                                 // have no effect.
-        new GoalEndState(0.0, Rotation2d.fromDegrees(0)) // Goal end state. You can set a holonomic rotation here. If
-                                                         // using a differential drivetrain, the rotation will have no
-                                                         // effect.
-    );
-
-  }
-
-  @Override
-  public void autonomousPeriodic() {
-  }
-
-  @Override
-  public void autonomousExit() {
-  }
-
-  @Override
-  public void teleopInit() {
-    if (autonomousCommand != null) {
-      autonomousCommand.cancel();
-    }
-  }
-
-  @Override
-  public void teleopPeriodic() {
-  }
-
-  @Override
-  public void teleopExit() {
-  }
-
-  @Override
-  public void testInit() {
-    CommandScheduler.getInstance().cancelAll();
-  }
-
-  @Override
-  public void testPeriodic() {
-  }
-
-  @Override
-  public void testExit() {
-  }
+	private Command autonomousCommand;
+
+	private RobotContainer m_robotContainer;
+
+	@Override
+	public void robotInit() {
+		m_robotContainer = new RobotContainer();
+		SmartDashboard.putData("IntakeCommand", new IntakeCommand(this.m_robotContainer.getDriveSubsystem(), 5));
+
+	}
+	@Override
+	public void robotPeriodic() {
+		CommandScheduler.getInstance().run();
+	}
+
+	@Override
+	public void disabledInit() {
+	}
+
+	@Override
+	public void disabledPeriodic() {
+	}
+
+	@Override
+	public void disabledExit() {
+	}
+
+	@Override
+	public void autonomousInit() {
+		autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+		if (autonomousCommand != null) {
+			autonomousCommand.schedule();
+		}
+
+		List<Translation2d> bezierPoints = PathPlannerPath.bezierFromPoses(
+				new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0)), new Pose2d(3.0, 1.0, Rotation2d.fromDegrees(0)),
+				new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(0)));
+
+		// Create the path using the bezier points created above
+		PathPlannerPath path = new PathPlannerPath(bezierPoints,
+				new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), // The constraints for this path. If using a
+																			// differential drivetrain, the angular
+																			// constraints
+																			// have no effect.
+				new GoalEndState(0.0, Rotation2d.fromDegrees(0)) // Goal end state. You can set a holonomic rotation
+																	// here. If
+																	// using a differential drivetrain, the rotation
+																	// will have no
+																	// effect.
+		);
+
+	}
+
+	@Override
+	public void autonomousPeriodic() {
+	}
+
+	@Override
+	public void autonomousExit() {
+	}
+
+	@Override
+	public void teleopInit() {
+		if (autonomousCommand != null) {
+			autonomousCommand.cancel();
+		}
+	}
+
+	@Override
+	public void teleopPeriodic() {
+	}
+
+	@Override
+	public void teleopExit() {
+	}
+
+	@Override
+	public void testInit() {
+		CommandScheduler.getInstance().cancelAll();
+	}
+
+	@Override
+	public void testPeriodic() {
+	}
+
+	@Override
+	public void testExit() {
+	}
+	@Override
+	public void simulationInit() {
+		DriverStationSim.setEnabled(true);
+	}
 }