Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-08 16:28:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

denver comp (im tweakin)

Browse Source
This commit is contained in:
Abhishrek05
2024-03-21 09:01:42 -06:00
parent 037d2172d7
commit 6b0e4b9cad
7 changed files with 388 additions and 296 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+164 -74
View File
@@ -7,7 +7,10 @@
package frc4388.robot; package frc4388.robot;
import org.opencv.video.Video;
import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.VideoMode;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID; 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.IntakeConstants;
import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.ShooterConstants; 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.Autos.PlaybackChooser;
import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickPlayback;
import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Swerve.JoystickRecorder;
@@ -87,7 +90,7 @@ public class RobotContainer {
private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
private Command interrupt = new InstantCommand(() -> {}, 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_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.idle()) 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))), // 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 // ! Teleop Commands
private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); //private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
private SequentialCommandGroup autoShoot = new SequentialCommandGroup( private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
// MoveToSpeaker, // MoveToSpeaker,
autoAlign, //autoAlign,
new InstantCommand(() -> m_robotShooter.spin()), new InstantCommand(() -> m_robotShooter.spin()),
new WaitCommand(3.0), new WaitCommand(3.0),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(3.0), new WaitCommand(3.0),
new InstantCommand(() -> m_robotShooter.idle()), new InstantCommand(() -> m_robotShooter.idle())
new InstantCommand(() -> autoAlign.reverse()), // new InstantCommand(() -> autoAlign.reverse()),
autoAlign.asProxy() // autoAlign.asProxy()
); );
@@ -290,7 +293,7 @@ public class RobotContainer {
DriverStation.silenceJoystickConnectionWarning(true); DriverStation.silenceJoystickConnectionWarning(true);
CameraServer.startAutomaticCapture(); CameraServer.startAutomaticCapture().setConfigJson("{ \"width\": -512, \"height\": 512 }");
/* Default Commands */ /* Default Commands */
// drives the robot with a two-axis input from the driver controller // 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) new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); .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())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180) new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()));
// * /* D-Pad Stuff */ // * /* D-Pad Stuff */
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
new Translation2d(0, 0), // new Translation2d(0, 0),
true))) // true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0), // new Translation2d(0, 0),
true))); // true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
new Translation2d(0, 0), // new Translation2d(0, 0),
true))) // true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0), // new Translation2d(0, 0),
true))); // true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
new Translation2d(0, 0), // new Translation2d(0, 0),
true))) // true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0), // new Translation2d(0, 0),
true))); // true)));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
new Translation2d(0, 0), // new Translation2d(0, 0),
true))) // true)))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
new Translation2d(0, 0), // new Translation2d(0, 0),
true))); // true)));
// ! /* Auto Recording */ // ! /* Auto Recording */
// new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
// .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
// new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
// () -> autoplaybackName.get())) () -> autoplaybackName.get()))
// .onFalse(new InstantCommand()); .onFalse(new InstantCommand());
// new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
// .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
// () -> autoplaybackName.get(), () -> autoplaybackName.get(),
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
// true, false)) true, false))
// .onFalse(new InstantCommand()); .onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive, // .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
@@ -475,25 +478,101 @@ public class RobotContainer {
} }
private void configureVirtualButtonBindings() { private void configureVirtualButtonBindings() {
/* Driver Buttons */
// new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
// /* Speed */ // ? /* Driver Buttons */
// 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 */
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())) .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor()));
@@ -506,37 +585,48 @@ public class RobotContainer {
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors()));
new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON) new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(emergencyRetract.asProxy()); .onTrue(emergencyRetract);
// Override Intake Position encoder: out // Override Intake Position encoder: out
new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON) 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 // Override Intake Position encoder: in
new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON) 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) new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter)) .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter))
.onFalse(turnOffShoot.asProxy()); .onFalse(turnOffShoot);
new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(i.asProxy()) .onTrue(i)
.onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn()));
//spins up shooter, no wind down //spins up shooter, no wind down
new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); .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)) // .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake));
new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) 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)));
} }
/** /**
src/main/java/frc4388/robot/commands/Autos/AutoAlign.java
+166 -166
View File
@@ -1,208 +1,208 @@
package frc4388.robot.commands.Autos; // package frc4388.robot.commands.Autos;
import frc4388.robot.subsystems.Limelight; // import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive; // import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.Constants.AutoAlignConstants; // import frc4388.robot.Constants.AutoAlignConstants;
import frc4388.robot.Constants.VisionConstants; // import frc4388.robot.Constants.VisionConstants;
import edu.wpi.first.wpilibj2.command.Command; // import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.math.geometry.Translation2d; // import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Rotation2d; // 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;
import edu.wpi.first.wpilibj.DriverStation.Alliance; // import edu.wpi.first.wpilibj.DriverStation.Alliance;
public class AutoAlign extends Command { // public class AutoAlign extends Command {
private SwerveDrive swerve; // private SwerveDrive swerve;
private Limelight limelight; // private Limelight limelight;
private Pose2d pose; // private Pose2d pose;
private Translation2d targetPos; // private Translation2d targetPos;
private Rotation2d targetRot; // private Rotation2d targetRot;
private Optional<Alliance> alliance; // private Optional<Alliance> alliance;
private boolean isRed; // private boolean isRed;
private boolean isFinished; // private boolean isFinished;
private boolean isReverseFinished; // private boolean isReverseFinished;
private boolean reverseAfterFinish; // private boolean reverseAfterFinish;
private Translation2d[] moveStickReplayArr; // private Translation2d[] moveStickReplayArr;
private Translation2d[] rotStickReplayArr; // private Translation2d[] rotStickReplayArr;
private int replayIndex; // private int replayIndex;
// PID Stuff // // PID Stuff
private double prevError; // private double prevError;
private double cumError; // private double cumError;
public AutoAlign(SwerveDrive swerve, Limelight limelight) { // public AutoAlign(SwerveDrive swerve, Limelight limelight) {
this.swerve = swerve; // this.swerve = swerve;
this.limelight = limelight; // this.limelight = limelight;
} // }
// Calc the closest point on a circle, to the center of the speaker // // Calc the closest point on a circle, to the center of the speaker
private Translation2d calcTargetPos(){ // private Translation2d calcTargetPos(){
final double R = VisionConstants.targetPosDistance; // final double R = VisionConstants.targetPosDistance;
final double cX; // final double cX;
final double cY; // final double cY;
if(isRed){ // if(isRed){
cX = VisionConstants.RedSpeakerCenter.getX(); // cX = VisionConstants.RedSpeakerCenter.getX();
cY = VisionConstants.RedSpeakerCenter.getY(); // cY = VisionConstants.RedSpeakerCenter.getY();
}else{ // }else{
cX = VisionConstants.BlueSpeakerCenter.getX(); // cX = VisionConstants.BlueSpeakerCenter.getX();
cY = VisionConstants.BlueSpeakerCenter.getY(); // cY = VisionConstants.BlueSpeakerCenter.getY();
} // }
final double pX = pose.getX(); // final double pX = pose.getX();
final double pY = pose.getY(); // 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 // // 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 vX = pX - cX;
double vY = pY - cY; // double vY = pY - cY;
double magV = Math.sqrt(vX*vX + vY*vY); // double magV = Math.sqrt(vX*vX + vY*vY);
double aX = cX + vX / magV * R; // double aX = cX + vX / magV * R;
double aY = cY + vY / 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 // // Calculate the angle facing the center, at the target rot
private Rotation2d calcTargetRot() { // private Rotation2d calcTargetRot() {
final double R = VisionConstants.targetPosDistance; // final double R = VisionConstants.targetPosDistance;
final double cX; // final double cX;
final double cY; // final double cY;
if(isRed){ // if(isRed){
cX = VisionConstants.RedSpeakerCenter.getX(); // cX = VisionConstants.RedSpeakerCenter.getX();
cY = VisionConstants.RedSpeakerCenter.getY(); // cY = VisionConstants.RedSpeakerCenter.getY();
}else{ // }else{
cX = VisionConstants.BlueSpeakerCenter.getX(); // cX = VisionConstants.BlueSpeakerCenter.getX();
cY = VisionConstants.BlueSpeakerCenter.getY(); // cY = VisionConstants.BlueSpeakerCenter.getY();
} // }
final double pX = pose.getX(); // final double pX = pose.getX();
final double pY = pose.getY(); // final double pY = pose.getY();
final double dX = pX - cX; // final double dX = pX - cX;
final double dY = pY - cY; // 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 // // Clamp to a circle, like an xbox controller
private Translation2d clamp(double oldX, double oldY){ // private Translation2d clamp(double oldX, double oldY){
// Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley // // 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 alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360;
final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); // final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI));
final double maxY = Math.abs(Math.sin(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 newX = Math.max(-maxX, Math.min(maxX, oldX));
final double newY = Math.max(-maxY, Math.min(maxY, oldY)); // final double newY = Math.max(-maxY, Math.min(maxY, oldY));
return new Translation2d(newX, newY); // return new Translation2d(newX, newY);
} // }
private Translation2d calcMoveStick(){ // private Translation2d calcMoveStick(){
final double curX = pose.getX(); // final double curX = pose.getX();
final double curY = pose.getY(); // final double curY = pose.getY();
// I think this might work, assuming the constants are good // // I think this might work, assuming the constants are good
double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; // double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed;
double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; // double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed;
return clamp(stickX, stickY); // return clamp(stickX, stickY);
} // }
private Translation2d calcRotStick(){ // private Translation2d calcRotStick(){
double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); // double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
cumError += error * .02; // 20 ms // cumError += error * .02; // 20 ms
double delta = error - prevError; // double delta = error - prevError;
final double kP = 4; // final double kP = 4;
final double kI = 4; // final double kI = 4;
final double kD = 4; // final double kD = 4;
final double kF = 4; // final double kF = 4;
double output = error * kP; // double output = error * kP;
output += cumError * kI; // output += cumError * kI;
output += delta * kD; // output += delta * kD;
output += kF; // output += kF;
prevError = error; // prevError = error;
return clamp(output, 0); // return clamp(output, 0);
} // }
public void reverse() { // public void reverse() {
this.reverseAfterFinish = true; // this.reverseAfterFinish = true;
} // }
// Called when the command is initially scheduled. // // Called when the command is initially scheduled.
@Override // @Override
public final void initialize() { // public final void initialize() {
isRed = alliance.get() == DriverStation.Alliance.Red; // isRed = alliance.get() == DriverStation.Alliance.Red;
if(reverseAfterFinish){ // if(reverseAfterFinish){
// isReverseFinished = false; // // isReverseFinished = false;
replayIndex = 0; // replayIndex = 0;
}else{ // }else{
moveStickReplayArr = new Translation2d[]{}; // moveStickReplayArr = new Translation2d[]{};
rotStickReplayArr = new Translation2d[]{}; // rotStickReplayArr = new Translation2d[]{};
} // }
} // }
// Called every time the scheduler runs while the command is scheduled. // // Called every time the scheduler runs while the command is scheduled.
@Override // @Override
public void execute() { // public void execute() {
// Update limelight pos // // Update limelight pos
pose = limelight.getPose(); // pose = limelight.getPose();
// These must be 0, or it will error // // These must be 0, or it will error
Translation2d moveStick = new Translation2d(0, 0); // Translation2d moveStick = new Translation2d(0, 0);
Translation2d rotStick = new Translation2d(0, 0); // Translation2d rotStick = new Translation2d(0, 0);
// Regular replay // // Regular replay
if(!isFinished){ // if(!isFinished){
targetPos = calcTargetPos(); // targetPos = calcTargetPos();
targetRot = calcTargetRot(); // targetRot = calcTargetRot();
moveStick = calcMoveStick(); // moveStick = calcMoveStick();
rotStick = calcRotStick(); // rotStick = calcRotStick();
// This is a way of appending... // // This is a way of appending...
moveStickReplayArr[moveStickReplayArr.length] = moveStick; // moveStickReplayArr[moveStickReplayArr.length] = moveStick;
rotStickReplayArr[rotStickReplayArr.length] = rotStick; // rotStickReplayArr[rotStickReplayArr.length] = rotStick;
// if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ // // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){
// replayIndex // // replayIndex
// } // // }
isFinished = limelight.isNearSpeaker(); // isFinished = limelight.isNearSpeaker();
// If reverseAfterFinish, then loop back over and replay in reverse // // If reverseAfterFinish, then loop back over and replay in reverse
}else if(reverseAfterFinish && !isReverseFinished){ // }else if(reverseAfterFinish && !isReverseFinished){
// Get reverse direction // // Get reverse direction
moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; // moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1];
rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; // rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1];
// Invert sticks // // Invert sticks
moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); // moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1);
rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); // rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1);
replayIndex++; // replayIndex++;
if(replayIndex >= moveStickReplayArr.length){ // if(replayIndex >= moveStickReplayArr.length){
reverseAfterFinish = true; // reverseAfterFinish = true;
} // }
} // }
// This would greatly benifit from having feild Relative implemented. // // This would greatly benifit from having feild Relative implemented.
swerve.driveWithInput(moveStick, rotStick, true); // swerve.driveWithInput(moveStick, rotStick, true);
} // }
// Returns true when the command should end. // // Returns true when the command should end.
@Override // @Override
public final boolean isFinished() { // public final boolean isFinished() {
return isFinished && (isReverseFinished || !reverseAfterFinish); // return isFinished && (isReverseFinished || !reverseAfterFinish);
} // }
} // }
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+1 -1
View File
@@ -68,7 +68,7 @@ public class JoystickRecorder extends CommandBase {
new Translation2d(inputs.rightX, inputs.rightY), new Translation2d(inputs.rightX, inputs.rightY),
true); true);
System.out.println("RECORDING"); //System.out.println("RECORDING");
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
src/main/java/frc4388/robot/subsystems/Intake.java
+4 -2
View File
@@ -117,9 +117,9 @@ public class Intake extends SubsystemBase {
// in init function, set slot 0 gains // in init function, set slot 0 gains
var slot0Configs = new Slot0Configs(); 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.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); talonPivot.getConfigurator().apply(slot0Configs);
@@ -336,5 +336,7 @@ public class Intake extends SubsystemBase {
// SmartDashboard.putNumber("Pivot Position", getArmPos()); // SmartDashboard.putNumber("Pivot Position", getArmPos());
//smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); //smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
//SmartDashboard.putBoolean("Limit Switch State", getTalonIntakeLimitSwitchState());
} }
} }
src/main/java/frc4388/robot/subsystems/Limelight.java
+45 -45
View File
@@ -18,65 +18,65 @@ import frc4388.robot.Constants.VisionConstants;
public class Limelight extends SubsystemBase { public class Limelight extends SubsystemBase {
// [X, Y, Z, Roll, Pitch, Yaw] // // [X, Y, Z, Roll, Pitch, Yaw]
private double[] cameraPose; // private double[] cameraPose;
private boolean isTag; // private boolean isTag;
private Pose2d pose; // private Pose2d pose;
private boolean isNearSpeaker; // private boolean isNearSpeaker;
public boolean getIsTag() { // public boolean getIsTag() {
return isTag; // return isTag;
} // }
private void update() { // private void update() {
SmartDashboard.putBoolean("Apriltag", isTag); // SmartDashboard.putBoolean("Apriltag", isTag);
if(!isTag){ // if(!isTag){
return; // return;
} // }
double x = cameraPose[0]; // double x = cameraPose[0];
double y = cameraPose[1]; // double y = cameraPose[1];
double yaw = cameraPose[5]; // 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){ // if(isRed){
distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); // distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter);
}else{ // }else{
distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); // distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter);
} // }
isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; // isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
//SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); // //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
//SmartDashboard.putNumber("speakerDistance", distance); // //SmartDashboard.putNumber("speakerDistance", distance);
} // }
public Pose2d getPose() { // public Pose2d getPose() {
return pose; // return pose;
} // }
public boolean isNearSpeaker() { // public boolean isNearSpeaker() {
return isNearSpeaker; // return isNearSpeaker;
} // }
@Override // @Override
public void periodic() { // public void periodic() {
// This method will be called once per scheduler run // // This method will be called once per scheduler run
//isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; // //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
//double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); // //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
//if(newPose != cameraPose){ // //if(newPose != cameraPose){
// cameraPose = newPose; // // cameraPose = newPose;
//update(); // //update();
//} // //}
} // }
} }
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+6 -6
View File
@@ -62,7 +62,7 @@ public class SwerveDrive extends SubsystemBase {
// ! drift correction // ! drift correction
if (rightStick.getNorm() > 0.05) { if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle(); rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; rot = rightStick.getX();
// SmartDashboard.putBoolean("drift correction", false); // SmartDashboard.putBoolean("drift correction", false);
stopped = false; stopped = false;
} else if(leftStick.getNorm() > 0.05) { } else if(leftStick.getNorm() > 0.05) {
@@ -144,7 +144,7 @@ public class SwerveDrive extends SubsystemBase {
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); 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. } else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); 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() { public void resetGyro() {
gyro.reset(); gyro.reset();
rotTarget = 0.0; rotTarget = gyro.getAngle();
} }
public void resetGyroFlip() { public void resetGyroFlip() {
gyro.resetFlip(); gyro.resetFlip();
rotTarget = 0.0; rotTarget = gyro.getAngle();
} }
public void resetGyroRightBlue() { public void resetGyroRightBlue() {
gyro.resetRightSideBlue(); gyro.resetRightSideBlue();
rotTarget = 0.0; rotTarget = gyro.getAngle();
} }
public void resetGyroRightAmp() { public void resetGyroRightAmp() {
gyro.resetAmpSide(); gyro.resetAmpSide();
rotTarget = 0.0; rotTarget = gyro.getAngle();
} }
public void stopModules() { public void stopModules() {
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+2 -2
View File
@@ -30,7 +30,7 @@ public class SwerveModule extends SubsystemBase {
private WPI_TalonFX angleMotor; private WPI_TalonFX angleMotor;
private CANCoder encoder; private CANCoder encoder;
private int selfid; private int selfid;
private ConfigurableDouble offsetGetter; // private ConfigurableDouble offsetGetter;
private static int swerveId = 0; private static int swerveId = 0;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
@@ -39,7 +39,7 @@ public class SwerveModule extends SubsystemBase {
this.driveMotor = driveMotor; this.driveMotor = driveMotor;
this.angleMotor = angleMotor; this.angleMotor = angleMotor;
this.encoder = encoder; this.encoder = encoder;
this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset);
this.selfid = swerveId; this.selfid = swerveId;
swerveId++; swerveId++;
TalonFXConfiguration angleConfig = new TalonFXConfiguration(); TalonFXConfiguration angleConfig = new TalonFXConfiguration();