99 lines
2.5 KiB
Java
99 lines
2.5 KiB
Java
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
package frc.robot;
|
|
|
|
import edu.wpi.first.math.VecBuilder;
|
|
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;
|
|
|
|
public class Robot extends TimedRobot {
|
|
private Command m_autonomousCommand;
|
|
|
|
private final RobotContainer m_robotContainer;
|
|
|
|
public Robot() {
|
|
m_robotContainer = new RobotContainer();
|
|
}
|
|
|
|
@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");
|
|
var stdDevs = LimelightHelpers.getLimelightDoubleArrayEntry("limelight_tag", "stddevs");
|
|
var limelightStdDevs = VecBuilder.fill(.5, .5, 9999999);
|
|
if (stdDevs.exists()) {
|
|
limelightStdDevs = VecBuilder.fill(stdDevs.get()[6], stdDevs.get()[7], stdDevs.get()[12]);
|
|
}
|
|
if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
|
|
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds,
|
|
limelightStdDevs);
|
|
}
|
|
}
|
|
|
|
@Override
|
|
public void disabledInit() {
|
|
}
|
|
|
|
@Override
|
|
public void disabledPeriodic() {
|
|
}
|
|
|
|
@Override
|
|
public void disabledExit() {
|
|
}
|
|
|
|
@Override
|
|
public void autonomousInit() {
|
|
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
|
|
|
if (m_autonomousCommand != null) {
|
|
m_autonomousCommand.schedule();
|
|
}
|
|
}
|
|
|
|
@Override
|
|
public void autonomousPeriodic() {
|
|
}
|
|
|
|
@Override
|
|
public void autonomousExit() {
|
|
}
|
|
|
|
@Override
|
|
public void teleopInit() {
|
|
if (m_autonomousCommand != null) {
|
|
m_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() {
|
|
}
|
|
}
|