diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c6deaeb..cd1daf3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -454,7 +454,7 @@ public class RobotContainer { // * assume turret is already pointed towards target. return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), - new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true)); // * aim with RotateUntilTarget // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 8a51cc8..5fc7593 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -35,6 +35,8 @@ public class TrackTarget extends CommandBase { BoomBoom m_boomBoom; Hood m_hood; + boolean isAuto; + static double velocity; static double hoodPosition; @@ -46,18 +48,35 @@ public class TrackTarget extends CommandBase { boolean isExecuted = false; - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { + // timing + boolean isAimed; + + boolean timerStarted; + long startTime; + private double timeTolerance; + + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) { m_turret = turret; m_boomBoom = boomBoom; m_hood = hood; m_visionOdometry = visionOdometry; + this.isAuto = isAuto; + this.timeTolerance = 1000; + addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); } + public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { + this(turret, boomBoom, hood, visionOdometry, false); + } + // Called when the command is initially scheduled. @Override public void initialize() { + timerStarted = false; + startTime = 0; + velocity = 0; hoodPosition = 0; } @@ -110,6 +129,8 @@ public class TrackTarget extends CommandBase { } catch (Exception e){ e.printStackTrace(); } + + // timing } // public ArrayList filterPoints(ArrayList points) { @@ -161,7 +182,15 @@ public class TrackTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + if (this.isAuto) { + if (targetLocked& !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + } else { + return false; + } // return isExecuted && Math.abs(output) < .1; } }