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