From a7e9ef9951ec670befb35a1c32b2bd81638c4165 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Wed, 6 Mar 2024 16:29:42 -0700 Subject: [PATCH] florida comp --- build.gradle | 2 +- src/main/java/frc4388/robot/Constants.java | 12 ++- .../java/frc4388/robot/RobotContainer.java | 93 ++++++++++++++----- .../robot/commands/Autos/ArmIntakeInAuto.java | 2 +- .../commands/Autos/ArmIntakeInTimeout.java | 58 ++++++++++++ .../commands/Swerve/JoystickPlayback.java | 6 +- .../commands/Swerve/JoystickRecorder.java | 2 +- .../java/frc4388/robot/subsystems/Intake.java | 13 +++ .../frc4388/robot/subsystems/SwerveDrive.java | 46 ++++++++- src/main/java/frc4388/utility/RobotGyro.java | 24 +++++ 10 files changed, 225 insertions(+), 33 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java diff --git a/build.gradle b/build.gradle index e7eff7d..a794b07 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" } java { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7a6bb26..6d949fa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -25,21 +25,23 @@ import frc4388.utility.Gains; public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 1.5; + public static final double MAX_ROT_SPEED = 3.5; + public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 0.8; public static double ROTATION_SPEED = MAX_ROT_SPEED; + public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - public static final double SLOW_SPEED = 0.4; + public static final double SLOW_SPEED = 0.2; public static final double FAST_SPEED = 1.0; public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { public static final double FRONT_LEFT_ROT_OFFSET = 130.957 + 90; - public static final double FRONT_RIGHT_ROT_OFFSET = 77.783 + 90;//-202.588; + public static final double FRONT_RIGHT_ROT_OFFSET = 77.783 + 45 + 90 ;//-202.588; public static final double BACK_LEFT_ROT_OFFSET = 347.871 + 90; public static final double BACK_RIGHT_ROT_OFFSET = 52.646 + 90; } @@ -65,7 +67,7 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0, 0.0, 0, 1.0); + public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.1, 0.0, 0, 1.0); } public static final class AutoConstants { @@ -168,7 +170,7 @@ public final class Constants { public static final class ShooterConstants { public static final int LEFT_SHOOTER_ID = 15; // final public static final int RIGHT_SHOOTER_ID = 16; // final - public static final double SHOOTER_SPEED = 0.40; // final + public static final double SHOOTER_SPEED = 0.50; // final public static final double SHOOTER_IDLE = 0.35; // final public static final double SHOOTER_IDLE_LIMELIGHT = 0.8; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a82dec8..799debc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -32,6 +32,7 @@ import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Intake; +import frc4388.utility.DeferredBlock; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.VirtualController; import frc4388.utility.controller.XboxController; @@ -76,6 +77,7 @@ public class RobotContainer { // private final VirtualController m_virtualOperator = new VirtualController(1); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); + private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.talonPIDIn()) @@ -94,6 +96,8 @@ public class RobotContainer { // ); + // ! Teleop Commands + private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); private SequentialCommandGroup autoShoot = new SequentialCommandGroup( @@ -124,22 +128,33 @@ public class RobotContainer { new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); + private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( + interrupt, + new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); + + private SequentialCommandGroup ampShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.ampPosition()), + new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed + ); + // ! /* Autos */ private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand(); private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt"); private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); - private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); + //Help Simplify Shooting - private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); + // private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.talonPIDIn()), + // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + // new WaitCommand(1.4).asProxy(), + // new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + // new WaitCommand(0.5), + // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + // ); private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), @@ -170,11 +185,6 @@ public class RobotContainer { taxi.asProxy() ); - private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( - interrupt, - new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), @@ -184,7 +194,13 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - pullInArmtoShoot, + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") // new WaitCommand(1).asProxy(), // new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"), @@ -197,6 +213,23 @@ public class RobotContainer { // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); + private SequentialCommandGroup stayTwoNoteStartingFromSpeaker = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); + private SequentialCommandGroup threeNoteStartingFromSpeaker = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new WaitCommand(1).asProxy(), @@ -205,19 +238,32 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - pullInArmtoShoot, + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), //? Create Another Parallel Command Group :( - pullInArmtoShoot, + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") ); private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) .addOption("Taxi Auto", taxi.asProxy()) - .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker) - .addOption("One Note Auto Starting in Front of Speaker, But Stay", oneNoteStartingSpeakerStationary) - // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft) - // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight) - .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker) + .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) + .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) + // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) + // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) + .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) + .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) .buildDisplay(); @@ -228,6 +274,7 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); // configureVirtualButtonBindings(); + new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip()); DriverStation.silenceJoystickConnectionWarning(true); diff --git a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java index 40bc938..cb6ac90 100644 --- a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java +++ b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java @@ -26,6 +26,6 @@ public class ArmIntakeInAuto extends ParallelCommandGroup { this.intake = intake; this.shooter = shooter; this.swerve = swerve; - addCommands(new ArmIntakeIn(intake, shooter), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt"))); + addCommands((new ArmIntakeInTimeout(intake, shooter).withTimeout(3)), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt"))); } } diff --git a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java new file mode 100644 index 0000000..1e1e523 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.Autos; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.SwerveDrive; + +public class ArmIntakeInTimeout extends Command { + /** Creates a new ArmIntakeIn. */ + private Intake robotIntake; + private Shooter robotShooter; + + public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) { + // Use addRequirements() here to declare subsystem dependencies. + this.robotIntake = robotIntake; + this.robotShooter = robotShooter; + + addRequirements(this.robotIntake, this.robotShooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + robotIntake.talonPIDOut(); + robotIntake.talonSpinIntakeMotor(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if(interrupted) { + robotIntake.talonPIDIn(); + robotIntake.talonStopIntakeMotors(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return robotIntake.getTalonIntakeLimitSwitchState(); + // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) + // { + // return !true==true; + // } + // else + // { + // return !false==!(!(true)); + // } + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 4a4d7d8..e92b487 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -119,7 +119,11 @@ public class JoystickPlayback extends CommandBase { // new Translation2d(out.rightX, out.rightY), // true); - this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + // new Translation2d(lerpRX, lerpRY), + // true); + + this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), new Translation2d(lerpRX, lerpRY), true); diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index 7c83c58..b4907c5 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -64,7 +64,7 @@ public class JoystickRecorder extends CommandBase { outputs.add(inputs); - swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), + swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY), new Translation2d(inputs.rightX, inputs.rightY), true); diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 684fb8f..fff19b6 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -145,6 +145,10 @@ public class Intake extends SubsystemBase { talonIntake.set(IntakeConstants.INTAKE_SPEED); } + public void talonSpinIntakeMotor(double speed) { + talonIntake.set(speed); + } + public boolean getTalonIntakeLimitSwitchState() { if(r = talonIntake.getForwardLimit().getValue().value == 0) { return true; @@ -174,6 +178,15 @@ public class Intake extends SubsystemBase { } } + public void ampPosition() { + PositionVoltage request = new PositionVoltage(-0); + talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value + } + + public void ampShoot(double speed) { + talonSpinIntakeMotor(speed); + } + // ! NEO Methods //hanoff diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6ef7843..20b5964 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -33,7 +33,8 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default + public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public double rotTarget = 0.0; public Rotation2d orientRotTarget = new Rotation2d(); @@ -87,6 +88,41 @@ public class SwerveDrive extends SubsystemBase { setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } + public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (fieldRelative) { + + double rot = 0; + + // ! drift correction + if (rightStick.getNorm() > 0.05) { + rotTarget = gyro.getAngle(); + rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; + // SmartDashboard.putBoolean("drift correction", false); + stopped = false; + } else if(leftStick.getNorm() > 0.05) { + if (!stopped) { + stopModules(); + stopped = true; + } + + // SmartDashboard.putBoolean("drift correction", true); + + rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + + } + + // Use the left joystick to set speed. Apply a cubic curve and the set max speed. + Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); + // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + + // Convert field-relative speeds to robot-relative speeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + } + public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { Translation2d rightStick = new Translation2d(rightX, rightY); @@ -157,6 +193,11 @@ public class SwerveDrive extends SubsystemBase { return this.kinematics; } + public boolean getSpeedState() { + + return false; + } + @Override public void periodic() { // This method will be called once per scheduler run\ @@ -170,6 +211,7 @@ public class SwerveDrive extends SubsystemBase { this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; } else { this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } } @@ -220,4 +262,6 @@ public class SwerveDrive extends SubsystemBase { } } + + } diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 4d36404..09f8cf1 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -103,6 +103,7 @@ public class RobotGyro implements Gyro { } else { m_navX.reset(); } + } public void resetFlip() { @@ -113,6 +114,29 @@ public class RobotGyro implements Gyro { } else { m_navX.reset(); } + + } + + public void resetNinety() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(90); + } else { + m_navX.reset(); + } + + } + + public void resetTwoSeventy() { + resetZeroValues(); + + if (m_isGyroAPigeon) { + m_pigeon.setYaw(270); + } else { + m_navX.reset(); + } + } /**