Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

visionOdo

Browse Source
This commit is contained in:
ryan123rudder
2022-03-06 00:24:51 -07:00
parent f452b7cd7c
commit 9034e538de
5 changed files with 34 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/AimToCenter.java
+8 -2
View File
@@ -45,8 +45,14 @@ public class AimToCenter extends CommandBase {
m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
m_turret.runshooterRotatePID(m_targetAngle);
// Check if limelight is within range
m_visionOdometry.setLEDs(Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE);
// Check if limelight is within range (comment out to disable vision odo)
if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
m_visionOdometry.updateOdometryWithVision();
m_visionOdometry.setLEDs(true);
}
else{
m_visionOdometry.setLEDs(false);
}
}
public static double angleToCenter(double x, double y, double gyro) {