From d19ff47543c96e8f623fa270479dd13c9a5ba93c Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 18 Jun 2024 11:52:58 -0600 Subject: [PATCH] PID no work --- build.gradle | 2 +- src/main/java/frc4388/robot/Constants.java | 7 +- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 261 ++---------------- src/main/java/frc4388/robot/RobotMap.java | 9 +- .../frc4388/robot/subsystems/Limelight.java | 238 ++++++++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 8 +- .../robot/subsystems/SwerveModule.java | 88 ++++-- vendordeps/photonlib.json | 42 --- 9 files changed, 220 insertions(+), 437 deletions(-) delete mode 100644 vendordeps/photonlib.json diff --git a/build.gradle b/build.gradle index dc84aef..9638f90 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 827a6b5..5aebb9f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -47,8 +47,8 @@ public final class Constants { } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 0; - public static final int RIGHT_FRONT_STEER_ID = 1; + public static final int RIGHT_FRONT_WHEEL_ID = 3; + public static final int RIGHT_FRONT_STEER_ID = 2; public static final int RIGHT_FRONT_ENCODER_ID = 12; // public static final int LEFT_FRONT_WHEEL_ID = 4; @@ -68,6 +68,9 @@ public final class Constants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + + public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + } public static final class AutoConstants { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 36c4330..cd9c24c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -124,7 +124,7 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); + // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3888775..a0e9a78 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -68,18 +68,12 @@ public class RobotContainer { // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); - // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - // m_robotMap.rightFront, - // m_robotMap.leftBack, - // m_robotMap.rightBack, + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, + m_robotMap.rightFront, + m_robotMap.leftBack, + m_robotMap.rightBack, - // m_robotMap.gyro); - /* Limelight */ - // private final Limelight limelight = new Limelight(); - - // private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); - - // private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor); + m_robotMap.gyro); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -119,53 +113,6 @@ public class RobotContainer { // ); - // private SequentialCommandGroup i = new SequentialCommandGroup( - // intakeToShootStuff, intakeToShoot, - // new InstantCommand(() -> m_robotShooter.idle()) - // ); - - // private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - // new WaitCommand(0.75), - // new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) - // ); - - // private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter) - // // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - // ); - - // private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( - // interrupt, - // new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake), - // new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) - // ); - - // private SequentialCommandGroup ampShoot = new SequentialCommandGroup( - // new InstantCommand(() -> m_robotIntake.ampPosition()), - // new InstantCommand(() -> m_robotIntake.ampOuttake(0.1)) //TODO: Find Actual Speed - // ); - - // ! /* Autos */ - // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); - // private String lastAutoName = "final_red_center_4note_taxi.auto"; - // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); - // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, - // () -> autoplaybackName.get(), // lastAutoName - // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - // true, false); - - - // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - // .addOption("Taxi Auto", taxi.asProxy()) - // .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) - // .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) - // // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) - // // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) - // .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) - // .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) - // .buildDisplay(); - /** @@ -173,7 +120,7 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); - configureVirtualButtonBindings(); + // configureVirtualButtonBindings(); // new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto())); // new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); @@ -202,7 +149,17 @@ public class RobotContainer { // getDeadbandedDriverController().getRightX(), // getDeadbandedDriverController().getRightY(), // true); - // }, m_robotSwerveDrive) + // }, m_robotSwerveDrive)); + + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.oneModuleTest( + m_robotMap.rightFront, + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight()); + }, m_robotSwerveDrive)); + + + // .withName("SwerveDrive OrientationCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); @@ -223,190 +180,6 @@ public class RobotContainer { */ private void configureButtonBindings() { - // ? /* Driver Buttons */ - - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - // * /* 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.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.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(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 Trigger(() -> getDeadbandedDriverController().getPOV() == 270) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); - - // new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .whileTrue(new InstantCommand(() -> - // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true), m_robotSwerveDrive)); - - - //? /* Operator Buttons */ - - // new Joystick(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotMap.rightFront.go(new Translation2d()))); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.handoff())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) - // .onTrue(emergencyRetract); - - - // Override Intake Position encoder: out - // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake)); - - // // Override Intake Position encoder: in - // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake)); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) - // .onFalse(turnOffShoot); - - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(i) - // .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn())); - - // //spins up shooter, no wind down - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); - - // // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - // // .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake)) - // // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - // .onTrue(emergencyRetract); - - // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5) - // .onTrue(new InstantCommand(() -> m_robotClimber.climbOut())) - // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5) - // .onTrue(new InstantCommand(() -> m_robotClimber.climbIn())) - // .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb())); - - // new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) - // .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5))); - - } - - /** - * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}.

- * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons. - */ - private void configureVirtualButtonBindings() { - - // ? /* Driver Buttons */ - - /* Notice: the following buttons have not been replicated - * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback - * Swerve Drive Rotation Gear Shifts : Same reason as Slow and Fast mode. - * Auto Recording controls : We don't want an Null Ouroboros for an auto. - */ - - // ? /* Operator Buttons */ - - /* Notice: the following buttons have not been replicated - * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot. - * We don't need it in an auto. - * Climbing controls : We don't need to climb in auto. - */ - - // 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/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index aa9fba8..7769faf 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -31,6 +31,7 @@ import frc4388.utility.RobotGyro; public class RobotMap { // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); // public RobotGyro gyro = new RobotGyro(m_pigeon2); + public RobotGyro gyro = null; public SwerveModule leftFront; public SwerveModule rightFront; @@ -152,8 +153,14 @@ public class RobotMap { // rightBackSteer.setNeutralMode(NeutralMode.Brake); // // initialize SwerveModules - // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); + this.leftFront = this.rightFront; + this.leftBack = this.rightFront; + this.rightBack = this.rightFront; + + // this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + // this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); // this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); // this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 9d1289b..8e7da41 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -1,165 +1,165 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// // Copyright (c) FIRST and other WPILib contributors. +// // Open Source Software; you can modify and/or share it under the terms of +// // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; +// 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 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.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; +// 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.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc4388.robot.Constants.VisionConstants; -public class Limelight extends SubsystemBase { +// public class Limelight extends SubsystemBase { - private PhotonCamera cam; - private PhotonPoseEstimator photonPoseEstimator; +// // private PhotonCamera cam; +// // private PhotonPoseEstimator photonPoseEstimator; - private boolean lightOn; +// private boolean lightOn; - /** Creates a new Limelight. */ - public Limelight() { - cam = new PhotonCamera(VisionConstants.NAME); - cam.setDriverMode(false); - } +// /** Creates a new Limelight. */ +// public Limelight() { +// // cam = new PhotonCamera(VisionConstants.NAME); +// // cam.setDriverMode(false); +// } - public void setLEDs(boolean on) { - lightOn = on; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } +// public void setLEDs(boolean on) { +// lightOn = on; +// // cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); +// } - public void toggleLEDs() { - lightOn = !lightOn; - cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } +// public void toggleLEDs() { +// lightOn = !lightOn; +// // cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff); +// } - public void setDriverMode(boolean driverMode) { - cam.setDriverMode(driverMode); - } +// public void setDriverMode(boolean driverMode) { +// // cam.setDriverMode(driverMode); +// } - public void setToLimePipeline() { - cam.setPipelineIndex(1); - setLEDs(true); - } +// public void setToLimePipeline() { +// // cam.setPipelineIndex(1); +// setLEDs(true); +// } - public void setToAprilPipeline() { - cam.setPipelineIndex(0); - setLEDs(false); - } +// public void setToAprilPipeline() { +// // cam.setPipelineIndex(0); +// setLEDs(false); +// } - public PhotonTrackedTarget getAprilPoint() { - if (!cam.isConnected()) return null; +// public PhotonTrackedTarget getAprilPoint() { +// // if (!cam.isConnected()) return null; - PhotonPipelineResult result = cam.getLatestResult(); +// // PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) return null; +// // if (!result.hasTargets()) return null; - return result.getBestTarget(); - } +// return result.getBestTarget(); +// } - private List getAprilCorners() { - if (!cam.isConnected()) return null; +// private List getAprilCorners() { +// if (!cam.isConnected()) return null; - PhotonPipelineResult result = cam.getLatestResult(); +// PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) return null; +// if (!result.hasTargets()) return null; - return result.getBestTarget().getDetectedCorners(); - } +// return result.getBestTarget().getDetectedCorners(); +// } - public double getAprilSkew() { - List corners = getAprilCorners(); - ArrayList bottomSide = getAprilBottomSide(corners); +// public double getAprilSkew() { +// List corners = getAprilCorners(); +// ArrayList bottomSide = getAprilBottomSide(corners); - if (bottomSide == null) return 0; +// 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); +// 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; - } +// return bottomLeft.y - bottomRight.y; +// } - private ArrayList getAprilBottomSide(List box) { - if (box == null) return null; +// private ArrayList getAprilBottomSide(List box) { +// if (box == null) return null; - ArrayList bottomSide = new ArrayList<>(); +// ArrayList bottomSide = new ArrayList<>(); - TargetCorner l1 = new TargetCorner(-1, -1); - TargetCorner l2 = new TargetCorner(-1, -1); +// 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) l1 = c; +// } - for (TargetCorner c : box) { - if (c.y == l1.y) continue; - if (c.y > l2.y) l2 = c; - } +// for (TargetCorner c : box) { +// if (c.y == l1.y) continue; +// if (c.y > l2.y) l2 = c; +// } - bottomSide.add(l1); - bottomSide.add(l2); +// bottomSide.add(l1); +// bottomSide.add(l2); - return bottomSide; - } +// return bottomSide; +// } - public double getDistanceToApril() { - PhotonTrackedTarget aprilPoint = getAprilPoint(); - if (aprilPoint == null) return -1; +// 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 aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT; +// double theta = 35.0 + aprilPoint.getPitch(); - double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); - return distanceToApril; - } +// double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta)); +// return distanceToApril; +// } - public PhotonTrackedTarget getLowestTape() { - if (!cam.isConnected()) return null; +// public PhotonTrackedTarget getLowestTape() { +// if (!cam.isConnected()) return null; - PhotonPipelineResult result = cam.getLatestResult(); +// PhotonPipelineResult result = cam.getLatestResult(); - if (!result.hasTargets()) return null; +// if (!result.hasTargets()) return null; - ArrayList points = (ArrayList) result.getTargets(); +// ArrayList points = (ArrayList) result.getTargets(); - PhotonTrackedTarget lowest = points.get(0); - for (PhotonTrackedTarget point : points) { - if (point.getPitch() < lowest.getPitch()) { - lowest = point; - } - } +// PhotonTrackedTarget lowest = points.get(0); +// for (PhotonTrackedTarget point : points) { +// if (point.getPitch() < lowest.getPitch()) { +// lowest = point; +// } +// } - return lowest; - } +// return lowest; +// } - public double getDistanceToTape() { - PhotonTrackedTarget tapePoint = getLowestTape(); - if (tapePoint == null) return -1; +// 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 tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; +// double theta = 35.0 + tapePoint.getPitch(); - double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); - return distanceToTape; - } +// double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); +// return distanceToTape; +// } - @Override - public void periodic() {} -} +// @Override +// public void periodic() {} +// } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 965bbcb..adf7d5e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -53,9 +53,15 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } + public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ + double ang = (Math.atan2(rightStick.getY(), rightStick.getX()) / (Math.PI*2)); + System.out.println(ang); + module.go(ang); + } + boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - rightFront.go(leftStick); + // if (fieldRelative) { // double rot = 0; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index ef0d062..8d5322f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -14,12 +14,16 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource; // import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; // import com.ctre.phoenix.sensors.CANCoder; // import com.ctre.phoenix.sensors.SensorInitializationStrategy; - +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.hardware.CANcoder; @@ -38,6 +42,8 @@ public class SwerveModule extends SubsystemBase { private TalonFX driveMotor; private TalonFX angleMotor; private CANcoder encoder; + // private final StatusSignal cc_pos; + // private final StatusSignal cc_vel; // private int selfid; // private ConfigurableDouble offsetGetter; private static int swerveId = 0; @@ -49,19 +55,42 @@ public class SwerveModule extends SubsystemBase { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; + + // TalonFXConfiguration pidConfigs = new TalonFXConfiguration(); + // pidConfigs.Slot0.kP = 2.4; // An error of 1 rotation results in 2.4 V output + // pidConfigs.Slot0.kI = 0; // no output for integrated error + // pidConfigs.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output + + // angleMotor.getConfigurator().apply(slot0Configs); + + // var fx_cfg = new FeedbackConfigs(); + // fx_cfg.FeedbackRemoteSensorID = encoder.getDeviceID(); + // fx_cfg.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + // angleMotor.getConfigurator().apply(fx_cfg); // // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); // this.selfid = swerveId; // swerveId++; - // TalonFXConfiguration angleConfig = new TalonFXConfiguration(); - // angleConfig.slot0.kP = swerveGains.kP; - // angleConfig.slot0.kI = swerveGains.kI; - // angleConfig.slot0.kD = swerveGains.kD; + TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + // angleConfig.Slot0.kP = swerveGains.kP; + // angleConfig.Slot0.kI = swerveGains.kI; + // angleConfig.Slot0.kD = swerveGains.kD; + angleConfig.Slot0.kP = 2.4; // An error of 1 rotation results in 2.4 V output + angleConfig.Slot0.kI = 0; // no output for integrated error + angleConfig.Slot0.kD = 0.1; // A velocity of 1 rps results in 0.1 V output - // // use the CANcoder as the remote sensor for the primary TalonFX PID - // angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); - // angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; - // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; - // angleMotor.configAllSettings(angleConfig); + // var fx_cfg = new FeedbackConfigs(); + angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); + angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + // angleConfig.Feedback. = FeedbackDevice.RemoteSensor0; + // angleConfig.Feedback = fx_cfg; + + angleMotor.getConfigurator().apply(angleConfig); + + // use the CANcoder as the remote sensor for the primary TalonFX PID + // angleConfig.Fee = encoder.getDeviceID(); + // angleConfig.FeedbackSensorSource = RemoteSensorSource.CANCoder; + // angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + // angleMotor.getConfigurator().apply(angleConfig); // //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); // reset(0); @@ -71,9 +100,16 @@ public class SwerveModule extends SubsystemBase { // driveMotor.config_kP(0, 0.2); } - public void go(Translation2d leftStick){ - System.out.println(leftStick.getY()); - driveMotor.set(leftStick.getY()); + public void go(double ang){ + double curang = this.encoder.getAbsolutePosition().getValue(); + System.out.println(ang-curang); + + final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); + + // set position to 10 rotations + angleMotor.setControl(m_request.withPosition(ang)); + + // System.out.println(this.cc_pos.getValue()); } @Override @@ -166,26 +202,26 @@ public class SwerveModule extends SubsystemBase { * Set the speed and rotation of the SwerveModule from a SwerveModuleState object * @param desiredState a SwerveModuleState representing the desired new state of the module // */ - // public void setDesiredState(SwerveModuleState desiredState) { - // Rotation2d currentRotation = this.getAngle(); + public void setDesiredState(SwerveModuleState desiredState) { + // Rotation2d currentRotation = this.getAngle(); - // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + // SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); - // // calculate the difference between our current rotational position and our new rotational position - // Rotation2d rotationDelta = state.angle.minus(currentRotation); + // // calculate the difference between our current rotational position and our new rotational position + // Rotation2d rotationDelta = state.angle.minus(currentRotation); - // // calculate the new absolute position of the SwerveModule based on the difference in rotation - // double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + // // calculate the new absolute position of the SwerveModule based on the difference in rotation + // double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; - // // convert the CANCoder from its position reading to ticks - // double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + // // convert the CANCoder from its position reading to ticks + // double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); - // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + // angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); - // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + // double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - // driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - // } + // driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + } // public void reset(double position) { // encoder.setPositionToAbsolute(); diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index c940b75..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,42 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.1.1-beta-3.2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", - "version": "v2024.1.1-beta-3.2", - "libName": "Photon", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", - "version": "v2024.1.1-beta-3.2" - }, - { - "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", - "version": "v2024.1.1-beta-3.2" - } - ] -} \ No newline at end of file