diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index afdd483c..5a5283cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,10 +35,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; -import frc.robot.commands.DriveCommands; -import frc.robot.commands.FeedForwardCharacterization; -import frc.robot.commands.IntakeControlCommand; -import frc.robot.commands.ShooterAutoCommand; +import frc.robot.commands.*; import frc.robot.subsystems.arm.*; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOKraken; @@ -224,8 +221,13 @@ private void configureButtonBindings() { () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) ))); - ampLineupTrigger.whileTrue(new ScheduleCommand(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP)) + ampLineupTrigger.whileTrue(new alignmentDriveCommand(m_driveSubsystem, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX(), + () -> FieldConstants.AMP_LINEUP) .andThen(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); + ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), () -> m_shooter.setKickerPower(0.0), m_shooter) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index b4ae68db..555d427b 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -26,7 +26,7 @@ import java.util.function.Supplier; public class DriveCommands { - private static final double DEADBAND = 0.1; + public static final double DEADBAND = 0.1; private DriveCommands() {} diff --git a/src/main/java/frc/robot/commands/alignmentDriveCommand.java b/src/main/java/frc/robot/commands/alignmentDriveCommand.java new file mode 100644 index 00000000..e5d21049 --- /dev/null +++ b/src/main/java/frc/robot/commands/alignmentDriveCommand.java @@ -0,0 +1,116 @@ +package frc.robot.commands; + +import com.pathplanner.lib.commands.PathfindHolonomic; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.drive.DriveSubsystem; +import lib.utils.AllianceFlipUtil; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import static frc.robot.commands.DriveCommands.DEADBAND; + + +public class alignmentDriveCommand extends Command { + private final DriveSubsystem driveSubsystem; + private final Supplier pointSupplier; + private Command alignmentCommand; + + private final DoubleSupplier xSupplier; + private final DoubleSupplier ySupplier; + private final DoubleSupplier thetaSupplier; + + public alignmentDriveCommand(DriveSubsystem driveSubsystem, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier thetaSupplier, + Supplier point) { + this.driveSubsystem = driveSubsystem; + this.pointSupplier = point; + + + + this.xSupplier = xSupplier; + this.ySupplier =ySupplier; + this.thetaSupplier = thetaSupplier; + + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.driveSubsystem); + } + + @Override + public void initialize() { + Pose2d point = AllianceFlipUtil.apply(pointSupplier.get()); + alignmentCommand = new PathfindHolonomic( + point, + DriveConstants.DEFAULT_CONSTRAINTS, + 0.0, + driveSubsystem::getVisionPose, + driveSubsystem::getRobotRelativeSpeeds, + driveSubsystem::runVelocity, + DriveConstants.HOLONOMIC_CONFIG, + 0.0, + driveSubsystem + ); + } + + @Override + public void execute() { + double xInput = DriveCommands.setSensitivity(xSupplier.getAsDouble(), 0.25); + double yInput = DriveCommands.setSensitivity(ySupplier.getAsDouble(), 0.25); + double omegaInput = DriveCommands.setSensitivity(thetaSupplier.getAsDouble(), 0.0) * 0.85; + + // if we have actual driver input + if (xInput > DEADBAND + || yInput > DEADBAND + || omegaInput > DEADBAND) { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xInput, yInput), DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xInput, yInput); + double omega = MathUtil.applyDeadband(omegaInput, DEADBAND); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + Rotation2d heading; + + // if red change heading goal + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + heading = driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); + } else { + heading = driveSubsystem.getRotation(); + } + + // Convert to field relative speeds & send command + driveSubsystem.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * driveSubsystem.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * driveSubsystem.getMaxLinearSpeedMetersPerSec(), + omega * driveSubsystem.getMaxAngularSpeedRadPerSec(), + heading)); + } else { + alignmentCommand.execute(); + } + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 4600f977..69092632 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -395,4 +395,8 @@ public Command runSysidQuasistatic(SysIdRoutine.Direction direction) { public Command runSysidDynamic(SysIdRoutine.Direction direction) { return m_sysId.dynamic(direction); } - } + + public ChassisSpeeds getRobotRelativeSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } +}