sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

debug de samedi

Browse Source
This commit is contained in:
samuel desharnais
2026-03-21 16:11:24 -04:00
parent e0c1e26933
commit a7fb3108ea
18 changed files with 178 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/main/java/frc/robot/Robot.java
View File

@@ -4,9 +4,11 @@
package frc.robot;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.subsystems.Limelight3G;
public class Robot extends TimedRobot {
private Command m_autonomousCommand;
@@ -20,6 +22,15 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
var driveState = m_robotContainer.drivetrain.getState();
double headingDeg = driveState.Pose.getRotation().getDegrees();
double omegaRps = Units.radiansToRotations(driveState.Speeds.omegaRadiansPerSecond);
LimelightHelpers.SetRobotOrientation("limelight_tag", headingDeg, 0, 0, 0, 0, 0);
var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight_tag");
if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
}
}
@Override