From bf2e2e8e3c208e5327921b36b413a93398b58be1 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Fri, 1 Mar 2024 15:49:15 -0500 Subject: [PATCH] removed aimbot and updated logic on intakecommand --- .../frc/robot/commands/AimBotCommand.java | 39 -------- .../frc/robot/commands/DriveCommands.java | 6 +- .../subsystems/shooter/ShooterIOKraken.java | 20 +++- .../subsystems/shooter/ShooterSubsystem.java | 91 ++++++------------- 4 files changed, 46 insertions(+), 110 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/AimBotCommand.java diff --git a/src/main/java/frc/robot/commands/AimBotCommand.java b/src/main/java/frc/robot/commands/AimBotCommand.java deleted file mode 100644 index 2f477cba..00000000 --- a/src/main/java/frc/robot/commands/AimBotCommand.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.arm.ArmSubsystem; -import frc.robot.subsystems.drive.DriveSubsystem; - - -public class AimBotCommand extends Command { - private final ArmSubsystem m_armSubsystem; - private final DriveSubsystem m_driveSubsystem; - - public AimBotCommand(ArmSubsystem armSubsystem, DriveSubsystem driveSubsystem) { - this.m_armSubsystem = armSubsystem; - this.m_driveSubsystem = driveSubsystem; - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.m_armSubsystem, this.m_driveSubsystem); - } - - @Override - public void initialize() { - - } - - @Override - public void execute() { - - } - - @Override - public boolean isFinished() { - return true; - } - - @Override - public void end(boolean interrupted) { - m_driveSubsystem.stopWithX(); - } -} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 4222dcbe..1fd40e18 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -39,9 +39,9 @@ public static Command joystickDrive( DoubleSupplier omegaSupplier) { return Commands.run( () -> { - double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25); - double yInput = setSensitivity(ySupplier.getAsDouble(), 0.25); - double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.15); + double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25); + double yInput = setSensitivity(ySupplier.getAsDouble(), 0.25); + double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.15); // Apply deadband double linearMagnitude = diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java index a0246761..8303a776 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java @@ -13,7 +13,6 @@ import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; import lib.properties.phoenix6.PidPropertyPublic; import frc.robot.Constants.ShooterConstants; -import org.littletonrobotics.junction.Logger; public class ShooterIOKraken implements ShooterIO { private final TalonFX m_leftTalon; @@ -148,9 +147,22 @@ public ShooterIOKraken() { @Override public void updateInputs(ShooterIOInputsAutoLogged inputs) { BaseStatusSignal.refreshAll( - m_leftVelSignal, - m_rightVelSignal, - m_leftVoltOutSignal + m_leftVelSignal, + m_rightVelSignal, + m_leftVoltOutSignal, + m_rightVoltOutSignal, + m_kickerVoltOutSignal, + m_intakeVoltOutSignal, + m_indexerVoltOutSignal, + m_leftCurrentDrawSignal, + m_rightCurrentDrawSignal, + m_kickerCurrentDrawSignal, + m_intakeCurrentDrawSignal, + m_indexerCurrentDrawSignal, + m_leftTemperatureSignal, + m_rightTemperatureSignal, + m_intakeTemperatureSignal, + m_indexerTemperatureSignal ); inputs.tlVelocityRPM = m_leftVelSignal.getValueAsDouble() * 60.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index a60c5f90..cb5c3946 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -1,17 +1,11 @@ package frc.robot.subsystems.shooter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Voltage; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import lib.utils.AllianceFlipUtil; -import lib.utils.FieldConstants; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -20,7 +14,8 @@ import static edu.wpi.first.units.BaseUnits.Voltage; import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; public class ShooterSubsystem extends SubsystemBase { @@ -30,53 +25,12 @@ public class ShooterSubsystem extends SubsystemBase { private final LoggedDashboardNumber m_leftSetpoint; private final LoggedDashboardNumber m_rightSetpoint; - private final SysIdRoutine m_sysIdLeft; - private final SysIdRoutine m_sysIdRight; - public ShooterSubsystem(ShooterIO io) { m_io = io; m_inputs = new ShooterIOInputsAutoLogged(); m_leftSetpoint = new LoggedDashboardNumber("Shooter/Left Flywheel Setpoint RPM"); m_rightSetpoint = new LoggedDashboardNumber("Shooter/Right Flywheel Setpoint RPM"); - - if (m_io.getClass() == ShooterIOKraken.class || true) { - m_sysIdLeft = new SysIdRoutine( - new SysIdRoutine.Config(null, - Voltage.of(9), - null, - (SysIdRoutineLog.State state) -> SignalLogger.writeString("shooter-left-state", state.toString())), - new SysIdRoutine.Mechanism( - (Measure volt) -> setShooterPowerLeft(volt.in(Volts) / 12.0), - null, - this)); - m_sysIdRight = new SysIdRoutine( - new SysIdRoutine.Config(null, - Voltage.of(9), - null, - (SysIdRoutineLog.State state) -> SignalLogger.writeString("shooter-right-state", state.toString())), - new SysIdRoutine.Mechanism( - (Measure volt) -> setShooterPowerRight(volt.in(Volts) / 12.0), - null, - this)); - } else { - m_sysIdLeft = new SysIdRoutine( - new SysIdRoutine.Config(null, Voltage.of(9), null), - new SysIdRoutine.Mechanism( - (Measure volt) -> setShooterPowerLeft(volt.in(Volts) / 12.0), - (SysIdRoutineLog log) -> log.motor("shooter-left") - .voltage(mutable(Volts.of(m_inputs.tlAppliedVolts))) - .angularVelocity(mutable(RotationsPerSecond.of(m_inputs.tlVelocityRPM * 60.0))), - this)); - m_sysIdRight = new SysIdRoutine( - new SysIdRoutine.Config(null, Voltage.of(9), null), - new SysIdRoutine.Mechanism( - (Measure volt) -> setShooterPowerRight(volt.in(Volts) / 12.0), - (SysIdRoutineLog log) -> log.motor("shooter-right") - .voltage(mutable(Volts.of(m_inputs.trAppliedVolts))) - .angularVelocity(mutable(RotationsPerSecond.of(m_inputs.trVelocityRPM * 60.0))), - this)); - } } @Override @@ -114,6 +68,31 @@ public boolean hasPiece() { return m_inputs.tofDistanceIn < 60; } + /** Command Factories */ + + public Command intakeCommand(double intakePower, double kickerPower, double timeout) { + return (run(() -> { + setIntakePower(intakePower); + setKickerPower(kickerPower); + }).until(this::hasPiece) + .andThen(() -> { + setIntakePower(intakePower * 0.5); + setKickerPower(kickerPower); + }) + .withTimeout(timeout) + .andThen(() -> setKickerPower(kickerPower * -0.25)) + .withTimeout(timeout * 0.5) + .finallyDo(() -> { + setIntakePower(0.0); + setKickerPower(0.0); + }) + ) + .handleInterrupt(() -> { + setIntakePower(0.0); + setKickerPower(0.0); + }); + } + public Command setShooterPowerFactory(double left, double right, double intake) { return run(() -> { setShooterPowerLeft(left); @@ -122,21 +101,5 @@ public Command setShooterPowerFactory(double left, double right, double intake) setIntakePower(intake); }); } - - public Command sysIdQuasistatic(SysIdRoutine.Direction direction, BooleanSupplier isLeft) { - if (isLeft.getAsBoolean()) { - return m_sysIdLeft.quasistatic(direction); - } else { - return m_sysIdRight.quasistatic(direction); - } - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction, BooleanSupplier isLeft) { - if (isLeft.getAsBoolean()) { - return m_sysIdLeft.dynamic(direction); - } else { - return m_sysIdRight.dynamic(direction); - } - } }