Track target working

This commit is contained in:
Ryan Manley
2022-03-16 10:32:15 -06:00
parent b346fcbec5
commit ca4d87977a
7 changed files with 25 additions and 12 deletions
@@ -217,7 +217,7 @@ public class BoomBoom extends SubsystemBase {
public void runDrumShooterVelocityPID(double targetVel) {
SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset);
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel + pidOffset); // Init
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
// New BoomBoom controller stuff
// Controls a motor with the output of the BangBang controller
@@ -59,6 +59,8 @@ public class VisionOdometry extends SubsystemBase {
public ArrayList<Point> getTargetPoints() throws VisionObscuredException {
PhotonPipelineResult result = m_camera.getLatestResult();
latency = result.getLatencyMillis();
System.out.println("Result: " + result.hasTargets() + ", latency: " + latency);
if(!result.hasTargets())
throw new VisionObscuredException();