From 890a3de02a0a775ea8b3c4776651fe7a00a9928b Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 16 Feb 2024 21:49:50 -0700 Subject: [PATCH 01/14] Lots of bs (im geekin) --- src/main/java/frc4388/robot/Constants.java | 13 +- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 47 ++++- .../robot/commands/Autos/PlaybackChooser.java | 8 +- .../frc4388/robot/subsystems/Apriltags.java | 36 ---- .../java/frc4388/robot/subsystems/Intake.java | 43 +++-- .../java/frc4388/robot/subsystems/LED.java | 2 +- .../frc4388/robot/subsystems/Limelight.java | 169 +++--------------- .../frc4388/robot/subsystems/Shooter.java | 27 ++- .../frc4388/robot/subsystems/SwerveDrive.java | 4 +- src/main/java/frc4388/utility/AprilTag.java | 13 -- 11 files changed, 135 insertions(+), 229 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/Apriltags.java delete mode 100644 src/main/java/frc4388/utility/AprilTag.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7b5ba85..6b9c00d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -161,12 +161,13 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_MOTOR_ID = 18; //TODO: - public static final int PIVOT_MOTOR_ID = 17; //TODO: - public static final double INTAKE_SPEED = 0.75; //TODO: - public static final double INTAKE_OUT_SPEED = 0.1; - public static final double HANDOFF_SPEED = 0.4; //TODO: - public static final double PIVOT_SPEED = 0.2; //TODO: + public static final int INTAKE_MOTOR_ID = 18; + public static final int PIVOT_MOTOR_ID = 17; + public static final double INTAKE_SPEED = 0.75; + public static final double INTAKE_OUT_SPEED_UNPRESSED = 0.7; + public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; + public static final double HANDOFF_SPEED = 0.4; + public static final double PIVOT_SPEED = 0.2; public static final class ArmPID { public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 44970b5..e5e850a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -38,7 +38,7 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - CameraServer.startAutomaticCapture(); + // CameraServer.startAutomaticCapture(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3fda500..4528086 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Intake.ArmIntakeIn; import frc4388.robot.commands.Intake.RotateIntakeToPosition; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Intake; @@ -61,11 +62,9 @@ public class RobotContainer { private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + private Limelight limelight = new Limelight(); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); - - private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.pidIn()), new InstantCommand(() -> m_robotShooter.spin()) @@ -96,13 +95,34 @@ public class RobotContainer { new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) ); - + /* Autos */ + private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + private Command startLeftMoveRight = new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt"); + private Command startRightMoveLeft = new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); - + private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( + ejectToShoot, + taxi + ); + private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( + startLeftMoveRight, + ejectToShoot, + taxi + ); + private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup( + startRightMoveLeft, + ejectToShoot, + taxi + ); private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - .addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + .addOption("Taxi Auto", taxi) + .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker) + .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft) + .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight) .buildDisplay(); + + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -122,7 +142,7 @@ public class RobotContainer { m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); + // SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); // m_robotIntake.resetPostion(); } @@ -177,10 +197,19 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + // Override Intake Position encoder: out + new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(-53), m_robotIntake)); + + // Override Intake Position encoder: out + new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(0), m_robotIntake)); + // //Pull arm in // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake)) @@ -202,7 +231,7 @@ public class RobotContainer { // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); //Spin Shooter Motors - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index 2438a38..a270d7b 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -66,7 +66,13 @@ public class PlaybackChooser { public void nextChooser() { SendableChooser chooser = m_choosers.get(m_cmdNum++); - for (String auto : m_dir.list()) { + String[] dirs = m_dir.list(); + + if(dirs == null){ // Fix funny error + return; + } + + for (String auto : dirs) { chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); } for (var cmd_name : m_commandPool.keySet()) { diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java deleted file mode 100644 index c6062e8..0000000 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc4388.robot.subsystems; - -//import edu.wpi.first.apriltag.AprilTag; -//import edu.wpi.first.math.geometry.Pose3d; -//import edu.wpi.first.math.geometry.Rotation3d; -//import edu.wpi.first.networktables.NetworkTable; -//import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Apriltags { - public static class Tag { - public boolean visible = true; - public double x, y, z = 0; - public double ry, rp, rr = 0; - } - - public Tag getTagPosRot() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - final Tag tag = new Tag(); - tag.visible = isAprilTag(); - tag.x = tagTable.getEntry("TagPosX").getDouble(0); - tag.y = tagTable.getEntry("TagPosY").getDouble(0); - tag.z = tagTable.getEntry("TagPosZ").getDouble(0); - tag.ry = tagTable.getEntry("TagRotY").getDouble(0); - tag.rp = tagTable.getEntry("TagRotP").getDouble(0); - tag.rr = tagTable.getEntry("TagRotR").getDouble(0); - - return tag; - } - - public boolean isAprilTag() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return tagTable.getEntry("IsTag").getBoolean(false); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 4bf31f6..6cbb9e2 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -35,6 +35,8 @@ public class Intake extends SubsystemBase { private BooleanSupplier sup = () -> true; private BooleanSupplier dup = () -> false; + + private double val; @@ -53,9 +55,6 @@ public class Intake extends SubsystemBase { reverseLimit.enableLimitSwitch(true); intakeMotor.restoreFactoryDefaults(); - intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - - intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); @@ -76,7 +75,6 @@ public class Intake extends SubsystemBase { //Rotate robot in for handoff public void rotateArmIn() { - //pivot.set(IntakeConstants.PIVOT_SPEED); pivot.set(IntakeConstants.PIVOT_SPEED); } @@ -90,6 +88,10 @@ public class Intake extends SubsystemBase { m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition); //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); } + + public void pidOut() { + m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); + } public void limitNote() { if (intakeforwardLimit.isPressed()) { @@ -99,10 +101,6 @@ public class Intake extends SubsystemBase { } } - public void pidOut() { - m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); - } - public void rotateArmOut2() { if(reverseLimit.isPressed()){ stopArmMotor(); @@ -120,7 +118,15 @@ public class Intake extends SubsystemBase { } public void handoff() { - intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED); + intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + } + + public void handoff2() { + if(intakeforwardLimit.isPressed()) { + intakeMotor.set(-val); + } else { + intakeMotor.set(-val); + } } public void stopIntakeMotors() { @@ -156,7 +162,11 @@ public class Intake extends SubsystemBase { } public void resetPostion() { - pivot.getEncoder().setPosition(0); + setPosition(0); + } + + public void setPosition(int val) { + pivot.getEncoder().setPosition(val); } public void resetPosition1() { @@ -169,6 +179,10 @@ public class Intake extends SubsystemBase { return pivot.getEncoder().getPosition(); } + public double getIntakeVelocity() { + return intakeMotor.getEncoder().getVelocity(); + } + public void rotateArm() { } @@ -181,11 +195,20 @@ public class Intake extends SubsystemBase { } } + public void changeIntakeNeutralState() { + if(forwardLimit.isPressed()) { + intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + } + } + @Override public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Vel Output", getVelocity()); SmartDashboard.putNumber("Position", getPos()); resetPosition1(); + changeIntakeNeutralState(); + + val = SmartDashboard.getNumber("Intake Speed", 0.5); } } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 85919b5..e9e070c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -33,7 +33,7 @@ public class LED extends SubsystemBase { if(m_self != null) return; m_led = new AddressableLED(9); - m_ledBuffer = new AddressableLEDBuffer(130); + m_ledBuffer = new AddressableLEDBuffer(10); m_led.setLength(m_ledBuffer.getLength()); m_led.setData(m_ledBuffer); m_led.start(); diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9d1289b..989b55b 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -3,163 +3,40 @@ // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; - -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; + +// Look at vvv for networktables stuff +// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data public class Limelight extends SubsystemBase { - - private PhotonCamera cam; - private PhotonPoseEstimator photonPoseEstimator; + private double[] cameraPose; + private Boolean isTag; - private boolean lightOn; - - /** Creates a new Limelight. */ - public Limelight() { - cam = new PhotonCamera(VisionConstants.NAME); - cam.setDriverMode(false); + public boolean getIsTag() { + return isTag; } - public void setLEDs(boolean on) { - lightOn = on; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } + public Pose2d getPose() { + //TODO - Get actual values! - public void toggleLEDs() { - lightOn = !lightOn; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } + double x = 0; + double y = 0; + double yaw = 0; - public void setDriverMode(boolean driverMode) { - cam.setDriverMode(driverMode); - } + Rotation2d rot = Rotation2d.fromDegrees(yaw); - public void setToLimePipeline() { - cam.setPipelineIndex(1); - setLEDs(true); - } - - public void setToAprilPipeline() { - cam.setPipelineIndex(0); - setLEDs(false); - } - - public PhotonTrackedTarget getAprilPoint() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - return result.getBestTarget(); - } - - private List getAprilCorners() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - return result.getBestTarget().getDetectedCorners(); - } - - public double getAprilSkew() { - List corners = getAprilCorners(); - ArrayList bottomSide = getAprilBottomSide(corners); - - if (bottomSide == null) return 0; - - TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1); - TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0); - - return bottomLeft.y - bottomRight.y; - } - - private ArrayList getAprilBottomSide(List box) { - if (box == null) return null; - - ArrayList bottomSide = new ArrayList<>(); - - TargetCorner l1 = new TargetCorner(-1, -1); - TargetCorner l2 = new TargetCorner(-1, -1); - - for (TargetCorner c : box) { - if (c.y > l1.y) l1 = c; - } - - for (TargetCorner c : box) { - if (c.y == l1.y) continue; - if (c.y > l2.y) l2 = c; - } - - bottomSide.add(l1); - bottomSide.add(l2); - - return bottomSide; - } - - public double getDistanceToApril() { - PhotonTrackedTarget aprilPoint = getAprilPoint(); - if (aprilPoint == null) return -1; - - double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + aprilPoint.getPitch(); - - double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); - return distanceToApril; - } - - public PhotonTrackedTarget getLowestTape() { - if (!cam.isConnected()) return null; - - PhotonPipelineResult result = cam.getLatestResult(); - - if (!result.hasTargets()) return null; - - ArrayList points = (ArrayList) result.getTargets(); - - PhotonTrackedTarget lowest = points.get(0); - for (PhotonTrackedTarget point : points) { - if (point.getPitch() < lowest.getPitch()) { - lowest = point; - } - } - - return lowest; - } - - public double getDistanceToTape() { - PhotonTrackedTarget tapePoint = getLowestTape(); - if (tapePoint == null) return -1; - - double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; - double theta = 35.0 + tapePoint.getPitch(); - - double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); - return distanceToTape; + return new Pose2d(x, y, rot); } @Override - public void periodic() {} -} + public void periodic() { + // This method will be called once per scheduler run + + isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false); + cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 8d340ed..6339fb2 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; @@ -29,13 +30,31 @@ public class Shooter extends SubsystemBase { rightShooter.setNeutralMode(NeutralModeValue.Coast); } + public Shooter(TalonFX leftShooter) { + this.leftShooter = leftShooter; + leftShooter.setNeutralMode(NeutralModeValue.Coast); + } + + public void singleSpin() { + leftShooter.set(1.0); + } + + public void singleSpin(double speed) { + leftShooter.set(speed); + } + public void spin() { spin(ShooterConstants.SHOOTER_SPEED); } public void spin(double speed) { - leftShooter.set(-speed); - rightShooter.set(speed); + leftShooter.set(speed); + rightShooter.set(-speed); + } + + public void spin(double leftSpeed, double rightSpeed) { + leftShooter.set(leftSpeed); + rightShooter.set(-rightSpeed); } public void stop() { @@ -49,8 +68,8 @@ public class Shooter extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); - SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); + // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); + //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 86dd04b..8cb65d9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -58,7 +58,7 @@ public class SwerveDrive extends SubsystemBase { if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - SmartDashboard.putBoolean("drift correction", false); + // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { if (!stopped) { @@ -66,7 +66,7 @@ public class SwerveDrive extends SubsystemBase { stopped = true; } - SmartDashboard.putBoolean("drift correction", true); + // SmartDashboard.putBoolean("drift correction", true); rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java deleted file mode 100644 index c2ad269..0000000 --- a/src/main/java/frc4388/utility/AprilTag.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc4388.utility; - -// This is a seperate class in case I want to encode rotation or other -// information about the tag -public class AprilTag { - public final double x, y, z; - - public AprilTag(double _x, double _y, double _z) { - x = _x; - y = _y; - z = _z; - } -} From cb35732f2b3ea03cab420fb15c12320c03a4ae2e Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 17 Feb 2024 10:16:20 -0700 Subject: [PATCH 02/14] Clean up robot container and a some of intake code --- .gitignore | 2 + .../java/frc4388/robot/RobotContainer.java | 79 +++---------------- .../Intake/RotateIntakeToPosition.java | 38 --------- .../java/frc4388/robot/subsystems/Intake.java | 25 +++--- 4 files changed, 24 insertions(+), 120 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java diff --git a/.gitignore b/.gitignore index 5528d4f..b5b18bb 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,5 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ +simgui.json +simgui-ds.json diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4528086..107e314 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,10 +7,7 @@ package frc4388.robot; -import java.time.Instant; - import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -23,7 +20,6 @@ import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Intake.ArmIntakeIn; -import frc4388.robot.commands.Intake.RotateIntakeToPosition; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; @@ -70,17 +66,6 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.spin()) ); - private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin()), - new InstantCommand(() -> m_robotIntake.handoff()) - ); - - private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake), - new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) - ); - private SequentialCommandGroup i = new SequentialCommandGroup( intakeToShootStuff, intakeToShoot ); @@ -101,22 +86,22 @@ public class RobotContainer { private Command startRightMoveLeft = new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( - ejectToShoot, - taxi + ejectToShoot.asProxy(), + taxi.asProxy() ); private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( - startLeftMoveRight, - ejectToShoot, - taxi + startLeftMoveRight.asProxy(), + ejectToShoot.asProxy(), + taxi.asProxy() ); private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup( - startRightMoveLeft, - ejectToShoot, - taxi + startRightMoveLeft.asProxy(), + ejectToShoot.asProxy(), + taxi.asProxy() ); private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - .addOption("Taxi Auto", taxi) + .addOption("Taxi Auto", taxi.asProxy()) .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker) .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft) .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight) @@ -141,10 +126,6 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - - // SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); - - // m_robotIntake.resetPostion(); } /** @@ -185,13 +166,6 @@ public class RobotContainer { /* Operator Buttons */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter)); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) @@ -204,46 +178,17 @@ public class RobotContainer { // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(-53), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); - // Override Intake Position encoder: out + // Override Intake Position encoder: in new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(0), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); - // //Pull arm in - // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); - - // //Pull arm out - // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); - - // //Intake Note - // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - - // //Outtake Note - // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - //Spin Shooter Motors new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); - // //Intake Note and ramp up shooter to 40% - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(intakeToShoot); - - // //Ramps up shooter to 100% to Shooter - // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - // .onTrue(outtakeToShootFull); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(ejectToShoot) .onFalse(turnOffShoot); diff --git a/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java b/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java deleted file mode 100644 index 2dd7a55..0000000 --- a/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java +++ /dev/null @@ -1,38 +0,0 @@ -// 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.Intake; - - -import com.revrobotics.CANSparkMax; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.SwerveDrive; -import edu.wpi.first.wpilibj.motorcontrol.Spark; - -import frc4388.robot.subsystems.Intake; -public class RotateIntakeToPosition extends PID { - - Intake intake; - double targetAngle; - - /** Creates a new PIDSparkMax. */ - public RotateIntakeToPosition(Intake intake, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.intake = intake; - this.targetAngle = targetAngle; - - addRequirements(intake); - } - - @Override - public double getError() { - return targetAngle - (((intake.getEncoder().getPosition()) * (360))%360); - } - - @Override - public void runWithOutput(double output) { - intake.setVoltage(output / Math.abs(getError())); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 6cbb9e2..b10ff7d 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -36,10 +36,7 @@ public class Intake extends SubsystemBase { private BooleanSupplier sup = () -> true; private BooleanSupplier dup = () -> false; - private double val; - - - + private double smartDashboardOuttakeValue; /** Creates a new Intake. */ public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { @@ -66,6 +63,8 @@ public class Intake extends SubsystemBase { m_spedController.setP(armGains.kP); m_spedController.setI(armGains.kI); m_spedController.setD(armGains.kD); + + SmartDashboard.putNumber("Intake Speed", 0.5); } //hanoff @@ -123,9 +122,9 @@ public class Intake extends SubsystemBase { public void handoff2() { if(intakeforwardLimit.isPressed()) { - intakeMotor.set(-val); + intakeMotor.set(-smartDashboardOuttakeValue); } else { - intakeMotor.set(-val); + intakeMotor.set(-smartDashboardOuttakeValue); } } @@ -161,17 +160,13 @@ public class Intake extends SubsystemBase { return pivot.getEncoder().getVelocity(); } - public void resetPostion() { - setPosition(0); - } - - public void setPosition(int val) { + public void setPivotEncoderPosition(int val) { pivot.getEncoder().setPosition(val); } - public void resetPosition1() { + public void resetPosition() { if(forwardLimit.isPressed()) { - resetPostion(); + setPivotEncoderPosition(0); } } @@ -206,9 +201,9 @@ public class Intake extends SubsystemBase { // This method will be called once per scheduler run SmartDashboard.putNumber("Vel Output", getVelocity()); SmartDashboard.putNumber("Position", getPos()); - resetPosition1(); + resetPosition(); changeIntakeNeutralState(); - val = SmartDashboard.getNumber("Intake Speed", 0.5); + smartDashboardOuttakeValue = SmartDashboard.getNumber("Intake Speed", 0.5); } } From 5482cd7fb985d265a2c41fa6104aac06f55a7cc3 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 17 Feb 2024 11:06:50 -0700 Subject: [PATCH 03/14] no more joystick warning --- src/main/java/frc4388/robot/RobotContainer.java | 3 +++ src/main/java/frc4388/robot/subsystems/Intake.java | 8 +++----- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 107e314..fb14d47 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,7 @@ package frc4388.robot; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; @@ -115,6 +116,8 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); + DriverStation.silenceJoystickConnectionWarning(true); + /* Default Commands */ // drives the robot with a two-axis input from the driver controller m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index b10ff7d..503db7c 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -30,9 +30,7 @@ public class Intake extends SubsystemBase { private SparkLimitSwitch reverseLimit; private SparkLimitSwitch intakeforwardLimit; private SparkLimitSwitch intakereverseLimit; - - private Shooter shooter; - + private BooleanSupplier sup = () -> true; private BooleanSupplier dup = () -> false; @@ -64,7 +62,7 @@ public class Intake extends SubsystemBase { m_spedController.setI(armGains.kI); m_spedController.setD(armGains.kD); - SmartDashboard.putNumber("Intake Speed", 0.5); + SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } //hanoff @@ -204,6 +202,6 @@ public class Intake extends SubsystemBase { resetPosition(); changeIntakeNeutralState(); - smartDashboardOuttakeValue = SmartDashboard.getNumber("Intake Speed", 0.5); + smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } } From 891237a728e92ab4daf523aa145f44a6e5f864a0 Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Sat, 17 Feb 2024 20:41:46 -0700 Subject: [PATCH 04/14] Configurables --- .../configurable/ConfigurableDouble.java | 23 +++++++++++++++++++ .../configurable/ConfigurableString.java | 23 +++++++++++++++++++ 2 files changed, 46 insertions(+) create mode 100644 src/main/java/frc4388/utility/configurable/ConfigurableDouble.java create mode 100644 src/main/java/frc4388/utility/configurable/ConfigurableString.java diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java new file mode 100644 index 0000000..c0384db --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -0,0 +1,23 @@ +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ConfigurableDouble { + private double defualtValue; + private String name; + + /** + * Creates an new ConfigurableDouble through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableDouble(String name, double defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putNumber(name, defualtValue); + } + + public double get() { + return SmartDashboard.getNumber(name, defualtValue); + } +} diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java new file mode 100644 index 0000000..34c0290 --- /dev/null +++ b/src/main/java/frc4388/utility/configurable/ConfigurableString.java @@ -0,0 +1,23 @@ +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class ConfigurableString { + private String defualtValue; + private String name; + + /** + * Creates an new ConfigurableString through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableString(String name, String defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putString(name, defualtValue); + } + + public String get() { + return SmartDashboard.getString(name, defualtValue); + } +} From 2660adcd68827f910f880545711db179b86eb519 Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Sun, 18 Feb 2024 18:55:02 -0700 Subject: [PATCH 05/14] commit Neo AutoRecording code, needs polish. --- sample.auto | Bin 0 -> 5297 bytes src/main/java/frc4388/robot/Robot.java | 12 +- .../java/frc4388/robot/RobotContainer.java | 129 +++++++++++----- .../Autos/neo AutoRecoding format.txt | 20 +++ .../commands/Swerve/JoystickPlayback.java | 2 +- .../commands/Swerve/neoJoystickPlayback.java | 145 ++++++++++++++++++ .../commands/Swerve/neoJoystickRecorder.java | 105 +++++++++++++ src/main/java/frc4388/utility/DataUtils.java | 35 +++++ .../java/frc4388/utility/UtilityStructs.java | 10 ++ .../utility/controller/VirtualController.java | 110 +++++++++++++ 10 files changed, 520 insertions(+), 48 deletions(-) create mode 100644 sample.auto create mode 100644 src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt create mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java create mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java create mode 100644 src/main/java/frc4388/utility/DataUtils.java create mode 100644 src/main/java/frc4388/utility/controller/VirtualController.java diff --git a/sample.auto b/sample.auto new file mode 100644 index 0000000000000000000000000000000000000000..6b4c98bedd1465c081dafb6a031a222f595612e6 GIT binary patch literal 5297 zcmc)Ktqa0H7=ZEJ1|nGeSQuz98Vrh6un1PcVpua+1gl`M7!3x&Di{n#gTb(2u?YSF z7K2@bV4Sj m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> getVirtualDriverController().setOutput(XboxController.A_BUTTON, true))) + // .onFalse(new InstantCommand(() -> getVirtualDriverController().setOutput(XboxController.A_BUTTON, false))); + // new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) + // //.onTrue(new InstantCommand(() -> System.out.println("Hello"))); + // .onTrue(new InstantCommand(() -> getVirtualDriverController().setRumble(RumbleType.kBothRumble, 1.0))) + // .onFalse(new InstantCommand(() -> getVirtualDriverController().setRumble(RumbleType.kBothRumble, 0.0))); /* Auto Recording */ // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, @@ -154,52 +165,79 @@ public class RobotContainer { // "IntenseTaxi.txt")) // .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + // .onFalse(new InstantCommand()); + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + "sample.auto")) + .onFalse(new InstantCommand()); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + "sample.auto", + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + true, false)) + .onFalse(new InstantCommand()); + + new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> System.out.println("Pressed A"))) + .onFalse(new InstantCommand(() -> System.out.println("Released A"))); + + new JoystickButton(getVirtualDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> System.out.println("Pressed B"))) + .onFalse(new InstantCommand(() -> System.out.println("Released B"))); + + new JoystickButton(getVirtualDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> System.out.println("Pressed X"))) + .onFalse(new InstantCommand(() -> System.out.println("Released X"))); + + new JoystickButton(getVirtualDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> System.out.println("Pressed Y"))) + .onFalse(new InstantCommand(() -> System.out.println("Released Y"))); /* Speed */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); /* Operator Buttons */ - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - // Override Intake Position encoder: out - new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); + // // Override Intake Position encoder: out + // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); - // Override Intake Position encoder: in - new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); + // // Override Intake Position encoder: in + // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); - //Spin Shooter Motors - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) - .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); + // //Spin Shooter Motors + // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) + // .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(ejectToShoot) - .onFalse(turnOffShoot); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(ejectToShoot) + // .onFalse(turnOffShoot); - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(i) - .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(i) + // .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); } @@ -210,7 +248,8 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - //no auto + //no auto + return playbackChooser.getCommand(); } @@ -224,4 +263,12 @@ public class RobotContainer { public DeadbandedXboxController getDeadbandedOperatorController() { return this.m_operatorXbox; } + + public VirtualController getVirtualDriverController() { + return m_virtualDriver; + } + + public VirtualController getVirtualOperatorController() { + return m_virtualOperator; + } } diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt new file mode 100644 index 0000000..a65aea9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt @@ -0,0 +1,20 @@ +AUTO file format + +HEADER static size 0x5 +0x00 BYTE NUM AXES: defualts to 6 +0x01 BYTE NUM POV: defualts to 1 +0x02 BYTE NUM CONTROLLERS: defualts to 2 +0x03 SHORT FRAMES: any value greator or equal than one. + +FRAME PER CONTROLLER: defualt size 0x34 +0x00 DOUBLE AXES[NUM AXES] +0x30 SHORT BUTTONS +0x32 SHORT POVs[NUM POV] + +FRAME: size varrys +FRAME PER CONTROLLER[NUM CONTROLLERS] +INT UNIXTIMESTAMP + +FILE: +HEADER +FRAME[FRAMES] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index 3752550..4a91f7a 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -102,7 +102,7 @@ public class JoystickPlayback extends Command { return; } lastIndex = i; - } + } // Why is this done rather than using the variable counter TimedOutput lastOut = outputs.get(lastIndex - 1); TimedOutput out = outputs.get(lastIndex); diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java new file mode 100644 index 0000000..529bd34 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -0,0 +1,145 @@ +package frc4388.robot.commands.Swerve; + +import java.io.FileInputStream; +import java.util.ArrayList; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.DataUtils; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.VirtualController; + +public class neoJoystickPlayback extends Command { + private final SwerveDrive swerve; + private final String filename; + private final VirtualController[] controllers; + private final ArrayList frames = new ArrayList<>(); + private int frame_index = 0; + private long startTime = 0; + private long playbackTime = 0; + private boolean m_finished = false; // ! There is no better way. + private boolean m_shouldfree = false; // should free memory on ending + + private byte m_numAxes = 0; + private byte m_numPOVs = 0; + private byte m_numControllers = 0; + private short m_numFrames = -1; + + public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { + this.swerve = swerve; + this.filename = filename; + this.controllers = controllers; + this.m_shouldfree = shouldfree; + + if (instantload) loadAuto(); + addRequirements(this.swerve); + } + + public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { + this(swerve, filename, controllers, true, false); + } + + public boolean loadAuto() { + try (FileInputStream stream = new FileInputStream("./" + filename)) { + if (m_numFrames != -1 && m_numFrames == frames.size()) { + System.out.println("AUTOPLAYBACK: Auto Already loaded."); + return true; + } + + m_numAxes = stream.readNBytes(1)[0]; + m_numPOVs = stream.readNBytes(1)[0]; + m_numControllers = stream.readNBytes(1)[0]; + m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); + + if (m_numControllers > controllers.length) { + System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers + + " virtual controllers but only " + controllers.length + " were given"); + return false; + } + + for (int i = 0; i < m_numFrames; i++) { + AutoRecordingFrame frame = new AutoRecordingFrame(); + for (int j = 0; j < m_numControllers; j++) { + AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); + double[] axes = new double[m_numAxes]; + for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. + axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + } + short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); + short[] POV = new short[m_numPOVs]; + for (int k = 0; k < m_numPOVs; k++) { + POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); + } + controllerFrame.axes = axes; + controllerFrame.button = button; + controllerFrame.POV = POV; + frame.controllerFrames[j] = controllerFrame; + } + frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); + frames.add(frame); + } + + System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); + return true; + + } catch (Exception e) { + e.printStackTrace(); + System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); + return false; + } + } + + public void unloadAuto() { + System.out.println("AUTOPLAYBACK: Auto unloaded"); + frames.clear(); + } + + @Override + public void initialize() { + startTime = System.currentTimeMillis(); + playbackTime = 0; + frame_index = 0; + + m_finished = !loadAuto(); + } + + @Override + public void execute() { + if (frame_index >= m_numFrames) m_finished = true; + if (m_finished) return; + + // if (frame_index == 0) { + // startTime = System.currentTimeMillis(); + // playbackTime = 0; + // } else { + // playbackTime = System.currentTimeMillis() - startTime; + // } + + AutoRecordingFrame frame = frames.get(frame_index); + for (int i = 0; i < controllers.length; i++) { + AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; + controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); + if (i == 0) { + this.swerve.driveWithInput( + new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), + new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), + true); + } + } + frame_index++; + } + + @Override + public void end(boolean interrupted) { + for (VirtualController controller : controllers) controller.zeroControls(); + swerve.stopModules(); + if (m_shouldfree) frames.clear(); + } + + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java new file mode 100644 index 0000000..7cae811 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -0,0 +1,105 @@ +package frc4388.robot.commands.Swerve; + +import java.io.FileOutputStream; +import java.util.ArrayList; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.DataUtils; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.DeadbandedXboxController; + +public class neoJoystickRecorder extends Command { + private final SwerveDrive swerve; + private final XboxController[] controllers; + private final String filename; + private long startTime = -1; + private final ArrayList frames = new ArrayList<>(); + + public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { + this.swerve = swerve; + this.controllers = controllers; + this.filename = filename; + + addRequirements(this.swerve); + } + + @Override + public void initialize() { + frames.clear(); + + this.startTime = System.currentTimeMillis(); + AutoRecordingFrame frame = new AutoRecordingFrame(); + frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; + frames.add(frame); + } + + + @Override + public void execute() { + System.out.println("AUTORECORD: RECORDING"); + AutoRecordingFrame frame = new AutoRecordingFrame(); + frame.timeStamp = (int) (System.currentTimeMillis() - startTime); + for (int i = 0; i < controllers.length; i++) { + XboxController controller = controllers[i]; + AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); + double[] axes = {controller.getLeftX(), controller.getLeftY(), + controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), + controller.getRightX(), controller.getRightY()}; + short button = 0; + for (int j = 0; j < 10; j++) + if (controller.getRawButton(j+1)) + button |= 1 << j; + short[] POV = {(short)(controller.getPOV())}; + controllerFrame.axes = axes; + controllerFrame.button = button; + controllerFrame.POV = POV; + frame.controllerFrames[i] = controllerFrame; + } + + frames.add(frame); + + swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), + new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), + true); // Really jank way of doing this. + + } + @Override + public void end(boolean interrupted) { + try (FileOutputStream stream = new FileOutputStream("./" + filename)) { + // header: size of 0x5 + // byte Number of axes per controller + // byte Number of POVs per controller + // byte Number of controllers + // short Number of frames + stream.write(new byte[]{6, 1, (byte) controllers.length}); + stream.write(DataUtils.shortToByteArray((short) frames.size())); + + // frame + // controller frame * number of controllers + // int unix time stamp. + for (AutoRecordingFrame frame : frames) { + // controller frame + // double axis * Number of axes per controller + // short button states + // short POV * Number of POVs per controller + for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { + for (double axis: controllerFrame.axes) { + stream.write(DataUtils.doubleToByteArray(axis)); + } + stream.write(DataUtils.shortToByteArray(controllerFrame.button)); + for (short POV: controllerFrame.POV) { + stream.write(DataUtils.shortToByteArray(POV)); + } + } + stream.write(DataUtils.intToByteArray(frame.timeStamp)); + } + System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); + } catch (Exception e) { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/frc4388/utility/DataUtils.java b/src/main/java/frc4388/utility/DataUtils.java new file mode 100644 index 0000000..3d998b6 --- /dev/null +++ b/src/main/java/frc4388/utility/DataUtils.java @@ -0,0 +1,35 @@ +package frc4388.utility; + +import java.nio.ByteBuffer; + +public class DataUtils { + public static byte[] doubleToByteArray(double value) { + byte[] bytes = new byte[8]; + ByteBuffer.wrap(bytes).putDouble(value); + return bytes; + } + + public static double byteArrayToDouble(byte[] bytes) { + return ByteBuffer.wrap(bytes).getDouble(); + } + + public static byte[] intToByteArray(int value) { + byte[] bytes = new byte[4]; + ByteBuffer.wrap(bytes).putInt(value); + return bytes; + } + + public static int byteArrayToInt(byte[] bytes) { + return ByteBuffer.wrap(bytes).getInt(); + } + + public static byte[] shortToByteArray(short value) { + byte[] bytes = new byte[2]; + ByteBuffer.wrap(bytes).putShort(value); + return bytes; + } + + public static short byteArrayToShort(byte[] bytes) { + return ByteBuffer.wrap(bytes).getShort(); + } +} diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java index e8b10cc..4123ccb 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -9,4 +9,14 @@ public class UtilityStructs { public long timedOffset = 0; } + public static class AutoRecordingControllerFrame { + public double[] axes = new double[6]; + public short button = 0; + public short[] POV = new short[1]; + + } + public static class AutoRecordingFrame { + public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2]; + public int timeStamp; + } } diff --git a/src/main/java/frc4388/utility/controller/VirtualController.java b/src/main/java/frc4388/utility/controller/VirtualController.java new file mode 100644 index 0000000..12e98cf --- /dev/null +++ b/src/main/java/frc4388/utility/controller/VirtualController.java @@ -0,0 +1,110 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +public class VirtualController extends GenericHID { + private short m_buttonStates = 0; + private short m_buttonStatesLastFrame = 0; + private double[] m_axes = new double[6]; + private short[] m_pov = new short[1]; + + public VirtualController(int port) { + super(port); + } + + public void setFrame(double[] axes, short buttonFlags, short[] pov) { + m_axes = axes; + setOutputs(buttonFlags); + m_pov = pov; + } + + public void zeroControls() { + m_axes = new double[6]; + m_buttonStates = 0; + m_buttonStatesLastFrame = 0; + m_pov = new short[1]; + } + + public static boolean getFlag(int value, int index) { + return ((value & 1 << index) != 0); + } + + @Override + public boolean getRawButton(int button) { // man why are buttons indexed at 1. + return getFlag(m_buttonStates, button - 1); + } + + @Override + public boolean getRawButtonPressed(int button) { + return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button)); + } + + @Override + public boolean getRawButtonReleased(int button) { + return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button)); + } + + @Override + public double getRawAxis(int axis) { + return m_axes[axis]; + } + + @Override + public int getPOV(int pov) { + return m_pov[pov]; + } + + @Override + public int getAxisCount() { + return m_axes.length; + } + + @Override + public int getPOVCount() { + return m_pov.length; + } + + @Override + public int getButtonCount() { + return 10; + } + + @Override + public boolean isConnected() { + return true; + } + + @Override + public HIDType getType() { + return HIDType.kXInputGamepad; + } + + @Override + public String getName() { + return "Virtual Controller"; + } + + @Override + public int getAxisType(int axis) { + return 1; /* ! Warning, does not return accurate data. + Hopefully this isn't a problem */ + } + + @Override + public void setOutput(int outputNumber, boolean value) { + // do not use + //m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1]; + //m_buttonStates[outputNumber - 1] = value; + } + + @Override + public void setOutputs(int value) { + m_buttonStatesLastFrame = m_buttonStates; + m_buttonStates = (short) value; + } + + @Override + public void setRumble(RumbleType type, double value) { + System.out.println("Why are you Setting rumble on an virtual controller?"); + } +} \ No newline at end of file From 1032edbf4c4db40053a3b911b9978402c0c6d3c0 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Mon, 19 Feb 2024 09:41:58 -0700 Subject: [PATCH 06/14] last sataurday's changes --- src/main/java/frc4388/robot/Constants.java | 4 +- src/main/java/frc4388/robot/Robot.java | 25 +++--- .../java/frc4388/robot/RobotContainer.java | 82 +++++++++++++++---- .../robot/commands/Autos/PlaybackChooser.java | 10 +-- .../commands/Swerve/JoystickRecorder.java | 52 +++++++++++- .../frc4388/robot/subsystems/Shooter.java | 12 ++- .../utility/Configurable/Configurable.java | 5 ++ .../Configurable/ConfigurableDouble.java | 4 + .../java/frc4388/utility/UtilityStructs.java | 4 + 9 files changed, 156 insertions(+), 42 deletions(-) create mode 100644 src/main/java/frc4388/utility/Configurable/Configurable.java create mode 100644 src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6b9c00d..082c376 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -155,7 +155,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 = 1.0; // final + public static final double SHOOTER_SPEED = 0.35; // final public static final double SHOOTER_IDLE = 0.4; // final } @@ -164,7 +164,7 @@ public final class Constants { public static final int INTAKE_MOTOR_ID = 18; public static final int PIVOT_MOTOR_ID = 17; public static final double INTAKE_SPEED = 0.75; - public static final double INTAKE_OUT_SPEED_UNPRESSED = 0.7; + public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0; public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; public static final double HANDOFF_SPEED = 0.4; public static final double PIVOT_SPEED = 0.2; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e5e850a..d664129 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -38,7 +38,6 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - // CameraServer.startAutomaticCapture(); } /** @@ -84,20 +83,20 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { - // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // /* - // * String autoSelected = SmartDashboard.getString("Auto Selector", - // * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - // * = new MyAutoCommand(); break; case "Default Auto": default: - // * autonomousCommand = new ExampleCommand(); break; } - // */ + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ - // // schedule the autonomous command (example) - // if (m_autonomousCommand != null) { - // m_autonomousCommand.schedule(); - // } - // m_robotTime.startMatchTime(); + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + m_robotTime.startMatchTime(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fb14d47..51c0419 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,9 @@ package frc4388.robot; +import com.fasterxml.jackson.databind.ser.std.InetAddressSerializer; + +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; @@ -14,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; @@ -87,8 +91,22 @@ public class RobotContainer { private Command startRightMoveLeft = new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( - ejectToShoot.asProxy(), - taxi.asProxy() + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake), + new WaitCommand(1).asProxy(), + new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") + ); + private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup ( + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) ); private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( startLeftMoveRight.asProxy(), @@ -100,10 +118,31 @@ public class RobotContainer { ejectToShoot.asProxy(), taxi.asProxy() ); - + private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake), + intakeToShootStuff.asProxy(), + new WaitCommand(1).asProxy(), + new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt1.txt"), + intakeToShoot.asProxy(), + new WaitCommand(1).asProxy(), + new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"), + new WaitCommand(0.5).asProxy(), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) + ); 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) .buildDisplay(); @@ -117,6 +156,7 @@ public class RobotContainer { configureButtonBindings(); DriverStation.silenceJoystickConnectionWarning(true); + CameraServer.startAutomaticCapture(); /* Default Commands */ // drives the robot with a two-axis input from the driver controller @@ -145,26 +185,28 @@ public class RobotContainer { /* Auto Recording */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "IntenseTaxi.txt")) - // .onFalse(new InstantCommand()); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + () -> getDeadbandedDriverController().getLeftX(), + () -> getDeadbandedDriverController().getLeftY(), + () -> getDeadbandedDriverController().getRightX(), + () -> getDeadbandedDriverController().getRightY(), + () -> getDeadbandedOperatorController().getLeftBumper(), + () -> getDeadbandedOperatorController().getRightBumper(), + "TwoNotePrt1.txt")) + .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) .onFalse(new InstantCommand()); - /* Speed */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + // /* Speed */ + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); @@ -178,6 +220,10 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); + // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index a270d7b..49ffcaf 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -42,6 +42,7 @@ public class PlaybackChooser { m_playback = m_choosers.get(0); nextChooser(); + // ! This does not work, why? Shuffleboard.getTab("Auto Chooser") .add("Add Sequence", new InstantCommand(() -> nextChooser())) .withPosition(4, 0); @@ -68,13 +69,12 @@ public class PlaybackChooser { String[] dirs = m_dir.list(); - if(dirs == null){ // Fix funny error - return; + if(dirs != null){ // Fix funny error + for (String auto : dirs) { + chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } } - for (String auto : dirs) { - chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); - } for (var cmd_name : m_commandPool.keySet()) { chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); } diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index 620dd86..56275b8 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -12,33 +12,66 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.Intake.ArmIntakeIn; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; public class JoystickRecorder extends Command { public final SwerveDrive swerve; + public final Shooter m_robotShooter; + public final Intake m_robotIntake; public final Supplier leftX; public final Supplier leftY; public final Supplier rightX; public final Supplier rightY; + public final Supplier OPLB; + public final Supplier OPRB; + + private Command intakeToShootStuff; + private Command intakeToShoot; + private Command i; + + private boolean lastOPLB; + private boolean lastOPRB; + private String filename; public final ArrayList outputs = new ArrayList<>(); private long startTime = -1; /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, + public JoystickRecorder(SwerveDrive swerve, Shooter m_robotShooter, Intake m_robotIntake, + Supplier leftX, Supplier leftY, Supplier rightX, Supplier rightY, + Supplier OPLB, Supplier OPRB, String filename) { this.swerve = swerve; + this.m_robotShooter = m_robotShooter; + this.m_robotIntake = m_robotIntake; + this.leftX = leftX; this.leftY = leftY; this.rightX = rightX; this.rightY = rightY; + this.OPLB = OPLB; + this.OPRB = OPRB; this.filename = filename; + intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); + intakeToShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.pidIn()), + new InstantCommand(() -> m_robotShooter.spin()) + ); + i = new SequentialCommandGroup( + intakeToShootStuff, intakeToShoot + ); + addRequirements(this.swerve); } @@ -60,6 +93,8 @@ public class JoystickRecorder extends Command { inputs.leftY = leftY.get(); inputs.rightX = rightX.get(); inputs.rightY = rightY.get(); + inputs.OPLB = OPLB.get(); + inputs.OPRB = OPRB.get(); inputs.timedOffset = System.currentTimeMillis() - startTime; outputs.add(inputs); @@ -67,8 +102,23 @@ public class JoystickRecorder extends Command { swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), new Translation2d(inputs.rightX, inputs.rightY), true); + + if(lastOPLB != inputs.OPLB && inputs.OPLB == true){ + m_robotShooter.spin(); + m_robotIntake.handoff(); + }else if(lastOPLB != inputs.OPLB && inputs.OPLB == false){ + + } + + if(lastOPRB != inputs.OPRB){ + m_robotShooter.spin(); + m_robotIntake.handoff(); + } System.out.println("RECORDING"); + + lastOPLB = inputs.OPLB; + lastOPRB = inputs.OPRB; } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 6339fb2..776dd98 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.ShooterConstants; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -20,7 +21,8 @@ public class Shooter extends SubsystemBase { private TalonFX leftShooter; private TalonFX rightShooter; - + + private double smartDashboardShooterSpeed; /** Creates a new Shooter. */ public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) { leftShooter = leftTalonFX; @@ -28,6 +30,8 @@ public class Shooter extends SubsystemBase { leftShooter.setNeutralMode(NeutralModeValue.Coast); rightShooter.setNeutralMode(NeutralModeValue.Coast); + SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + } public Shooter(TalonFX leftShooter) { @@ -44,11 +48,11 @@ public class Shooter extends SubsystemBase { } public void spin() { - spin(ShooterConstants.SHOOTER_SPEED); + spin(smartDashboardShooterSpeed); } public void spin(double speed) { - leftShooter.set(speed); + leftShooter.set(-speed); rightShooter.set(-speed); } @@ -70,6 +74,8 @@ public class Shooter extends SubsystemBase { // This method will be called once per scheduler run // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); + smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + } } diff --git a/src/main/java/frc4388/utility/Configurable/Configurable.java b/src/main/java/frc4388/utility/Configurable/Configurable.java new file mode 100644 index 0000000..d8c1c1d --- /dev/null +++ b/src/main/java/frc4388/utility/Configurable/Configurable.java @@ -0,0 +1,5 @@ +package frc4388.utility.Configurable; + +public abstract class Configurable { + +} diff --git a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java new file mode 100644 index 0000000..c1ea101 --- /dev/null +++ b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java @@ -0,0 +1,4 @@ +package frc4388.utility.Configurable; + +public class ConfigurableDouble { +} diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java index e8b10cc..ac414e2 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/UtilityStructs.java @@ -6,6 +6,10 @@ public class UtilityStructs { public double leftY = 0.0; public double rightX = 0.0; public double rightY = 0.0; + + public boolean OPLB; + public boolean OPRB; + public long timedOffset = 0; } From 83a4da30600759d65c6e14118e3e2960e5ce7d64 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Mon, 19 Feb 2024 15:04:55 -0700 Subject: [PATCH 07/14] mondays changes --- .../java/frc4388/robot/RobotContainer.java | 160 +++++++++++------- .../robot/commands/Autos/PlaybackChooser.java | 10 +- .../commands/Swerve/neoJoystickPlayback.java | 12 +- .../commands/Swerve/neoJoystickRecorder.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 21 +++ 5 files changed, 133 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 56230c6..6ef4e27 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -58,7 +58,9 @@ public class RobotContainer { /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); + /* Virtual Controllers */ private final VirtualController m_virtualDriver = new VirtualController(0); @@ -119,7 +121,9 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - configureButtonBindings(); + configureButtonBindings(); + configureVirtualButtonBindings(); + DriverStation.silenceJoystickConnectionWarning(true); @@ -144,17 +148,9 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> getVirtualDriverController().setOutput(XboxController.A_BUTTON, true))) - // .onFalse(new InstantCommand(() -> getVirtualDriverController().setOutput(XboxController.A_BUTTON, false))); - - // new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) - // //.onTrue(new InstantCommand(() -> System.out.println("Hello"))); - // .onTrue(new InstantCommand(() -> getVirtualDriverController().setRumble(RumbleType.kBothRumble, 1.0))) - // .onFalse(new InstantCommand(() -> getVirtualDriverController().setRumble(RumbleType.kBothRumble, 0.0))); /* Auto Recording */ // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, @@ -168,78 +164,119 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) // .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - "sample.auto")) + "2note.auto")) .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - "sample.auto", + "2note.auto", new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false)) .onFalse(new InstantCommand()); - new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> System.out.println("Pressed A"))) - .onFalse(new InstantCommand(() -> System.out.println("Released A"))); - - new JoystickButton(getVirtualDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> System.out.println("Pressed B"))) - .onFalse(new InstantCommand(() -> System.out.println("Released B"))); - - new JoystickButton(getVirtualDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> System.out.println("Pressed X"))) - .onFalse(new InstantCommand(() -> System.out.println("Released X"))); - - new JoystickButton(getVirtualDriverController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> System.out.println("Pressed Y"))) - .onFalse(new InstantCommand(() -> System.out.println("Released Y"))); /* Speed */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); /* Operator Buttons */ - // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - // // Override Intake Position encoder: out - // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); + // Override Intake Position encoder: out + new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); - // // Override Intake Position encoder: in - // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); + // Override Intake Position encoder: in + new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); - // //Spin Shooter Motors - // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) - // .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); + //Spin Shooter Motors + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) + .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); - // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(ejectToShoot) - // .onFalse(turnOffShoot); + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(ejectToShoot) + .onFalse(turnOffShoot); - // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(i) - // .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(i) + .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); + + //spins up shooter, no wind down + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); + + } + + private void configureVirtualButtonBindings() { + /* Driver Buttons */ + new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + + /* Speed */ + new JoystickButton(getVirtualDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + + new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + /* Operator Buttons */ + + new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + new JoystickButton(getVirtualOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + + // Override Intake Position encoder: out + new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); + + // Override Intake Position encoder: in + new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); + + //Spin Shooter Motors + new JoystickButton(getVirtualOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) + .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); + + new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(ejectToShoot) + .onFalse(turnOffShoot); + + + new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(i) + .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); + + + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); } /** @@ -249,8 +286,11 @@ public class RobotContainer { */ public Command getAutonomousCommand() { //no auto - - return playbackChooser.getCommand(); + return new neoJoystickPlayback(m_robotSwerveDrive, + "2note.auto", + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + true, false); + //return playbackChooser.getCommand(); } /** diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index a270d7b..4fdf02a 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -68,13 +68,13 @@ public class PlaybackChooser { String[] dirs = m_dir.list(); - if(dirs == null){ // Fix funny error - return; + if(dirs != null){ // Fix funny error + for (String auto : dirs) { + chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + } } - for (String auto : dirs) { - chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); - } + for (var cmd_name : m_commandPool.keySet()) { chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); } diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java index 529bd34..206a2b5 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -16,11 +16,11 @@ public class neoJoystickPlayback extends Command { private final String filename; private final VirtualController[] controllers; private final ArrayList frames = new ArrayList<>(); - private int frame_index = 0; - private long startTime = 0; - private long playbackTime = 0; - private boolean m_finished = false; // ! There is no better way. - private boolean m_shouldfree = false; // should free memory on ending + private int frame_index = 0; + private long startTime = 0; + private long playbackTime = 0; + private boolean m_finished = false; // ! There is no better way. + private boolean m_shouldfree = false; // should free memory on ending private byte m_numAxes = 0; private byte m_numPOVs = 0; @@ -42,7 +42,7 @@ public class neoJoystickPlayback extends Command { } public boolean loadAuto() { - try (FileInputStream stream = new FileInputStream("./" + filename)) { + try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { if (m_numFrames != -1 && m_numFrames == frames.size()) { System.out.println("AUTOPLAYBACK: Auto Already loaded."); return true; diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java index 7cae811..0ad4100 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -69,7 +69,7 @@ public class neoJoystickRecorder extends Command { } @Override public void end(boolean interrupted) { - try (FileOutputStream stream = new FileOutputStream("./" + filename)) { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { // header: size of 0x5 // byte Number of axes per controller // byte Number of POVs per controller diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8cb65d9..7554671 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,6 +4,7 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -35,6 +36,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ @@ -55,6 +57,7 @@ public class SwerveDrive extends SubsystemBase { double rot = 0; + // ! drift correction if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; @@ -84,6 +87,24 @@ public class SwerveDrive extends SubsystemBase { setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + + if(fieldRelative) { + double rot = 0; + if(rightStick.getNorm() > 0.5) { + orientRotTarget = rightStick.getAngle().minus(new Rotation2d(0, 1)); + rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians(); + } + + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + } + /** * Set each module of the swerve drive to the corresponding desired state. * @param desiredStates Array of module states to set. From 3aa82cc4f52c990a145ab8808954e81106ccd93b Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Mon, 19 Feb 2024 15:49:45 -0700 Subject: [PATCH 08/14] Update Constants.java --- src/main/java/frc4388/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6b9c00d..ca8b678 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -155,7 +155,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 = 1.0; // final + public static final double SHOOTER_SPEED = 0.35; // final public static final double SHOOTER_IDLE = 0.4; // final } From 1955885124693c471eb601c4bfd32a501c4c75d8 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 20 Feb 2024 00:46:44 -0700 Subject: [PATCH 09/14] Started changing intake class from spark max to talon; Not done limit switch or robot container stuff; (im actually tweakin) --- .../java/frc4388/robot/RobotContainer.java | 20 ++--- .../robot/commands/Intake/ArmIntakeIn.java | 4 +- .../java/frc4388/robot/subsystems/Intake.java | 89 ++++++++++++++++++- 3 files changed, 100 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 51c0419..9b06a59 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -185,16 +185,16 @@ public class RobotContainer { /* Auto Recording */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - () -> getDeadbandedDriverController().getLeftX(), - () -> getDeadbandedDriverController().getLeftY(), - () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY(), - () -> getDeadbandedOperatorController().getLeftBumper(), - () -> getDeadbandedOperatorController().getRightBumper(), - "TwoNotePrt1.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // () -> getDeadbandedOperatorController().getLeftBumper(), + // () -> getDeadbandedOperatorController().getRightBumper(), + // "TwoNotePrt1.txt")) + // .onFalse(new InstantCommand()); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index 1952c2d..9b63f58 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -28,8 +28,8 @@ public class ArmIntakeIn extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - robotIntake.pidOut(); - robotIntake.spinIntakeMotor(); + robotIntake.talonPIDOut(); + robotIntake.talonSpinIntakeMotor(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 503db7c..eae9b05 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -6,6 +6,22 @@ package frc4388.robot.subsystems; import java.util.function.BooleanSupplier; +import javax.swing.text.Position; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.can.TalonSRXPIDSetConfiguration; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.ForwardLimitValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import com.ctre.phoenix6.signals.ReverseLimitValue; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkLimitSwitch; @@ -13,6 +29,7 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.CAN; import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; @@ -22,14 +39,26 @@ import frc4388.utility.Gains; public class Intake extends SubsystemBase { + //NEO private CANSparkMax intakeMotor; private CANSparkMax pivot; private SparkPIDController m_spedController; - private static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; private SparkLimitSwitch forwardLimit; private SparkLimitSwitch reverseLimit; private SparkLimitSwitch intakeforwardLimit; private SparkLimitSwitch intakereverseLimit; + + //Talon + private TalonFX talonIntake; + private TalonFX talonPivot; + private CANcoder encoder; + + private HardwareLimitSwitchConfigs l; + + TalonFXConfiguration doodooController = new TalonFXConfiguration(); + + + public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; private BooleanSupplier sup = () -> true; private BooleanSupplier dup = () -> false; @@ -37,6 +66,7 @@ public class Intake extends SubsystemBase { private double smartDashboardOuttakeValue; /** Creates a new Intake. */ + //For NEO public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { this.intakeMotor = intakeMotor; this.pivot = pivot; @@ -65,6 +95,63 @@ public class Intake extends SubsystemBase { SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); } + //For Talon + public Intake(TalonFX talonIntake, TalonFX talonPivot) { + this.talonIntake = talonIntake; + this.talonPivot = talonPivot; + + talonIntake.getConfigurator().apply(new TalonFXConfiguration()); + talonPivot.getConfigurator().apply(new TalonFXConfiguration()); + + talonIntake.setNeutralMode(NeutralModeValue.Brake); + talonPivot.setNeutralMode(NeutralModeValue.Brake); + + talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); + talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs()); + + + + doodooController.Slot0.kP = armGains.kP; + doodooController.Slot1.kI = armGains.kI; + doodooController.Slot2.kD = armGains.kD; + + // in init function, set slot 0 gains + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 0.05; // An error of 0.5 rotations results in 12 V output + slot0Configs.kI = 0.0; // no output for integrated error + slot0Configs.kD = 0.0; // A velocity of 1 rps results in 0.1 V output + + talonPivot.getConfigurator().apply(slot0Configs); + + + } + + // ! Talon Methods + public void talonPIDIn() { + PositionVoltage request = new PositionVoltage(0).withSlot(0); + talonPivot.setControl(request.withPosition(53)); //TODO: Find actual value + } + + public void talonPIDOut() { + PositionVoltage request = new PositionVoltage(53).withSlot(53); + talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value + } + + public void talonHandoff() { + talonIntake.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + } + + public void talonSpinIntakeMotor() { + talonIntake.set(IntakeConstants.INTAKE_SPEED); + } + + public boolean getTalonIntakeLimitSwitchState() { + return false; + } + + + + // ! NEO Methods //hanoff public void spinIntakeMotor() { intakeMotor.set(IntakeConstants.INTAKE_SPEED); From 00e814a574a563ad1536af3aeeb0172e506fcebe Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 20 Feb 2024 16:46:37 -0700 Subject: [PATCH 10/14] no more javax --- src/main/java/frc4388/robot/subsystems/Intake.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index eae9b05..c94de2f 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -6,8 +6,6 @@ package frc4388.robot.subsystems; import java.util.function.BooleanSupplier; -import javax.swing.text.Position; - import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.can.TalonSRXPIDSetConfiguration; From 2758e01cd2a51dd0691bcf6ad6c63436f35422e9 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 20 Feb 2024 17:40:33 -0700 Subject: [PATCH 11/14] arm intake in perfect. --- src/main/java/frc4388/robot/RobotMap.java | 4 ++-- .../robot/commands/Intake/ArmIntakeIn.java | 15 +++++++++------ .../java/frc4388/robot/subsystems/Intake.java | 13 +++++++------ 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1259982..4f4d20f 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -70,8 +70,8 @@ public class RobotMap { public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); /* Intake Subsystem */ - public final CANSparkMax intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); - public final CANSparkMax pivotMotor = new CANSparkMax(IntakeConstants.PIVOT_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); + public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); void configureLEDMotorControllers() { } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index 9b63f58..f92bf4b 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -38,12 +38,15 @@ public class ArmIntakeIn extends Command { // Returns true when the command should end. @Override - public boolean isFinished() { - if(robotIntake.getIntakeLimitSwtichState() == true) { - return true; - } else { - return false; + public boolean isFinished() + { + 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/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index c94de2f..53565fc 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -104,14 +104,14 @@ public class Intake extends SubsystemBase { talonIntake.setNeutralMode(NeutralModeValue.Brake); talonPivot.setNeutralMode(NeutralModeValue.Brake); - talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); - talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs()); + // talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); + // talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs()); - doodooController.Slot0.kP = armGains.kP; - doodooController.Slot1.kI = armGains.kI; - doodooController.Slot2.kD = armGains.kD; + // doodooController.Slot0.kP = armGains.kP; + // doodooController.Slot1.kI = armGains.kI; + // doodooController.Slot2.kD = armGains.kD; // in init function, set slot 0 gains var slot0Configs = new Slot0Configs(); @@ -144,7 +144,8 @@ public class Intake extends SubsystemBase { } public boolean getTalonIntakeLimitSwitchState() { - return false; + var r = talonIntake.getForwardLimit().getValue().value == 1; + return r; } From 00f714f845f11de8bef1e4af15e618b98f4d43ec Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 20 Feb 2024 18:56:57 -0700 Subject: [PATCH 12/14] talon intake --- .../java/frc4388/robot/RobotContainer.java | 40 +++++++++---------- .../robot/commands/Intake/ArmIntakeIn.java | 20 +++++----- .../java/frc4388/robot/subsystems/Intake.java | 11 +++++ 3 files changed, 41 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b06a59..313fcd4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -67,7 +67,7 @@ public class RobotContainer { private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.pidIn()), + new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotShooter.spin()) ); @@ -77,12 +77,12 @@ public class RobotContainer { private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) ); private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); /* Autos */ @@ -93,20 +93,20 @@ public class RobotContainer { private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new WaitCommand(1).asProxy(), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new WaitCommand(1).asProxy(), new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") ); private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup ( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new WaitCommand(1).asProxy(), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( startLeftMoveRight.asProxy(), @@ -121,10 +121,10 @@ public class RobotContainer { private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new WaitCommand(1).asProxy(), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), intakeToShootStuff.asProxy(), new WaitCommand(1).asProxy(), new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt1.txt"), @@ -134,10 +134,10 @@ public class RobotContainer { new WaitCommand(0.5).asProxy(), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new WaitCommand(1).asProxy(), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) .addOption("Taxi Auto", taxi.asProxy()) @@ -213,25 +213,25 @@ public class RobotContainer { /* Operator Buttons */ new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut())) + .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor())) + .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake)); // Override Intake Position encoder: in new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake)); //Spin Shooter Motors new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) @@ -245,7 +245,7 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(i) - .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); + .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index f92bf4b..fc08304 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -38,15 +38,15 @@ public class ArmIntakeIn extends Command { // Returns true when the command should end. @Override - public boolean isFinished() - { - if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) - { - return !true==true; - } - else - { - return !false==!(!(true)); - } + 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/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 53565fc..c9c87b3 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -148,6 +148,17 @@ public class Intake extends SubsystemBase { return r; } + public void talonSetPivotEncoderPosition(int val) { + talonPivot.setPosition(val); + } + + public void talonStopIntakeMotors() { + talonIntake.set(0); + } + + public void talonStopArmMotor() { + talonPivot.set(0); + } // ! NEO Methods From 464f22f2c3583a351c0f7a16aa1be9c9dfda22ec Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 20 Feb 2024 19:19:21 -0700 Subject: [PATCH 13/14] confiig --- .../utility/Configurable/Configurable.java | 5 ----- .../Configurable/ConfigurableDouble.java | 21 ++++++++++++++++++- 2 files changed, 20 insertions(+), 6 deletions(-) delete mode 100644 src/main/java/frc4388/utility/Configurable/Configurable.java diff --git a/src/main/java/frc4388/utility/Configurable/Configurable.java b/src/main/java/frc4388/utility/Configurable/Configurable.java deleted file mode 100644 index d8c1c1d..0000000 --- a/src/main/java/frc4388/utility/Configurable/Configurable.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc4388.utility.Configurable; - -public abstract class Configurable { - -} diff --git a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java index c1ea101..c0384db 100644 --- a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java @@ -1,4 +1,23 @@ -package frc4388.utility.Configurable; +package frc4388.utility.configurable; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ConfigurableDouble { + private double defualtValue; + private String name; + + /** + * Creates an new ConfigurableDouble through Smart Dashboard. + * @param name the name of the Smart Dashboard key. + * @param defualtValue the initilization value + */ + public ConfigurableDouble(String name, double defualtValue) { + this.name = name; + this.defualtValue = defualtValue; + SmartDashboard.putNumber(name, defualtValue); + } + + public double get() { + return SmartDashboard.getNumber(name, defualtValue); + } } From 3ec5525d5ad0295356d6f12cc774a661f2404ba0 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 20 Feb 2024 19:34:35 -0700 Subject: [PATCH 14/14] begonr --- src/main/java/frc4388/robot/RobotContainer.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4d81ee5..67cfede 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,8 +7,6 @@ package frc4388.robot; -import com.fasterxml.jackson.databind.ser.std.InetAddressSerializer; - import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; @@ -20,8 +18,8 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.Autos.PlaybackChooser; -//import frc4388.robot.commands.Swerve.JoystickPlayback; -//import frc4388.robot.commands.Swerve.JoystickRecorder; +import frc4388.robot.commands.Swerve.JoystickPlayback; +import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; import frc4388.robot.commands.Intake.ArmIntakeIn;