diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5e81e40..3b3af33 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -164,6 +164,19 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); + + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveIntakeOrientation( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight() + + ); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); + })); } /** diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index a927fe6..249724c 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -234,6 +234,30 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rightStick.getAngle())); } + public void driveIntakeOrientation(Translation2d leftStick, Translation2d rightStick){ + // if (invert){ + // Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); + // driveFieldAngle(stick, heading); + + // } else{ + // driveFieldAngle(leftStick, heading); + // } + double speed = rightStick.getNorm(); + + if(speed < 0.3) { + driveWithInput(leftStick, new Translation2d(), true); + } else { + + + + Rotation2d heading = new Rotation2d(rightStick.getX(), rightStick.getY());//.r otateBy(Rotation2d.fromDegrees(90)); + + heading = heading.rotateBy(Rotation2d.fromDegrees(90)); + rotTarget = heading.getDegrees(); + + driveFieldAngle(leftStick, heading); + } + } public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {