From 6b0e4b9cad20032d565946754b5b68085380a60a Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 21 Mar 2024 09:01:42 -0600 Subject: [PATCH] denver comp (im tweakin) --- .../java/frc4388/robot/RobotContainer.java | 238 +++++++++---- .../robot/commands/Autos/AutoAlign.java | 332 +++++++++--------- .../commands/Swerve/JoystickRecorder.java | 2 +- .../java/frc4388/robot/subsystems/Intake.java | 6 +- .../frc4388/robot/subsystems/Limelight.java | 90 ++--- .../frc4388/robot/subsystems/SwerveDrive.java | 12 +- .../robot/subsystems/SwerveModule.java | 4 +- 7 files changed, 388 insertions(+), 296 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bf50581..5ffbde2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,7 +7,10 @@ package frc4388.robot; +import org.opencv.video.Video; + import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.VideoMode; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; @@ -24,7 +27,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.commands.Autos.AutoAlign; +//import frc4388.robot.commands.Autos.AutoAlign; import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; @@ -87,7 +90,7 @@ public class RobotContainer { private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); - private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup( + private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotShooter.idle()) // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))), @@ -108,18 +111,18 @@ public class RobotContainer { // ! Teleop Commands - private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); + //private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); private SequentialCommandGroup autoShoot = new SequentialCommandGroup( // MoveToSpeaker, - autoAlign, + //autoAlign, new InstantCommand(() -> m_robotShooter.spin()), new WaitCommand(3.0), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new WaitCommand(3.0), - new InstantCommand(() -> m_robotShooter.idle()), - new InstantCommand(() -> autoAlign.reverse()), - autoAlign.asProxy() + new InstantCommand(() -> m_robotShooter.idle()) + // new InstantCommand(() -> autoAlign.reverse()), + // autoAlign.asProxy() ); @@ -290,7 +293,7 @@ public class RobotContainer { DriverStation.silenceJoystickConnectionWarning(true); - CameraServer.startAutomaticCapture(); + CameraServer.startAutomaticCapture().setConfigJson("{ \"width\": -512, \"height\": 512 }"); /* Default Commands */ // drives the robot with a two-axis input from the driver controller @@ -329,58 +332,58 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())); // * /* D-Pad Stuff */ - new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - new Translation2d(0, 0), - true))) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - new Translation2d(0, 0), - true))); + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); - new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), - new Translation2d(0, 0), - true))) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - new Translation2d(0, 0), - true))); + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); - new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), - new Translation2d(0, 0), - true))) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - new Translation2d(0, 0), - true))); + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); - new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), - new Translation2d(0, 0), - true))) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - new Translation2d(0, 0), - true))); + // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); // ! /* Auto Recording */ - // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) - // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, - // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - // () -> autoplaybackName.get())) - // .onFalse(new InstantCommand()); + new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) + .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + () -> autoplaybackName.get())) + .onFalse(new InstantCommand()); - // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - // () -> autoplaybackName.get(), - // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - // true, false)) - // .onFalse(new InstantCommand()); + new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + () -> autoplaybackName.get(), + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + true, false)) + .onFalse(new InstantCommand()); // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, @@ -475,25 +478,101 @@ public class RobotContainer { } 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())); - - new Trigger(() -> getDeadbandedDriverController().getPOV() == 0) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())); - - - // /* Operator Buttons */ + // ? /* Driver Buttons */ - new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) + new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); + + new JoystickButton(getVirtualDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())); + + new JoystickButton(getVirtualDriverController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())); + + // * /* D-Pad Stuff */ + // new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), + // new Translation2d(0, 0), + // true))) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), + // new Translation2d(0, 0), + // true))); + + // ! /* Auto Recording */ + // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) + // .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + // new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + // () -> autoplaybackName.get())) + // .onFalse(new InstantCommand()); + + // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + // () -> autoplaybackName.get(), + // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + // true, false)) + // .onFalse(new InstantCommand()); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Taxi.txt")) + // .onFalse(new InstantCommand()); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + // .onFalse(new InstantCommand()); + // ! /* 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())); + + new Trigger(() -> getVirtualDriverController().getPOV() == 270) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); + + new Trigger(() -> getVirtualDriverController().getPOV() == 90) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); + + // new JoystickButton(getVirtualDriverController(), XboxController.Y_BUTTON) + // .whileTrue(new InstantCommand(() -> + // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + // new Translation2d(0, 0), + // true), m_robotSwerveDrive)); + + + //? /* Operator Buttons */ + + new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); @@ -506,37 +585,48 @@ public class RobotContainer { .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON) - .onTrue(emergencyRetract.asProxy()); + .onTrue(emergencyRetract); // Override Intake Position encoder: out new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-55), m_robotIntake)); // Override Intake Position encoder: in new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake)); - + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-6.2), m_robotIntake)); new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) - .onFalse(turnOffShoot.asProxy()); + .onFalse(turnOffShoot); new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(i.asProxy()) + .onTrue(i) .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); //spins up shooter, no wind down new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); - // new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) + // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) // .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake)) // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - .onTrue(emergencyRetract.asProxy()); + .onTrue(emergencyRetract); + + new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) + .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) + .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) + .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) + .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); + + new Trigger(() -> getVirtualOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); + } /** diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java index ce8b37c..58eb3ed 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -1,208 +1,208 @@ -package frc4388.robot.commands.Autos; -import frc4388.robot.subsystems.Limelight; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.Constants.AutoAlignConstants; -import frc4388.robot.Constants.VisionConstants; -import edu.wpi.first.wpilibj2.command.Command; +// package frc4388.robot.commands.Autos; +// import frc4388.robot.subsystems.Limelight; +// import frc4388.robot.subsystems.SwerveDrive; +// import frc4388.robot.Constants.AutoAlignConstants; +// import frc4388.robot.Constants.VisionConstants; +// import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.geometry.Rotation2d; -import java.util.Optional; +// import java.util.Optional; -import org.opencv.core.RotatedRect; +// import org.opencv.core.RotatedRect; -import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; -public class AutoAlign extends Command { - private SwerveDrive swerve; - private Limelight limelight; - private Pose2d pose; - private Translation2d targetPos; - private Rotation2d targetRot; +// public class AutoAlign extends Command { +// private SwerveDrive swerve; +// private Limelight limelight; +// private Pose2d pose; +// private Translation2d targetPos; +// private Rotation2d targetRot; - private Optional alliance; - private boolean isRed; +// private Optional alliance; +// private boolean isRed; - private boolean isFinished; - private boolean isReverseFinished; +// private boolean isFinished; +// private boolean isReverseFinished; - private boolean reverseAfterFinish; - private Translation2d[] moveStickReplayArr; - private Translation2d[] rotStickReplayArr; - private int replayIndex; +// private boolean reverseAfterFinish; +// private Translation2d[] moveStickReplayArr; +// private Translation2d[] rotStickReplayArr; +// private int replayIndex; - // PID Stuff - private double prevError; - private double cumError; +// // PID Stuff +// private double prevError; +// private double cumError; - public AutoAlign(SwerveDrive swerve, Limelight limelight) { - this.swerve = swerve; - this.limelight = limelight; - } +// public AutoAlign(SwerveDrive swerve, Limelight limelight) { +// this.swerve = swerve; +// this.limelight = limelight; +// } - // Calc the closest point on a circle, to the center of the speaker - private Translation2d calcTargetPos(){ - final double R = VisionConstants.targetPosDistance; - final double cX; - final double cY; - if(isRed){ - cX = VisionConstants.RedSpeakerCenter.getX(); - cY = VisionConstants.RedSpeakerCenter.getY(); - }else{ - cX = VisionConstants.BlueSpeakerCenter.getX(); - cY = VisionConstants.BlueSpeakerCenter.getY(); - } - final double pX = pose.getX(); - final double pY = pose.getY(); +// // Calc the closest point on a circle, to the center of the speaker +// private Translation2d calcTargetPos(){ +// final double R = VisionConstants.targetPosDistance; +// final double cX; +// final double cY; +// if(isRed){ +// cX = VisionConstants.RedSpeakerCenter.getX(); +// cY = VisionConstants.RedSpeakerCenter.getY(); +// }else{ +// cX = VisionConstants.BlueSpeakerCenter.getX(); +// cY = VisionConstants.BlueSpeakerCenter.getY(); +// } +// final double pX = pose.getX(); +// final double pY = pose.getY(); - // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point - double vX = pX - cX; - double vY = pY - cY; - double magV = Math.sqrt(vX*vX + vY*vY); - double aX = cX + vX / magV * R; - double aY = cY + vY / magV * R; +// // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point +// double vX = pX - cX; +// double vY = pY - cY; +// double magV = Math.sqrt(vX*vX + vY*vY); +// double aX = cX + vX / magV * R; +// double aY = cY + vY / magV * R; - return new Translation2d(aX, aY); - } +// return new Translation2d(aX, aY); +// } - // Calculate the angle facing the center, at the target rot - private Rotation2d calcTargetRot() { - final double R = VisionConstants.targetPosDistance; - final double cX; - final double cY; - if(isRed){ - cX = VisionConstants.RedSpeakerCenter.getX(); - cY = VisionConstants.RedSpeakerCenter.getY(); - }else{ - cX = VisionConstants.BlueSpeakerCenter.getX(); - cY = VisionConstants.BlueSpeakerCenter.getY(); - } - final double pX = pose.getX(); - final double pY = pose.getY(); +// // Calculate the angle facing the center, at the target rot +// private Rotation2d calcTargetRot() { +// final double R = VisionConstants.targetPosDistance; +// final double cX; +// final double cY; +// if(isRed){ +// cX = VisionConstants.RedSpeakerCenter.getX(); +// cY = VisionConstants.RedSpeakerCenter.getY(); +// }else{ +// cX = VisionConstants.BlueSpeakerCenter.getX(); +// cY = VisionConstants.BlueSpeakerCenter.getY(); +// } +// final double pX = pose.getX(); +// final double pY = pose.getY(); - final double dX = pX - cX; - final double dY = pY - cY; +// final double dX = pX - cX; +// final double dY = pY - cY; - final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360); +// final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360); - return Rotation2d.fromDegrees(yaw); - } +// return Rotation2d.fromDegrees(yaw); +// } - // Clamp to a circle, like an xbox controller - private Translation2d clamp(double oldX, double oldY){ - // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley - final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360; - final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); - final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI)); +// // Clamp to a circle, like an xbox controller +// private Translation2d clamp(double oldX, double oldY){ +// // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley +// final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360; +// final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); +// final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI)); - final double newX = Math.max(-maxX, Math.min(maxX, oldX)); - final double newY = Math.max(-maxY, Math.min(maxY, oldY)); +// final double newX = Math.max(-maxX, Math.min(maxX, oldX)); +// final double newY = Math.max(-maxY, Math.min(maxY, oldY)); - return new Translation2d(newX, newY); - } +// return new Translation2d(newX, newY); +// } - private Translation2d calcMoveStick(){ - final double curX = pose.getX(); - final double curY = pose.getY(); +// private Translation2d calcMoveStick(){ +// final double curX = pose.getX(); +// final double curY = pose.getY(); - // I think this might work, assuming the constants are good - double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; - double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; +// // I think this might work, assuming the constants are good +// double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; +// double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; - return clamp(stickX, stickY); - } +// return clamp(stickX, stickY); +// } - private Translation2d calcRotStick(){ - double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); - cumError += error * .02; // 20 ms - double delta = error - prevError; +// private Translation2d calcRotStick(){ +// double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); +// cumError += error * .02; // 20 ms +// double delta = error - prevError; - final double kP = 4; - final double kI = 4; - final double kD = 4; - final double kF = 4; +// final double kP = 4; +// final double kI = 4; +// final double kD = 4; +// final double kF = 4; - double output = error * kP; - output += cumError * kI; - output += delta * kD; - output += kF; +// double output = error * kP; +// output += cumError * kI; +// output += delta * kD; +// output += kF; - prevError = error; - return clamp(output, 0); - } +// prevError = error; +// return clamp(output, 0); +// } - public void reverse() { - this.reverseAfterFinish = true; - } +// public void reverse() { +// this.reverseAfterFinish = true; +// } - // Called when the command is initially scheduled. - @Override - public final void initialize() { - isRed = alliance.get() == DriverStation.Alliance.Red; - if(reverseAfterFinish){ - // isReverseFinished = false; - replayIndex = 0; - }else{ - moveStickReplayArr = new Translation2d[]{}; - rotStickReplayArr = new Translation2d[]{}; - } - } +// // Called when the command is initially scheduled. +// @Override +// public final void initialize() { +// isRed = alliance.get() == DriverStation.Alliance.Red; +// if(reverseAfterFinish){ +// // isReverseFinished = false; +// replayIndex = 0; +// }else{ +// moveStickReplayArr = new Translation2d[]{}; +// rotStickReplayArr = new Translation2d[]{}; +// } +// } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // Update limelight pos - pose = limelight.getPose(); +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// // Update limelight pos +// pose = limelight.getPose(); - // These must be 0, or it will error - Translation2d moveStick = new Translation2d(0, 0); - Translation2d rotStick = new Translation2d(0, 0); +// // These must be 0, or it will error +// Translation2d moveStick = new Translation2d(0, 0); +// Translation2d rotStick = new Translation2d(0, 0); - // Regular replay - if(!isFinished){ - targetPos = calcTargetPos(); - targetRot = calcTargetRot(); +// // Regular replay +// if(!isFinished){ +// targetPos = calcTargetPos(); +// targetRot = calcTargetRot(); - moveStick = calcMoveStick(); - rotStick = calcRotStick(); +// moveStick = calcMoveStick(); +// rotStick = calcRotStick(); - // This is a way of appending... - moveStickReplayArr[moveStickReplayArr.length] = moveStick; - rotStickReplayArr[rotStickReplayArr.length] = rotStick; +// // This is a way of appending... +// moveStickReplayArr[moveStickReplayArr.length] = moveStick; +// rotStickReplayArr[rotStickReplayArr.length] = rotStick; - // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ - // replayIndex - // } - isFinished = limelight.isNearSpeaker(); +// // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ +// // replayIndex +// // } +// isFinished = limelight.isNearSpeaker(); - // If reverseAfterFinish, then loop back over and replay in reverse - }else if(reverseAfterFinish && !isReverseFinished){ - // Get reverse direction - moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; - rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; +// // If reverseAfterFinish, then loop back over and replay in reverse +// }else if(reverseAfterFinish && !isReverseFinished){ +// // Get reverse direction +// moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; +// rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; - // Invert sticks - moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); - rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); +// // Invert sticks +// moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); +// rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); - replayIndex++; +// replayIndex++; - if(replayIndex >= moveStickReplayArr.length){ - reverseAfterFinish = true; - } - } +// if(replayIndex >= moveStickReplayArr.length){ +// reverseAfterFinish = true; +// } +// } - // This would greatly benifit from having feild Relative implemented. - swerve.driveWithInput(moveStick, rotStick, true); - } +// // This would greatly benifit from having feild Relative implemented. +// swerve.driveWithInput(moveStick, rotStick, true); +// } - // Returns true when the command should end. - @Override - public final boolean isFinished() { - return isFinished && (isReverseFinished || !reverseAfterFinish); - } -} \ No newline at end of file +// // Returns true when the command should end. +// @Override +// public final boolean isFinished() { +// return isFinished && (isReverseFinished || !reverseAfterFinish); +// } +// } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index b4907c5..82ba36e 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -68,7 +68,7 @@ public class JoystickRecorder extends CommandBase { new Translation2d(inputs.rightX, inputs.rightY), true); - System.out.println("RECORDING"); + //System.out.println("RECORDING"); } // 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 cd9371c..abd12fc 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -117,9 +117,9 @@ public class Intake extends SubsystemBase { // in init function, set slot 0 gains var slot0Configs = new Slot0Configs(); - slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output + slot0Configs.kP = 1.3; // An error of 0.5 rotations results in 12 V output slot0Configs.kI = 0.0; // no output for integrated error - slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output + slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output talonPivot.getConfigurator().apply(slot0Configs); @@ -336,5 +336,7 @@ public class Intake extends SubsystemBase { // SmartDashboard.putNumber("Pivot Position", getArmPos()); //smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + + //SmartDashboard.putBoolean("Limit Switch State", getTalonIntakeLimitSwitchState()); } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index f8c39e0..2200b07 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -18,65 +18,65 @@ import frc4388.robot.Constants.VisionConstants; public class Limelight extends SubsystemBase { - // [X, Y, Z, Roll, Pitch, Yaw] - private double[] cameraPose; - private boolean isTag; + // // [X, Y, Z, Roll, Pitch, Yaw] + // private double[] cameraPose; + // private boolean isTag; - private Pose2d pose; - private boolean isNearSpeaker; + // private Pose2d pose; + // private boolean isNearSpeaker; - public boolean getIsTag() { - return isTag; - } + // public boolean getIsTag() { + // return isTag; + // } - private void update() { - SmartDashboard.putBoolean("Apriltag", isTag); - if(!isTag){ - return; - } + // private void update() { + // SmartDashboard.putBoolean("Apriltag", isTag); + // if(!isTag){ + // return; + // } - double x = cameraPose[0]; - double y = cameraPose[1]; - double yaw = cameraPose[5]; + // double x = cameraPose[0]; + // double y = cameraPose[1]; + // double yaw = cameraPose[5]; - Rotation2d rot = Rotation2d.fromDegrees(yaw); + // Rotation2d rot = Rotation2d.fromDegrees(yaw); - pose = new Pose2d(x, y, rot); + // pose = new Pose2d(x, y, rot); - boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; + // boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; - double distance; + // double distance; - if(isRed){ - distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); - }else{ - distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); - } + // if(isRed){ + // distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); + // }else{ + // distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); + // } - isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; + // isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; - //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); - //SmartDashboard.putNumber("speakerDistance", distance); - } + // //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); + // //SmartDashboard.putNumber("speakerDistance", distance); + // } - public Pose2d getPose() { - return pose; - } + // public Pose2d getPose() { + // return pose; + // } - public boolean isNearSpeaker() { - return isNearSpeaker; - } + // public boolean isNearSpeaker() { + // return isNearSpeaker; + // } - @Override - public void periodic() { - // This method will be called once per scheduler run + // @Override + // public void periodic() { + // // This method will be called once per scheduler run - //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; - //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); + // //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; + // //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); - //if(newPose != cameraPose){ - // cameraPose = newPose; - //update(); - //} - } + // //if(newPose != cameraPose){ + // // cameraPose = newPose; + // //update(); + // //} + // } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e4e6cc4..8af738b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -62,7 +62,7 @@ public class SwerveDrive extends SubsystemBase { // ! drift correction if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; + rot = rightStick.getX(); // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { @@ -144,7 +144,7 @@ public class SwerveDrive extends SubsystemBase { Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * 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); } @@ -183,22 +183,22 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - rotTarget = 0.0; + rotTarget = gyro.getAngle(); } public void resetGyroFlip() { gyro.resetFlip(); - rotTarget = 0.0; + rotTarget = gyro.getAngle(); } public void resetGyroRightBlue() { gyro.resetRightSideBlue(); - rotTarget = 0.0; + rotTarget = gyro.getAngle(); } public void resetGyroRightAmp() { gyro.resetAmpSide(); - rotTarget = 0.0; + rotTarget = gyro.getAngle(); } public void stopModules() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 7e112f9..1b8c36c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -30,7 +30,7 @@ public class SwerveModule extends SubsystemBase { private WPI_TalonFX angleMotor; private CANCoder encoder; private int selfid; - private ConfigurableDouble offsetGetter; + // private ConfigurableDouble offsetGetter; private static int swerveId = 0; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; @@ -39,7 +39,7 @@ public class SwerveModule extends SubsystemBase { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; - this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); + // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); this.selfid = swerveId; swerveId++; TalonFXConfiguration angleConfig = new TalonFXConfiguration();