diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index df12c6cc..75181371 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,6 +53,7 @@ import frc.robot.commands.shooter.SpinShooter; import frc.robot.commands.testing.RunDashboardShotTest; import frc.robot.commands.turret.DefaultTurretControl; +import frc.robot.commands.turret.ManualTurretMove; import frc.robot.commands.turret.RunTurretToFwdLimit; import frc.robot.commands.turret.RunTurretToRevLimit; import frc.robot.commands.turret.SetTurretAngle; @@ -297,6 +298,7 @@ private void configureBindings() { controller.b().onTrue(new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); controller.x().onTrue(new ForceDeployDown(intakeDeployer, shootState)); controller.y().onTrue(new ToggleAdjustedPosition(controllerSubsystem)); + controller.leftStick().whileTrue(new ManualTurretMove(turretSubsystem, controller::getLeftX)); controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); controller.povRight().onTrue(new SetShootingState(shootState, ShootState.STOPPED)); controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); @@ -320,7 +322,7 @@ private void configureBindings() { intakeDeployer.setDefaultCommand(new RunDeployer(intakeDeployer)); anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); - turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); + //turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem, shootState)); feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem)); } diff --git a/src/main/java/frc/robot/commands/turret/ManualTurretMove.java b/src/main/java/frc/robot/commands/turret/ManualTurretMove.java new file mode 100644 index 00000000..f7c956cf --- /dev/null +++ b/src/main/java/frc/robot/commands/turret/ManualTurretMove.java @@ -0,0 +1,37 @@ +package frc.robot.commands.turret; + +import java.util.function.DoubleSupplier; + +import frc.robot.constants.Constants; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class ManualTurretMove extends LoggableCommand { + private final TurretSubsystem subsystem; + private final DoubleSupplier input; + + + public ManualTurretMove(TurretSubsystem subsystem, DoubleSupplier input) { + this.subsystem = subsystem; + this.input = input; + addRequirements(subsystem); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + subsystem.manualMove(-input.getAsDouble()*Constants.MANUAL_MOVE_SCALING_FACTOR); + + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 17dfef6e..2b647ff2 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -121,8 +121,8 @@ public enum Mode { public static final double TURRET_LONG_RANGE_I = 0.000000; public static final double TURRET_LONG_RANGE_D = 0.0; public static final double TURRET_LONG_RANGE_FF = 0.0; - public static final double TURRET_ENCODER_MIN = 0; //Lowest encoder position of Turret - public static final double TURRET_ENCODER_MAX = 77; //Highest encoder position of Turret + public static final double TURRET_ENCODER_MIN = -10; //Lowest encoder position of Turret + public static final double TURRET_ENCODER_MAX = 10; //Highest encoder position of Turret public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward public static final double TURRET_MIN_ANGLE = -96; public static final double TURRET_MAX_ANGLE = 96; @@ -186,4 +186,6 @@ public enum Mode { public static final double SHUTTLING_TARGET_HIGHER_Y_POSITION = 5.5; public static final int TCP_SERVER_PORT1 = 5806; public static final int TCP_SERVER_PORT2 = 5807; + + public static final double MANUAL_MOVE_SCALING_FACTOR = 0.1; } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 2f30b0e8..47c862a8 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -27,7 +27,7 @@ public class TurretSubsystem extends SubsystemBase { private final SparkMaxPidMotorIo io; private final TunablePIDManager pidManager; private double lastAngle = 999; - + private double currentSetPosition = 0; public TurretSubsystem(SparkMaxPidMotorIo io) { this.io = io; this.pidManager = new TunablePIDManager(LOGGING_NAME, io, createPidConfig0()); @@ -71,6 +71,12 @@ public void setAngle(double targetAngle) { lastAngle = targetAngle; } } + public void manualMove(double input){ + if(((input > 0 && !isAtForwardLimit())||(input < 0 && !isAtReverseLimit())) && (currentSetPosition+input < Constants.TURRET_ENCODER_MAX) && (currentSetPosition+input > Constants.TURRET_ENCODER_MIN)){ + currentSetPosition += input; + } + setPosition(currentSetPosition); + } //Range translate code with a clamp public static double calculateRotationsForAngle(