diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 5ee7c8c..15fe417 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -102,14 +102,14 @@ public class AutoShootCommandChassisTurret extends Command{ // No april tag in camera view. Rotate towards our theoretical closest tag if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ - Logger.recordOutput("AutoAlignState", "Look for tag"); + Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); Pose3d botPose = driveSubsystem.getBotPose(); - //driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this + driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this } else // We have an april tag, center it to the camera frame { - Logger.recordOutput("AutoAlignState", "Center on tag"); + Logger.recordOutput("AutoAlignState-Tracking", "Camera"); // [tx, ty, tz, pitch, yaw, roll] double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); if(targetsPose.length >= 1){ @@ -119,8 +119,10 @@ public class AutoShootCommandChassisTurret extends Command{ // If robot is centered on HUB and shooter is up to speed. Run the kicker if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){ - Logger.recordOutput("AutoAlignState", "FIRE!"); + Logger.recordOutput("AutoAlignState-Shoot", "FIRE!"); hopperSubsystem.feedKicker(); + }else{ + Logger.recordOutput("AutoAlignState-Shoot", "Waiting"); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 1f709e1..9a2838e 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -99,12 +99,18 @@ public class TurretSubsystem extends TestableSubsystem { } public void setLauncher(double speed) { + if(speed == 0) + { + launcherMotor.stopMotor(); + return; + } launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - // launcherMotor.set(speed/1000); } public boolean isLauncherUpToSpeed() { - return true; + // Valid if velcity is within 10% of setpoint. + // May need to lower this + return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1; } public void turretControl(double position)