From 334f286c2248ea7fee77d9c15c2f552c1ed8bdc5 Mon Sep 17 00:00:00 2001 From: C4llSiqn Date: Wed, 27 Nov 2024 17:18:59 -0700 Subject: [PATCH] L + lobotomy + checking for packet loss on a protocol that has no basic checks for packet loss --- src/main/java/frc4388/robot/Constants.java | 164 --------- .../java/frc4388/robot/RobotContainer.java | 236 +------------ src/main/java/frc4388/robot/RobotMap.java | 67 +--- .../robot/commands/Autos/AutoAlign.java | 208 ----------- .../robot/commands/Autos/PlaybackChooser.java | 98 ------ .../frc4388/robot/commands/Autos/Taxi.txt | 225 ------------ .../Autos/neo AutoRecoding format.txt | 20 -- .../commands/Autos/neoPlaybackChooser.java | 107 ------ .../robot/commands/Intake/ArmIntakeIn.java | 52 --- src/main/java/frc4388/robot/commands/PID.java | 60 ---- .../commands/Swerve/JoystickPlayback.java | 145 -------- .../commands/Swerve/JoystickRecorder.java | 97 ----- .../robot/commands/Swerve/RotateToAngle.java | 35 -- .../commands/Swerve/neoJoystickPlayback.java | 197 ----------- .../commands/Swerve/neoJoystickRecorder.java | 129 ------- .../commands/packetLossCheckerInator.java | 74 ++++ .../frc4388/robot/subsystems/Apriltags.java | 36 -- .../frc4388/robot/subsystems/Climber.java | 57 --- .../frc4388/robot/subsystems/DiffDrive.java | 88 ----- .../java/frc4388/robot/subsystems/Intake.java | 110 ------ .../java/frc4388/robot/subsystems/LED.java | 90 ----- .../frc4388/robot/subsystems/Limelight.java | 82 ----- .../frc4388/robot/subsystems/Shooter.java | 114 ------ .../frc4388/robot/subsystems/SwerveDrive.java | 333 ------------------ .../robot/subsystems/SwerveModule.java | 242 ------------- .../java/frc4388/robot/subsystems/Vision.java | 38 -- src/main/java/frc4388/utility/AprilTag.java | 13 - src/main/java/frc4388/utility/DataUtils.java | 35 -- .../frc4388/utility/DualJoystickButton.java | 22 -- src/main/java/frc4388/utility/Gains.java | 83 ----- .../java/frc4388/utility/LEDPatterns.java | 45 --- .../configurable/ConfigurableDouble.java | 23 -- .../configurable/ConfigurableString.java | 23 -- 33 files changed, 90 insertions(+), 3258 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/Autos/AutoAlign.java delete mode 100644 src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java delete mode 100644 src/main/java/frc4388/robot/commands/Autos/Taxi.txt delete mode 100644 src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt delete mode 100644 src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java delete mode 100644 src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java delete mode 100644 src/main/java/frc4388/robot/commands/PID.java delete mode 100644 src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java delete mode 100644 src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java delete mode 100644 src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java delete mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java delete mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java create mode 100644 src/main/java/frc4388/robot/commands/packetLossCheckerInator.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Apriltags.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java delete mode 100644 src/main/java/frc4388/robot/subsystems/DiffDrive.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Intake.java delete mode 100644 src/main/java/frc4388/robot/subsystems/LED.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Limelight.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Shooter.java delete mode 100644 src/main/java/frc4388/robot/subsystems/SwerveDrive.java delete mode 100644 src/main/java/frc4388/robot/subsystems/SwerveModule.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java delete mode 100644 src/main/java/frc4388/utility/AprilTag.java delete mode 100644 src/main/java/frc4388/utility/DataUtils.java delete mode 100644 src/main/java/frc4388/utility/DualJoystickButton.java delete mode 100644 src/main/java/frc4388/utility/Gains.java delete mode 100644 src/main/java/frc4388/utility/LEDPatterns.java delete mode 100644 src/main/java/frc4388/utility/configurable/ConfigurableDouble.java delete mode 100644 src/main/java/frc4388/utility/configurable/ConfigurableString.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7027b5e..82caa7d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,11 +7,6 @@ package frc4388.robot; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import frc4388.utility.Gains; -import frc4388.utility.LEDPatterns; - /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be @@ -21,168 +16,9 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final class SwerveDriveConstants { - - public static final double MAX_ROT_SPEED = 3.5; - public static final double AUTO_MAX_ROT_SPEED = 1.5; - public static final double MIN_ROT_SPEED = 1.0; - public static double ROTATION_SPEED = MAX_ROT_SPEED; - public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; - public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; - - public static final String CANBUS_NAME = "IDK"; - public static final double CORRECTION_MIN = 10; - public static final double CORRECTION_MAX = 50; - - public static final double[] GEARS = {0.25, 0.5, 1.0}; - - public static final double SLOW_SPEED = 0.25; - public static final double FAST_SPEED = 0.5; - public static final double TURBO_SPEED = 1.0; - - public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 + 0.5; - public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 + 0.5; - public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875 + 0.5; - public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 + 0.5; - } - - public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 2; - public static final int RIGHT_FRONT_STEER_ID = 3; - public static final int RIGHT_FRONT_ENCODER_ID = 10; - - public static final int LEFT_FRONT_WHEEL_ID = 4; - public static final int LEFT_FRONT_STEER_ID = 5; - public static final int LEFT_FRONT_ENCODER_ID = 11; - - public static final int LEFT_BACK_WHEEL_ID = 6; - public static final int LEFT_BACK_STEER_ID = 7; - public static final int LEFT_BACK_ENCODER_ID = 12; - - public static final int RIGHT_BACK_WHEEL_ID = 8; - public static final int RIGHT_BACK_STEER_ID = 9; - public static final int RIGHT_BACK_ENCODER_ID = 13; - - public static final int DRIVE_PIGEON_ID = 14; - } - - public static final class PIDConstants { - 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(50, 0.0, 0.32, 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 { - public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - - public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value - public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value - } - - public static final class Conversions { - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5; - - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; - public static final double MOTOR_REV_PER_STEER_REV = 12.8; - - public static final double TICKS_PER_MOTOR_REV = 0.5; - public static final double WHEEL_DIAMETER_INCHES = 3.9; - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; - - public static final double TICK_TIME_TO_SECONDS = 10; - public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; - } - - public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; - public static final double NEUTRAL_DEADBAND = 0.04; - } - - public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; - public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - - // dimensions - public static final double WIDTH = 18.5; - public static final double HEIGHT = 18.5; - public static final double HALF_WIDTH = WIDTH / 2.d; - public static final double HALF_HEIGHT = HEIGHT / 2.d; - - // misc - public static final int TIMEOUT_MS = 30; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - - public static final class VisionConstants { - // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value - - public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); - public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593); - - public static final double SpeakerBubbleDistance = 3; - public static final double targetPosDistance = 1.5; - - } - - public static final class DriveConstants { - - - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - - public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; - - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - } - - public static final class ShooterConstants { - public static final int LEFT_SHOOTER_ID = 15; // final - public static final int RIGHT_SHOOTER_ID = 16; // final - public static final double SHOOTER_SPEED = 0.50; // final - public static final double SHOOTER_IDLE = 0.20; // final - public static final double SHOOTER_IDLE_LIMELIGHT = 0.20; - } - - public static final class IntakeConstants { - public static final int INTAKE_MOTOR_ID = 18; - public static final int PIVOT_MOTOR_ID = 17; - public static final double INTAKE_SPEED = 0.75; - public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0; - public static final double INTAKE_OUT_SPEED_PRESSED = 0.5; - public static final double HANDOFF_SPEED = 0.4; - public static final double PIVOT_SPEED = 0.2; - - public static final class ArmPID { - public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0); - } - } - - public static final class ClimbConstants { - public static final int CLIMB_MOTOR_ID = 19; - public static final double CLIMB_IN_SPEED = -1.0; - public static final double CLIMB_OUT_SPEED = 0.87; - } - public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; - public static final int XBOX_OPERATOR_ID = 1; - public static final int XBOX_PROGRAMMER_ID = 2; public static final double LEFT_AXIS_DEADBAND = 0.1; - } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 10caf89..cac9981 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -9,37 +9,17 @@ package frc4388.robot; // Drive Systems import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.GenericHID; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.utility.controller.VirtualController; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.packetLossCheckerInator; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; // Commands import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -// Autos -import frc4388.robot.commands.Intake.ArmIntakeIn; -import frc4388.utility.controller.VirtualController; -import frc4388.robot.commands.Swerve.neoJoystickPlayback; -import frc4388.robot.commands.Swerve.neoJoystickRecorder; - -// Subsystems -import frc4388.robot.subsystems.Climber; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.Limelight; -import frc4388.robot.subsystems.Shooter; -// import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.SwerveDrive; - -// Utilites -import frc4388.utility.DeferredBlock; -import frc4388.utility.configurable.ConfigurableString; /** * This class is where the bulk of the robot should be declared. Since @@ -51,71 +31,11 @@ import frc4388.utility.configurable.ConfigurableString; public class RobotContainer { /* RobotMap */ public final RobotMap m_robotMap = new RobotMap(); - - /* Subsystems */ - // private final LED m_robotLED = new LED(); - - 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, - - m_robotMap.gyro); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID); - - 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); - - /* Virtual Controllers */ - private final VirtualController m_virtualDriver = new VirtualController(0); - private final VirtualController m_virtualOperator = new VirtualController(1); - - private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); - private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); - - // ! Teleop Commands - - private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.PIDIn()), - new InstantCommand(() -> m_robotShooter.idle()) - ); - - private SequentialCommandGroup intakeNotePullInIdle = new SequentialCommandGroup( - intakeToShootStuff, intakeToShoot, - new InstantCommand(() -> m_robotShooter.idle()) - ); - - 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) - ); - - // ! /* Autos */ - private String lastAutoName = "four_note_taxi_kracken.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 neoJoystickPlayback ampShoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto", - new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - false, true); + private final Command packetLostTester = new packetLossCheckerInator(m_robotMap.m_pigeon2); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -123,47 +43,8 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip()); DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); - - /* Default Commands */ - // ! Swerve Drive Default Command (Regular Rotation) - // drives the robot with a two-axis input from the driver controller - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(), - true); - }, m_robotSwerveDrive) - .withName("SwerveDrive DefaultCommand")); - m_robotSwerveDrive.setToSlow(); - - // ! Swerve Drive One Module Test - // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft()); - // } - - // ! Swerve Drive Default Command (Orientation Rotation) - // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(), - // getDeadbandedDriverController().getRightX(), - // getDeadbandedDriverController().getRightY(), - // true); - // }, 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)); - - // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // m_robotSwerveDrive.driveWithInput( - // getDeadbandedDriverController().getLeft(), - // getDeadbandedDriverController().getRight(), - // true); - // }, m_robotSwerveDrive)); - - - - } /** @@ -173,97 +54,10 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - // ? /* Driver Buttons */ - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip())); - - // ! /* Speed */ - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - - 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())); - - // ? /* Operator Buttons */ - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - - 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(intakeNotePullInIdle) - .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(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))); - - new Trigger(() -> getDeadbandedOperatorController().getPOV() != -1) - .onTrue(ampShoot) - .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter, m_robotSwerveDrive)); - - // ? /* Programer Buttons (Controller 3)*/ - - // * /* 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.A_BUTTON) + .onTrue(packetLostTester); } /** @@ -299,7 +93,7 @@ public class RobotContainer { */ public Command getAutonomousCommand() { //no auto - return autoPlayback; + return null; //return playbackChooser.getCommand(); // return new Command() {}; } @@ -318,15 +112,15 @@ public class RobotContainer { return this.m_driverXbox; } - public DeadbandedXboxController getDeadbandedOperatorController() { - return this.m_operatorXbox; - } + // public DeadbandedXboxController getDeadbandedOperatorController() { + // return this.m_operatorXbox; + // } - public VirtualController getVirtualDriverController() { - return m_virtualDriver; - } + // public VirtualController getVirtualDriverController() { + // return m_virtualDriver; + // } - public VirtualController getVirtualOperatorController() { - return m_virtualOperator; - } + // public VirtualController getVirtualOperatorController() { + // return m_virtualOperator; + // } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4d1869f..9c578ba 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,80 +7,15 @@ package frc4388.robot; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.ClimbConstants; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.subsystems.SwerveModule; -import frc4388.utility.RobotGyro; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit * testing and modularization. */ public class RobotMap { - private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID); - public RobotGyro gyro = new RobotGyro(m_pigeon2); - - public SwerveModule leftFront; - public SwerveModule rightFront; - public SwerveModule leftBack; - public SwerveModule rightBack; - - public RobotMap() { - configureLEDMotorControllers(); - configureDriveMotorControllers(); - } - - /* LED Subsystem */ - // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - - /* Swreve Drive Subsystem */ - public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); - public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); - public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); - - - public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); - public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); - public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); - - public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); - public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); - public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); - - public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); - public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); - public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); - - /* Shooter Subsystem */ - public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); - public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); - - /* Intake Subsystem */ - public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); - public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); - - /* Climber Subsystem */ - public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID); - - void configureLEDMotorControllers() { - } - - void configureIntakeMotorControllers() { - } - - void configureDriveMotorControllers() { - // initialize SwerveModules - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_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); - } + public Pigeon2 m_pigeon2 = new Pigeon2(14); } diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java deleted file mode 100644 index 58eb3ed..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ /dev/null @@ -1,208 +0,0 @@ -// package frc4388.robot.commands.Autos; -// import frc4388.robot.subsystems.Limelight; -// import frc4388.robot.subsystems.SwerveDrive; -// import frc4388.robot.Constants.AutoAlignConstants; -// import frc4388.robot.Constants.VisionConstants; -// import edu.wpi.first.wpilibj2.command.Command; - -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.geometry.Rotation2d; - -// import java.util.Optional; - -// import org.opencv.core.RotatedRect; - -// import edu.wpi.first.math.geometry.Pose2d; - -// import edu.wpi.first.wpilibj.DriverStation; -// import edu.wpi.first.wpilibj.DriverStation.Alliance; - -// public class AutoAlign extends Command { -// private SwerveDrive swerve; -// private Limelight limelight; -// private Pose2d pose; -// private Translation2d targetPos; -// private Rotation2d targetRot; - -// private Optional alliance; -// private boolean isRed; - -// private boolean isFinished; -// private boolean isReverseFinished; - -// private boolean reverseAfterFinish; -// private Translation2d[] moveStickReplayArr; -// private Translation2d[] rotStickReplayArr; -// private int replayIndex; - -// // PID Stuff -// private double prevError; -// private double cumError; - -// public AutoAlign(SwerveDrive swerve, Limelight limelight) { -// this.swerve = swerve; -// this.limelight = limelight; -// } - -// // Calc the closest point on a circle, to the center of the speaker -// private Translation2d calcTargetPos(){ -// final double R = VisionConstants.targetPosDistance; -// final double cX; -// final double cY; -// if(isRed){ -// cX = VisionConstants.RedSpeakerCenter.getX(); -// cY = VisionConstants.RedSpeakerCenter.getY(); -// }else{ -// cX = VisionConstants.BlueSpeakerCenter.getX(); -// cY = VisionConstants.BlueSpeakerCenter.getY(); -// } -// final double pX = pose.getX(); -// final double pY = pose.getY(); - -// // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point -// double vX = pX - cX; -// double vY = pY - cY; -// double magV = Math.sqrt(vX*vX + vY*vY); -// double aX = cX + vX / magV * R; -// double aY = cY + vY / magV * R; - -// return new Translation2d(aX, aY); -// } - -// // Calculate the angle facing the center, at the target rot -// private Rotation2d calcTargetRot() { -// final double R = VisionConstants.targetPosDistance; -// final double cX; -// final double cY; -// if(isRed){ -// cX = VisionConstants.RedSpeakerCenter.getX(); -// cY = VisionConstants.RedSpeakerCenter.getY(); -// }else{ -// cX = VisionConstants.BlueSpeakerCenter.getX(); -// cY = VisionConstants.BlueSpeakerCenter.getY(); -// } -// final double pX = pose.getX(); -// final double pY = pose.getY(); - -// final double dX = pX - cX; -// final double dY = pY - cY; - -// final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360); - -// return Rotation2d.fromDegrees(yaw); -// } - -// // Clamp to a circle, like an xbox controller -// private Translation2d clamp(double oldX, double oldY){ -// // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley -// final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360; -// final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); -// final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI)); - -// final double newX = Math.max(-maxX, Math.min(maxX, oldX)); -// final double newY = Math.max(-maxY, Math.min(maxY, oldY)); - -// return new Translation2d(newX, newY); -// } - -// private Translation2d calcMoveStick(){ -// final double curX = pose.getX(); -// final double curY = pose.getY(); - -// // I think this might work, assuming the constants are good -// double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; -// double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; - -// return clamp(stickX, stickY); -// } - -// private Translation2d calcRotStick(){ -// double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); -// cumError += error * .02; // 20 ms -// double delta = error - prevError; - -// final double kP = 4; -// final double kI = 4; -// final double kD = 4; -// final double kF = 4; - -// double output = error * kP; -// output += cumError * kI; -// output += delta * kD; -// output += kF; - -// prevError = error; -// return clamp(output, 0); -// } - -// public void reverse() { -// this.reverseAfterFinish = true; -// } - -// // Called when the command is initially scheduled. -// @Override -// public final void initialize() { -// isRed = alliance.get() == DriverStation.Alliance.Red; -// if(reverseAfterFinish){ -// // isReverseFinished = false; -// replayIndex = 0; -// }else{ -// moveStickReplayArr = new Translation2d[]{}; -// rotStickReplayArr = new Translation2d[]{}; -// } -// } - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { -// // Update limelight pos -// pose = limelight.getPose(); - -// // These must be 0, or it will error -// Translation2d moveStick = new Translation2d(0, 0); -// Translation2d rotStick = new Translation2d(0, 0); - -// // Regular replay -// if(!isFinished){ -// targetPos = calcTargetPos(); -// targetRot = calcTargetRot(); - -// moveStick = calcMoveStick(); -// rotStick = calcRotStick(); - -// // This is a way of appending... -// moveStickReplayArr[moveStickReplayArr.length] = moveStick; -// rotStickReplayArr[rotStickReplayArr.length] = rotStick; - -// // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ -// // replayIndex -// // } -// isFinished = limelight.isNearSpeaker(); - -// // If reverseAfterFinish, then loop back over and replay in reverse -// }else if(reverseAfterFinish && !isReverseFinished){ -// // Get reverse direction -// moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; -// rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; - -// // Invert sticks -// moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); -// rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); - -// replayIndex++; - -// if(replayIndex >= moveStickReplayArr.length){ -// reverseAfterFinish = true; -// } -// } - -// // This would greatly benifit from having feild Relative implemented. -// swerve.driveWithInput(moveStick, rotStick, true); -// } - -// // Returns true when the command should end. -// @Override -// public final boolean isFinished() { -// return isFinished && (isReverseFinished || !reverseAfterFinish); -// } -// } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java deleted file mode 100644 index f3d636d..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ /dev/null @@ -1,98 +0,0 @@ -package frc4388.robot.commands.Autos; - -import java.io.File; -import java.util.ArrayList; -import java.util.HashMap; - -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc4388.robot.commands.Swerve.JoystickPlayback; -import frc4388.robot.subsystems.SwerveDrive; - -public class PlaybackChooser { - private final ArrayList> m_choosers = new ArrayList<>(); - private SendableChooser m_playback = null; - private final ArrayList m_widgets = new ArrayList<>(); - private final HashMap m_commandPool = new HashMap<>(); - - private final File m_dir = new File("/home/lvuser/autos/"); - private int m_cmdNum = 0; - private final SwerveDrive m_swerve; - - // commands - private Command m_noAuto = new InstantCommand(); - - public PlaybackChooser(SwerveDrive swerve, Object... pool) { - m_swerve = swerve; - } - - public PlaybackChooser addOption(String name, Command option) { - m_commandPool.put(name, option); - return this; - } - - public PlaybackChooser buildDisplay() { - for (int i = 0; i < 10; i++) { - appendCommand(); - } - m_playback = m_choosers.get(0); - nextChooser(); - - // ! This does not work, why? - Shuffleboard.getTab("Auto Chooser") - .add("Add Sequence", new InstantCommand(() -> nextChooser())) - .withPosition(4, 0); - return this; - } - - // This will be bound to a button for the time being - public void appendCommand() { - var chooser = new SendableChooser(); - chooser.setDefaultOption("No Auto", m_noAuto); - - m_choosers.add(chooser); - ComplexWidget widget = Shuffleboard.getTab("Auto Chooser") - .add("Command: " + m_choosers.size(), chooser) - .withSize(4, 1) - .withPosition(0, m_choosers.size() - 1) - .withWidget(BuiltInWidgets.kSplitButtonChooser); - - m_widgets.add(widget); - } - - public void nextChooser() { - SendableChooser chooser = m_choosers.get(m_cmdNum++); - - String[] dirs = m_dir.list(); - - if(dirs != null){ // Fix funny error - for (String auto : dirs) { - chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); - } - } - - - for (var cmd_name : m_commandPool.keySet()) { - chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); - } - } - - public Command getCommand() { - Command command = m_playback.getSelected(); - command = command == null ? m_noAuto : command.asProxy(); - - Command[] commands = new Command[m_cmdNum - 1]; - for (int i = 0; i < m_cmdNum - 1; i++) { - Command command2 = m_choosers.get(i + 1).getSelected(); - command2 = command2 == null ? m_noAuto : command2.asProxy(); - - commands[i] = command2.asProxy(); - } - - return command.andThen(commands); - } -} diff --git a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt deleted file mode 100644 index c99ee2c..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt +++ /dev/null @@ -1,225 +0,0 @@ -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,0 -0.0,0.0,0.0,0.0,12 -0.0,0.0,0.0,0.0,26 -0.0,0.0,0.0,0.0,37 -0.0,0.0,0.0,0.0,50 -0.0,0.0,0.0,0.0,62 -0.0,0.0,0.0,0.0,73 -0.0,0.0,0.0,0.0,88 -0.0,0.0,0.0,0.0,103 -0.0,0.0,0.0,0.0,116 -0.0,0.0,0.0,0.0,160 -0.0,0.0,0.0,0.0,173 -0.0,0.0,0.0,0.0,185 -0.0,0.0,0.0,0.0,200 -0.0,0.0,0.0,0.0,211 -0.0,0.0,0.0,0.0,223 -0.0,0.0,0.0,0.0,235 -0.0,0.0,0.0,0.0,247 -0.0,0.0,0.0,0.0,263 -0.0,0.0,0.0,0.0,283 -0.0,0.0,0.0,0.0,303 -0.0,-0.109375,0.0,0.0,323 -0.0,-0.1484375,0.0,0.0,343 -0.0,-0.2109375,0.0,0.0,363 -0.0,-0.3671875,0.0,0.0,398 -0.0,-0.4140625,0.0,0.0,411 -0.0,-0.4765625,0.0,0.0,425 -0.0,-0.5078125,0.0,0.0,443 -0.0,-0.5078125,0.0,0.0,463 -0.0,-0.53125,0.0,0.0,483 -0.0,-0.5546875,0.0,0.0,504 -0.0,-0.5625,0.0,0.0,523 -0.0,-0.5625,0.0,0.0,544 -0.0,-0.5703125,0.0,0.0,563 -0.0,-0.5859375,0.0,0.0,584 -0.0,-0.5859375,0.0,0.0,603 -0.0,-0.5859375,0.0,0.0,640 -0.0,-0.59375,0.0,0.0,657 -0.0,-0.6015625,0.0,0.0,672 -0.0,-0.6015625,0.0,0.0,685 -0.0,-0.6015625,0.0,0.0,703 -0.0,-0.6015625,0.0,0.0,723 -0.0,-0.6015625,0.0,0.0,743 -0.0,-0.6015625,0.0,0.0,763 -0.0,-0.6015625,0.0,0.0,783 -0.0,-0.6015625,0.0,0.0,803 -0.0,-0.6015625,0.0,0.0,823 -0.0,-0.6015625,0.0,0.0,844 -0.0,-0.6015625,0.0,0.0,878 -0.0,-0.6015625,0.0,0.0,893 -0.0,-0.6015625,0.0,0.0,907 -0.0,-0.6015625,0.0,0.0,924 -0.0,-0.609375,0.0,0.0,943 -0.0,-0.609375,0.0,0.0,963 -0.0,-0.609375,0.0,0.0,983 -0.0,-0.609375,0.0,0.0,1004 -0.0,-0.609375,0.0,0.0,1023 -0.0,-0.609375,0.0,0.0,1043 -0.0,-0.609375,0.0,0.0,1064 -0.0,-0.609375,0.0,0.0,1083 -0.0,-0.609375,0.0,0.0,1156 -0.0,-0.609375,0.0,0.0,1172 -0.0,-0.609375,0.0,0.0,1185 -0.0,-0.609375,0.0,0.0,1200 -0.0,-0.609375,0.0,0.0,1215 -0.0,-0.609375,0.0,0.0,1225 -0.0,-0.609375,0.0,0.0,1236 -0.0,-0.609375,0.0,0.0,1249 -0.0,-0.609375,0.0,0.0,1263 -0.0,-0.609375,0.0,0.0,1283 -0.0,-0.609375,0.0,0.0,1303 -0.0,-0.609375,0.0,0.0,1323 -0.0,-0.609375,0.0,0.0,1363 -0.0,-0.6015625,0.0,0.0,1376 -0.0,-0.6015625,0.0,0.0,1394 -0.0,-0.6015625,0.0,0.0,1405 -0.0,-0.6015625,0.0,0.0,1423 -0.0,-0.6015625,0.0,0.0,1443 -0.0,-0.6015625,0.0,0.0,1463 -0.0,-0.6015625,0.0,0.0,1483 -0.0,-0.6015625,0.0,0.0,1503 -0.0,-0.6015625,0.0,0.0,1523 -0.0,-0.6015625,0.0,0.0,1543 -0.0,-0.6015625,0.0,0.0,1563 -0.0,-0.6015625,0.0,0.0,1597 -0.0,-0.6015625,0.0,0.0,1608 -0.0,-0.6015625,0.0,0.0,1624 -0.0,-0.6015625,0.0,0.0,1643 -0.0,-0.6015625,0.0,0.0,1664 -0.0,-0.5859375,0.0,0.0,1683 -0.0,-0.5859375,0.0,0.0,1703 -0.0,-0.5625,0.0,0.0,1723 -0.0,-0.5625,0.0,0.0,1743 -0.0,-0.5625,0.0,0.0,1763 -0.0,-0.5625,0.0,0.0,1783 -0.0,-0.5625,0.0,0.0,1803 -0.0,-0.5625,0.0,0.0,1843 -0.0,-0.5625,0.0,0.0,1855 -0.0,-0.5625,0.0,0.0,1868 -0.0,-0.5625,0.0,0.0,1883 -0.0,-0.5625,0.0,0.0,1903 -0.0,-0.5625,0.0,0.0,1923 -0.0,-0.5625,0.0,0.0,1943 -0.0,-0.5625,0.0,0.0,1963 -0.0,-0.5625,0.0,0.0,1983 -0.0,-0.5625,0.0,0.0,2003 -0.0,-0.5625,0.0,0.0,2024 -0.0,-0.5625,0.0,0.0,2043 -0.0,-0.5625,0.0,0.0,2081 -0.0,-0.5625,0.0,0.0,2093 -0.0,-0.5625,0.0,0.0,2105 -0.0,-0.5625,0.0,0.0,2123 -0.0,-0.5625,0.0,0.0,2143 -0.0,-0.5625,0.0,0.0,2163 -0.0,-0.5625,0.0,0.0,2183 -0.0,-0.5625,0.0,0.0,2203 -0.0,-0.5625,0.0,0.0,2223 -0.0,-0.5625,0.0,0.0,2243 -0.0,-0.5625,0.0,0.0,2263 -0.0,-0.5625,0.0,0.0,2283 -0.0,-0.5625,0.0,0.0,2366 -0.0,-0.5625,0.0,0.0,2377 -0.0,-0.5625,0.0,0.0,2394 -0.0,-0.5703125,0.0,0.0,2405 -0.0,-0.5703125,0.0,0.0,2418 -0.0,-0.5703125,0.0,0.0,2431 -0.0,-0.5703125,0.0,0.0,2444 -0.0,-0.5703125,0.0,0.0,2458 -0.0,-0.5703125,0.0,0.0,2470 -0.0,-0.5703125,0.0,0.0,2485 -0.0,-0.5703125,0.0,0.0,2503 -0.0,-0.5703125,0.0,0.0,2523 -0.0,-0.5703125,0.0,0.0,2563 -0.0,-0.5703125,0.0,0.0,2577 -0.0,-0.5703125,0.0,0.0,2591 -0.0,-0.5703125,0.0,0.0,2608 -0.0,-0.5703125,0.0,0.0,2624 -0.0,-0.5703125,0.0,0.0,2643 -0.0,-0.5703125,0.0,0.0,2677 -0.0,-0.5703125,0.0,0.0,2698 -0.0,-0.5703125,0.0,0.0,2711 -0.0,-0.5703125,0.0,0.0,2725 -0.0,-0.5703125,0.0,0.0,2743 -0.0,-0.5703125,0.0,0.0,2764 -0.0,-0.5703125,0.0,0.0,2810 -0.0,-0.5703125,0.0,0.0,2820 -0.0,-0.5703125,0.0,0.0,2833 -0.0,-0.5703125,0.0,0.0,2845 -0.0,-0.5703125,0.0,0.0,2863 -0.0,-0.5703125,0.0,0.0,2883 -0.0,-0.5703125,0.0,0.0,2904 -0.0,-0.5703125,0.0,0.0,2924 -0.0,-0.5703125,0.0,0.0,2943 -0.0,-0.5703125,0.0,0.0,2963 -0.0,-0.5703125,0.0,0.0,2983 -0.0,-0.5703125,0.0,0.0,3003 -0.0,-0.5703125,0.0,0.0,3033 -0.0,-0.5703125,0.0,0.0,3050 -0.0,-0.5703125,0.0,0.0,3065 -0.0,-0.5703125,0.0,0.0,3083 -0.0,-0.5703125,0.0,0.0,3103 -0.0,-0.5703125,0.0,0.0,3123 -0.0,-0.5703125,0.0,0.0,3144 -0.0,-0.5703125,0.0,0.0,3164 -0.0,-0.5703125,0.0,0.0,3184 -0.0,-0.5703125,0.0,0.0,3203 -0.0,-0.5703125,0.0,0.0,3223 -0.0,-0.5703125,0.0,0.0,3243 -0.0,-0.5703125,0.0,0.0,3272 -0.0,-0.5703125,0.0,0.0,3289 -0.0,-0.5703125,0.0,0.0,3303 -0.0,-0.5703125,0.0,0.0,3323 -0.0,-0.5703125,0.0,0.0,3343 -0.0,-0.5703125,0.0,0.0,3363 -0.0,-0.5703125,0.0,0.0,3383 -0.0,-0.5703125,0.0,0.0,3403 -0.0,-0.5703125,0.0,0.0,3423 -0.0,-0.5703125,0.0,0.0,3443 -0.0,-0.5703125,0.0,0.0,3463 -0.0,-0.5703125,0.0,0.0,3483 -0.0,-0.5703125,0.0,0.0,3566 -0.0,-0.5703125,0.0,0.0,3578 -0.0,-0.5703125,0.0,0.0,3596 -0.0,-0.5703125,0.0,0.0,3610 -0.0,-0.5703125,0.0,0.0,3623 -0.0,-0.5703125,0.0,0.0,3640 -0.0,-0.5703125,0.0,0.0,3651 -0.0,-0.5703125,0.0,0.0,3663 -0.0,-0.5703125,0.0,0.0,3678 -0.0,-0.5703125,0.0,0.0,3691 -0.0,-0.5703125,0.0,0.0,3706 -0.0,-0.5703125,0.0,0.0,3723 -0.0,-0.5703125,0.0,0.0,3766 -0.0,-0.5703125,0.0,0.0,3778 -0.0,-0.5703125,0.0,0.0,3792 -0.0,-0.5703125,0.0,0.0,3807 -0.0,-0.5703125,0.0,0.0,3823 -0.0,-0.5703125,0.0,0.0,3843 -0.0,-0.53125,0.0,0.0,3863 -0.0,-0.53125,0.0,0.0,3884 -0.0,-0.421875,0.0,0.0,3904 -0.0,0.0,0.0,0.0,3924 -0.0,0.0,0.0,0.0,3944 -0.0,0.0,0.0,0.0,3963 -0.0,0.0,0.0,0.0,3999 -0.0,0.0,0.0,0.0,4010 -0.0,0.0,0.0,0.0,4025 -0.0,0.0,0.0,0.0,4043 -0.0,0.0,0.0,0.0,4063 -0.0,0.0,0.0,0.0,4083 -0.0,0.0,0.0,0.0,4103 -0.0,0.0,0.0,0.0,4123 -0.0,0.0,0.0,0.0,4143 -0.0,0.0,0.0,0.0,4163 -0.0,0.0,0.0,0.0,4183 -0.0,0.0,0.0,0.0,4203 -0.0,0.0,0.0,0.0,4236 -0.0,0.0,0.0,0.0,4247 -0.0,0.0,0.0,0.0,4264 -0.0,0.0,0.0,0.0,4284 -0.0,0.0,0.0,0.0,4304 -0.0,0.0,0.0,0.0,4324 -0.0,0.0,0.0,0.0,4343 -0.0,0.0,0.0,0.0,4363 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt deleted file mode 100644 index a65aea9..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt +++ /dev/null @@ -1,20 +0,0 @@ -AUTO file format - -HEADER static size 0x5 -0x00 BYTE NUM AXES: defualts to 6 -0x01 BYTE NUM POV: defualts to 1 -0x02 BYTE NUM CONTROLLERS: defualts to 2 -0x03 SHORT FRAMES: any value greator or equal than one. - -FRAME PER CONTROLLER: defualt size 0x34 -0x00 DOUBLE AXES[NUM AXES] -0x30 SHORT BUTTONS -0x32 SHORT POVs[NUM POV] - -FRAME: size varrys -FRAME PER CONTROLLER[NUM CONTROLLERS] -INT UNIXTIMESTAMP - -FILE: -HEADER -FRAME[FRAMES] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java deleted file mode 100644 index 86bc7b2..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java +++ /dev/null @@ -1,107 +0,0 @@ -// package frc4388.robot.commands.Autos; - -// import java.io.File; -// import java.util.ArrayList; -// import java.util.HashMap; - -// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; -// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -// import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj2.command.InstantCommand; -// import frc4388.robot.commands.Swerve.JoystickPlayback; -// import frc4388.robot.commands.Swerve.neoJoystickPlayback; -// import frc4388.robot.subsystems.SwerveDrive; -// import frc4388.utility.controller.VirtualController; - -// public class neoPlaybackChooser { -// private final SendableChooser m_teamChosser = new SendableChooser(); -// private final SendableChooser m_possitionChosser = new SendableChooser(); -// private final SendableChooser m_autoNameChosser = new SendableChooser(); - -// private final SwerveDrive m_swerve; -// private final VirtualController[] m_controllers; -// // private final ArrayList> m_choosers = new ArrayList<>(); -// // private SendableChooser m_playback = null; -// private final ArrayList m_widgets = new ArrayList<>(); -// // private final HashMap m_commandPool = new HashMap<>(); - -// // private final File m_dir = new File("/home/lvuser/autos/"); -// // private int m_cmdNum = 0; - -// // commands -// private Command m_noAuto = new InstantCommand(); - -// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { -// m_teamChosser.addOption("Red", "red"); -// m_teamChosser.setDefaultOption("Blue", "blue"); -// m_teamChosser.addOption("Nuetral", "nuetral"); -// m_possitionChosser.addOption("AMP", "amp"); -// m_possitionChosser.setDefaultOption("Center", "center"); -// m_possitionChosser.addOption("Source", "source"); -// m_swerve = swerve; -// m_controllers = controllers; -// } - -// public neoPlaybackChooser addOption(String name, String option) { -// m_autoNameChosser.addOption(name, option); -// return this; -// } - -// // public PlaybackChooser buildDisplay() { -// // for (int i = 0; i < 10; i++) { -// // appendCommand(); -// // } -// // m_playback = m_choosers.get(0); -// // nextChooser(); - -// // // ! This does not work, why? -// // Shuffleboard.getTab("Auto Chooser") -// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) -// // .withPosition(4, 0); -// // return this; -// // } - -// // This will be bound to a button for the time being -// public void render() { -// // var chooser = new SendableChooser(); -// // // chooser.setDefaultOption("No Auto", m_noAuto); - -// // m_choosers.add(chooser); -// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") -// .add("Command: " + m_choosers.size(), chooser) -// .withSize(4, 1) -// .withPosition(0, m_choosers.size() - 1) -// .withWidget(BuiltInWidgets.kSplitButtonChooser) -// .withWidget(BuiltInWidgets.kComboBoxChooser); - - -// m_widgets.add(widget); -// } - -// // public void nextChooser() { -// // SendableChooser chooser = m_choosers.get(m_cmdNum++); - -// // String[] dirs = m_dir.list(); - -// // if(dirs != null){ // Fix funny error -// // for (String auto : dirs) { -// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); -// // } -// // } - - -// // for (var cmd_name : m_commandPool.keySet()) { -// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); -// // } -// // } - -// public String autoName() { -// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; -// } - -// public Command getCommand() { -// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); -// } -// } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java deleted file mode 100644 index 5fb161a..0000000 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ /dev/null @@ -1,52 +0,0 @@ -// 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.commands.Intake; - -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.Shooter; - -public class ArmIntakeIn extends Command { - /** Creates a new ArmIntakeIn. */ - private Intake robotIntake; - private Shooter robotShooter; - - public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) { - // Use addRequirements() here to declare subsystem dependencies. - this.robotIntake = robotIntake; - this.robotShooter = robotShooter; - - addRequirements(this.robotIntake, this.robotShooter); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - robotIntake.PIDOut(); - robotIntake.spinIntakeMotor(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return robotIntake.getIntakeLimitSwitchState(); - // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) - // { - // return !true==true; - // } - // else - // { - // return !false==!(!(true)); - // } - } -} diff --git a/src/main/java/frc4388/robot/commands/PID.java b/src/main/java/frc4388/robot/commands/PID.java deleted file mode 100644 index 97233f8..0000000 --- a/src/main/java/frc4388/robot/commands/PID.java +++ /dev/null @@ -1,60 +0,0 @@ -// 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.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.utility.Gains; - -public abstract class PID extends Command { - protected Gains gains; - private double output = 0; - private double tolerance = 0; - - /** Creates a new PelvicInflammatoryDisease. */ - public PID(double kp, double ki, double kd, double kf, double tolerance) { - gains = new Gains(kp, ki, kd, kf, 0); - this.tolerance = tolerance; - } - - public PID(Gains gains, double tolerance) { - this.gains = gains; - this.tolerance = tolerance; - } - - /** produces the error from the setpoint */ - public abstract double getError(); - - /** todo: javadoc */ - public abstract void runWithOutput(double output); - - // Called when the command is initially scheduled. - @Override - public final void initialize() { - output = 0; - } - - private double prevError, cumError = 0; - - // Called every time the scheduler runs while the command is scheduled. - @Override - public final void execute() { - double error = getError(); - cumError += error * .02; // 20 ms - double delta = error - prevError; - - output = error * gains.kP; - output += cumError * gains.kI; - output += delta * gains.kD; - output += gains.kF; - - runWithOutput(output); - } - - // Returns true when the command should end. - @Override - public final boolean isFinished() { - return Math.abs(getError()) < tolerance; - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java deleted file mode 100644 index ae054ed..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ /dev/null @@ -1,145 +0,0 @@ -// 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.commands.Swerve; - -import java.io.File; -import java.io.FileNotFoundException; -import java.util.ArrayList; -import java.util.Scanner; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.UtilityStructs.TimedOutput; - -public class JoystickPlayback extends Command { - private final SwerveDrive swerve; - private String filename; - private int mult = 1; - private Scanner input; - private final ArrayList outputs = new ArrayList<>(); - private int counter = 0; - private long startTime = 0; - private long playbackTime = 0; - private int lastIndex; - private boolean m_finished = false; // ! find a better way - - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { - this.swerve = swerve; - this.filename = filename; - this.mult = mult; - - addRequirements(this.swerve); - } - - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename) { - this(swerve, filename, 1); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - m_finished = false; - - startTime = System.currentTimeMillis(); - playbackTime = 0; - lastIndex = 0; - try { - input = new Scanner(new File("/home/lvuser/autos/" + filename)); - - String line = ""; - while (input.hasNextLine()) { - line = input.nextLine(); - - if (line.isEmpty() || line.isBlank() || line.equals("\n")) { - continue; - } - - String[] values = line.split(","); - System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); - - var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]) * mult; - out.leftY = Double.parseDouble(values[1]); - out.rightX = Double.parseDouble(values[2]); - out.rightY = Double.parseDouble(values[3]); - - out.timedOffset = Long.parseLong(values[4]); - - outputs.add(out); - } - - input.close(); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (counter == 0) { - startTime = System.currentTimeMillis(); - playbackTime = 0; - } else { - playbackTime = System.currentTimeMillis() - startTime; - } - - // skip to reasonable time frame - // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing - { - int i = lastIndex == 0 ? 1 : lastIndex; - while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { - i++; - } - - if (i >= outputs.size()) { - m_finished = true; // ! kind of a hack - return; - } - lastIndex = i; - } - - TimedOutput lastOut = outputs.get(lastIndex - 1); - TimedOutput out = outputs.get(lastIndex); - - double deltaTime = out.timedOffset - lastOut.timedOffset; - double playbackDelta = playbackTime - lastOut.timedOffset; - - double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); - double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); - double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); - double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); - - // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), - // new Translation2d(out.rightX, out.rightY), - // true); - - // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), - // new Translation2d(lerpRX, lerpRY), - // true); - - this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), - new Translation2d(lerpRX, lerpRY), - true); - - counter++; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - input.close(); - swerve.stopModules(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return m_finished; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java deleted file mode 100644 index 0224fcf..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ /dev/null @@ -1,97 +0,0 @@ -// 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.commands.Swerve; - -import java.io.File; -import java.io.IOException; -import java.io.PrintWriter; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.UtilityStructs.TimedOutput; - -public class JoystickRecorder extends Command { - public final SwerveDrive swerve; - - public final Supplier leftX; - public final Supplier leftY; - public final Supplier rightX; - public final Supplier rightY; - private String filename; - public final ArrayList outputs = new ArrayList<>(); - private long startTime = -1; - - - /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, - Supplier rightX, Supplier rightY, - String filename) - { - this.swerve = swerve; - this.leftX = leftX; - this.leftY = leftY; - this.rightX = rightX; - this.rightY = rightY; - this.filename = filename; - - addRequirements(this.swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - - this.startTime = System.currentTimeMillis(); - - outputs.add(new TimedOutput()); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - var inputs = new TimedOutput(); - inputs.leftX = leftX.get(); - inputs.leftY = leftY.get(); - inputs.rightX = rightX.get(); - inputs.rightY = rightY.get(); - inputs.timedOffset = System.currentTimeMillis() - startTime; - - outputs.add(inputs); - - swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY), - new Translation2d(inputs.rightX, inputs.rightY), - true); - - //System.out.println("RECORDING"); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - File output = new File("/home/lvuser/autos/" + filename); - - try (PrintWriter writer = new PrintWriter(output)) { - for (var input : outputs) { - writer.println( input.leftX + "," + input.leftY + "," + - input.rightX + "," + input.rightY + "," + - input.timedOffset); - } - - writer.close(); - } catch (IOException e) { - e.printStackTrace(); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java deleted file mode 100644 index a2945c0..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ /dev/null @@ -1,35 +0,0 @@ -// 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.commands.Swerve; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.SwerveDrive; - -public class RotateToAngle extends PID { - - SwerveDrive drive; - double targetAngle; - - /** Creates a new RotateToAngle. */ - public RotateToAngle(SwerveDrive drive, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.drive = drive; - this.targetAngle = targetAngle; - - addRequirements(drive); - } - - @Override - public double getError() { - return targetAngle - drive.getGyroAngle(); - } - - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java deleted file mode 100644 index 8b5afdf..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ /dev/null @@ -1,197 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileInputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; -import frc4388.utility.controller.VirtualController; - - -/** - * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickPlayback extends Command { - private final SwerveDrive swerve; - private final VirtualController[] controllers; - private final ArrayList frames = new ArrayList<>(); - private final Supplier filenameGetter; - private String filename; - private int frame_index = 0; - private long startTime = 0; - private long playbackTime = 0; - private boolean m_finished = false; // ! There is no better way. - private boolean m_shouldfree = false; // should free memory on ending - - private byte m_numAxes = 0; - private byte m_numPOVs = 0; - private byte m_numControllers = 0; - private short m_numFrames = -1; - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree Unloads the auto on compleation or intruption. - * @param instantload Load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this.swerve = swerve; - this.filenameGetter = filenameGetter; - this.controllers = controllers; - this.m_shouldfree = shouldfree; - - if (instantload) loadAuto(); - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree unloads the auto on compleation or intruption. - * @param instantload load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this(swerve, () -> filename, controllers, shouldfree, instantload); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { - this(swerve, filenameGetter, controllers, true, false); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { - this(swerve, () -> filename, controllers, true, false); - } - - /** - * Load the auto file from disk into memory - * @return Returns true if loading was successful, else wise; return false - * @implNote if the auto is already loaded, it will return true. - */ - public boolean loadAuto() { - filename = filenameGetter.get(); - try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { - if (m_numFrames != -1 && m_numFrames == frames.size()) { - System.out.println("AUTOPLAYBACK: Auto Already loaded."); - return true; - } - - m_numAxes = stream.readNBytes(1)[0]; - m_numPOVs = stream.readNBytes(1)[0]; - m_numControllers = stream.readNBytes(1)[0]; - m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); - - if (m_numControllers > controllers.length) { - System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers - + " virtual controllers but only " + controllers.length + " were given"); - return false; - } - - for (int i = 0; i < m_numFrames; i++) { - AutoRecordingFrame frame = new AutoRecordingFrame(); - for (int j = 0; j < m_numControllers; j++) { - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = new double[m_numAxes]; - for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. - axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); - } - short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); - short[] POV = new short[m_numPOVs]; - for (int k = 0; k < m_numPOVs; k++) { - POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); - } - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[j] = controllerFrame; - } - frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); - frames.add(frame); - } - - System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); - return true; - - } catch (Exception e) { - e.printStackTrace(); - System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); - return false; - } - } - - /** - * Unloads the auto. - */ - public void unloadAuto() { - System.out.println("AUTOPLAYBACK: Auto unloaded"); - frames.clear(); - } - - @Override - public void initialize() { - startTime = System.currentTimeMillis(); - playbackTime = 0; - frame_index = 0; - - m_finished = !loadAuto(); - } - - @Override - public void execute() { - if (frame_index >= m_numFrames) m_finished = true; - if (m_finished) return; - - // if (frame_index == 0) { - // startTime = System.currentTimeMillis(); - // playbackTime = 0; - // } else { - // playbackTime = System.currentTimeMillis() - startTime; - // } - - AutoRecordingFrame frame = frames.get(frame_index); - for (int i = 0; i < controllers.length; i++) { - AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; - controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); - if (i == 0) { - this.swerve.driveWithInput( - new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), - new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), - true); - } - } - frame_index++; - } - - @Override - public void end(boolean interrupted) { - for (VirtualController controller : controllers) controller.zeroControls(); - swerve.stopModules(); - if (m_shouldfree) unloadAuto(); - } - - @Override - public boolean isFinished() { - return m_finished; - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java deleted file mode 100644 index 7f48a6c..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ /dev/null @@ -1,129 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileOutputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; -import frc4388.utility.controller.DeadbandedXboxController; - -/** - * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickRecorder extends Command { - private final SwerveDrive swerve; - private final XboxController[] controllers; - private String filename; - private final Supplier filenameGetter; - private long startTime = -1; - private final ArrayList frames = new ArrayList<>(); - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { - this.swerve = swerve; - this.controllers = controllers; - this.filenameGetter = filenameGetter; - this.filename = ""; - - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filename a String containing the name of the auto file you wish to playback. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { - this(swerve, controllers, () -> filename); - } - - @Override - public void initialize() { - frames.clear(); - - this.startTime = System.currentTimeMillis(); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; - frames.add(frame); - this.filename = this.filenameGetter.get(); - } - - - @Override - public void execute() { - System.out.println("AUTORECORD: RECORDING"); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.timeStamp = (int) (System.currentTimeMillis() - startTime); - for (int i = 0; i < controllers.length; i++) { - XboxController controller = controllers[i]; - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = {controller.getLeftX(), controller.getLeftY(), - controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), - controller.getRightX(), controller.getRightY()}; - short button = 0; - for (int j = 0; j < 10; j++) - if (controller.getRawButton(j+1)) - button |= 1 << j; - short[] POV = {(short)(controller.getPOV())}; - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[i] = controllerFrame; - } - - frames.add(frame); - - swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), - new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), - true); // Really jank way of doing this. - - } - @Override - public void end(boolean interrupted) { - try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { - // header: size of 0x5 - // byte Number of axes per controller - // byte Number of POVs per controller - // byte Number of controllers - // short Number of frames - stream.write(new byte[]{6, 1, (byte) controllers.length}); - stream.write(DataUtils.shortToByteArray((short) frames.size())); - - // frame - // controller frame * number of controllers - // int unix time stamp. - for (AutoRecordingFrame frame : frames) { - // controller frame - // double axis * Number of axes per controller - // short button states - // short POV * Number of POVs per controller - for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { - for (double axis: controllerFrame.axes) { - stream.write(DataUtils.doubleToByteArray(axis)); - } - stream.write(DataUtils.shortToByteArray(controllerFrame.button)); - for (short POV: controllerFrame.POV) { - stream.write(DataUtils.shortToByteArray(POV)); - } - } - stream.write(DataUtils.intToByteArray(frame.timeStamp)); - } - System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); - } catch (Exception e) { - e.printStackTrace(); - } - } -} diff --git a/src/main/java/frc4388/robot/commands/packetLossCheckerInator.java b/src/main/java/frc4388/robot/commands/packetLossCheckerInator.java new file mode 100644 index 0000000..5b87ae1 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/packetLossCheckerInator.java @@ -0,0 +1,74 @@ +// 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.commands; + +import java.util.HashMap; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.wpilibj2.command.Command; + +// archaic communication protocols with modern problems require out of the box thinking +public class packetLossCheckerInator extends Command { + private final Pigeon2 pigeon; + private int startTime; + private final int amountOfCycles = 10_000; // Estemated at 200s of robot time. + private int lastAngle = 0; + private int errorCount = 0; + private int iterations = 0; + private HashMap statusMap; + /** Creates a new packetLossCheckerInator. */ + public packetLossCheckerInator(Pigeon2 pigeon) { + // Use addRequirements() here to declare subsystem dependencies. + this.pigeon = pigeon; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + System.out.println("### Test started, this will take a bit. ###"); + startTime = (int) System.currentTimeMillis(); + statusMap = new HashMap(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double angle = pigeon.getAngle(); + if ((int) Math.round(angle) != lastAngle) errorCount++; + + angle += 1; + lastAngle = (int) angle; + StatusCode e = pigeon.setYaw(lastAngle); + if (statusMap.containsKey(e.value)) statusMap.put(e.value, statusMap.get(e.value) + 1); + else statusMap.put(e.value, 1); //statusMap.get(e.value) + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + int tElapsed = (int) System.currentTimeMillis() - startTime; + System.out.println("### Test concluded ###"); + System.out.print("Obvious Falures: "); + System.out.println(errorCount); + System.out.print("Cycles"); + System.out.println(amountOfCycles); + System.out.print("Time it took: "); + System.out.println(tElapsed); + System.out.println("## Status map counts ##"); + for (int key : statusMap.keySet()) { + System.out.print(key); + System.out.print(": "); + System.out.println(statusMap.get(key)); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return iterations >= amountOfCycles; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java deleted file mode 100644 index c6062e8..0000000 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc4388.robot.subsystems; - -//import edu.wpi.first.apriltag.AprilTag; -//import edu.wpi.first.math.geometry.Pose3d; -//import edu.wpi.first.math.geometry.Rotation3d; -//import edu.wpi.first.networktables.NetworkTable; -//import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Apriltags { - public static class Tag { - public boolean visible = true; - public double x, y, z = 0; - public double ry, rp, rr = 0; - } - - public Tag getTagPosRot() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - final Tag tag = new Tag(); - tag.visible = isAprilTag(); - tag.x = tagTable.getEntry("TagPosX").getDouble(0); - tag.y = tagTable.getEntry("TagPosY").getDouble(0); - tag.z = tagTable.getEntry("TagPosZ").getDouble(0); - tag.ry = tagTable.getEntry("TagRotY").getDouble(0); - tag.rp = tagTable.getEntry("TagRotP").getDouble(0); - tag.rr = tagTable.getEntry("TagRotR").getDouble(0); - - return tag; - } - - public boolean isAprilTag() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return tagTable.getEntry("IsTag").getBoolean(false); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java deleted file mode 100644 index 5ed1472..0000000 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ /dev/null @@ -1,57 +0,0 @@ -// 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 com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ClimbConstants; - -public class Climber extends SubsystemBase { - /** Creates a new Climber. */ - TalonFX climbMotor; - - public Climber(TalonFX climbMotor) { - this.climbMotor = climbMotor; - this.climbMotor.setInverted(true); - - climbMotor.setNeutralMode(NeutralModeValue.Brake); - - var slot0Configs = new Slot0Configs(); - slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output - slot0Configs.kI = 0.0; // no output for integrated error - slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output - - climbMotor.getConfigurator().apply(slot0Configs); - } - - public void climbOut() { - //PositionVoltage request = new PositionVoltage(0); - //climbMotor.setControl(request.withPosition(-520)); - - climbMotor.set(ClimbConstants.CLIMB_OUT_SPEED); - } - - public void climbIn() { - //PositionVoltage request = new PositionVoltage(-520); - //climbMotor.setControl(request.withPosition(0)); - climbMotor.set(ClimbConstants.CLIMB_IN_SPEED); - } - - public void stopClimb() { - climbMotor.set(0.d); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - //SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue()); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java deleted file mode 100644 index 91de2e9..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,88 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.DriveConstants; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotTime; - -/** - * Add your docs here. - */ -public class DiffDrive extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private TalonFX m_leftFrontMotor; - private TalonFX m_rightFrontMotor; - private TalonFX m_leftBackMotor; - private TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - private RobotGyro m_gyro; - - /** - * Add your docs here. - */ - public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, - TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false)); - m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false)); - m_driveTrain = driveTrain; - m_gyro = gyro; - } - - @Override - public void periodic() { - m_gyro.updatePigeonDeltas(); - - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - //SmartDashboard.putData(m_gyro); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java deleted file mode 100644 index 5ffa72a..0000000 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ /dev/null @@ -1,110 +0,0 @@ -// 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 com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.utility.Gains; - -public class Intake extends SubsystemBase { - private TalonFX intakeMotor; - private TalonFX pivotMotor; - public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; - - /** Creates a new Intake. */ - public Intake(TalonFX intakeMotor, TalonFX pivotMotor) { - this.intakeMotor = intakeMotor; - this.pivotMotor = pivotMotor; - - intakeMotor.getConfigurator().apply(new TalonFXConfiguration()); - pivotMotor.getConfigurator().apply(new TalonFXConfiguration()); - - intakeMotor.setNeutralMode(NeutralModeValue.Brake); - pivotMotor.setNeutralMode(NeutralModeValue.Brake); - - // in init function, set slot 0 gains - var slot0Configs = new Slot0Configs(); - slot0Configs.kP = 1.3; // An error of 0.5 rotations results in 12 V output - slot0Configs.kI = 0.0; // no output for integrated error - slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output - - pivotMotor.getConfigurator().apply(slot0Configs); - } - - // ! Talon Methods - public void PIDIn() { - PIDPosition(0); - } - - public void PIDOut() { - PIDPosition(-53); - } - - public void PIDPosition(double pos) { - PositionVoltage request = new PositionVoltage(pos); - pivotMotor.setControl(request); - } - - public void handoff() { - intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - } - - public void spinIntakeMotor() { - intakeMotor.set(IntakeConstants.INTAKE_SPEED); - } - - public void spinIntakeMotor(double speed) { - intakeMotor.set(speed); - } - - public boolean getIntakeLimitSwitchState() { - return intakeMotor.getForwardLimit().getValue().value == 0; - } - - public void setPivotEncoderPosition(double val) { - pivotMotor.setPosition(val); - } - - public void stopIntakeMotors() { - intakeMotor.set(0); - } - - public void stopArmMotor() { - pivotMotor.set(0); - } - - public void stop() { - intakeMotor.set(0); - pivotMotor.set(0); - } - - public double getArmPos() { - return pivotMotor.getPosition().getValue(); - } - - public void resetArmPosition() { - if (getIntakeLimitSwitchState()) { - // talonPivot.setPosition(0); - } - } - - public void ampPosition() { - PIDPosition(-59); //TODO: Find actual value - } - - public void ampOuttake(double speed) { - spinIntakeMotor(speed); - } - - @Override - public void periodic() { - resetArmPosition(); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java deleted file mode 100644 index e9e070c..0000000 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ /dev/null @@ -1,90 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.LEDConstants; -import frc4388.utility.LEDPatterns; - -/** - * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED - * Driver - */ -public class LED extends SubsystemBase { - - static AddressableLED m_led; - static AddressableLEDBuffer m_ledBuffer; - static LED m_self; - /** - * Add your docs here. - */ - - public LED(){ - if(m_self != null) - return; - m_led = new AddressableLED(9); - m_ledBuffer = new AddressableLEDBuffer(10); - m_led.setLength(m_ledBuffer.getLength()); - m_led.setData(m_ledBuffer); - m_led.start(); - System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); - } - public static LED getInstance() { - if(m_self == null) - m_self = new LED(); - return m_self; - } - @Override - public void periodic(){ - //gamermode(); - //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); - return; - } - static int firstcolor = 0; - static void gamermode() { - for(int i = 0; i < m_ledBuffer.getLength(); i++) { - final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; - setLEDHSV(i, hue, 255, 128); - } - firstcolor +=3; - firstcolor %= 180; - } - /** - * Add your docs here. - */ - public static void updateLED (){ - gamermode(); - // m_LEDController.set(m_currentPattern.getValue()); - } - - /** - * Add your docs here. - */ - public static void setLEDRGB(int lednum, int r, int g, int b){ - m_ledBuffer.setRGB(lednum, r, g, b); - //m_currentPattern = pattern; - // m_LEDController.set(m_currentPattern.getValue()); - } - public static void setLEDHSV(int lednum, int hue, int sat, int val){ - m_ledBuffer.setRGB(lednum, hue, sat, val); - //m_currentPattern = pattern; - // m_LEDController.set(m_currentPattern.getValue()); - } - /** - * Add your docs here. - * @return - */ - public AddressableLEDBuffer getBuffer() { - return m_ledBuffer; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java deleted file mode 100644 index 2200b07..0000000 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ /dev/null @@ -1,82 +0,0 @@ -// 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 edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.VisionConstants; - -// Look at vvv for networktables stuff -// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data - -public class Limelight extends SubsystemBase { - - // // [X, Y, Z, Roll, Pitch, Yaw] - // private double[] cameraPose; - // private boolean isTag; - - // private Pose2d pose; - // private boolean isNearSpeaker; - - // public boolean getIsTag() { - // return isTag; - // } - - // private void update() { - // SmartDashboard.putBoolean("Apriltag", isTag); - // if(!isTag){ - // return; - // } - - // double x = cameraPose[0]; - // double y = cameraPose[1]; - // double yaw = cameraPose[5]; - - // Rotation2d rot = Rotation2d.fromDegrees(yaw); - - // pose = new Pose2d(x, y, rot); - - // boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; - - // double distance; - - // if(isRed){ - // distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); - // }else{ - // distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); - // } - - // isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; - - // //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); - // //SmartDashboard.putNumber("speakerDistance", distance); - // } - - // public Pose2d getPose() { - // return pose; - // } - - // public boolean isNearSpeaker() { - // return isNearSpeaker; - // } - - // @Override - // public void periodic() { - // // This method will be called once per scheduler run - - // //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; - // //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); - - // //if(newPose != cameraPose){ - // // cameraPose = newPose; - // //update(); - // //} - // } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java deleted file mode 100644 index fb80e19..0000000 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ /dev/null @@ -1,114 +0,0 @@ -// 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 edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.motorcontrol.Talon; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.Constants.ShooterConstants; - -import frc4388.robot.subsystems.Limelight; - -// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -// import com.ctre.phoenix.motorcontrol.NeutralMode; - - -public class Shooter extends SubsystemBase { - - private TalonFX leftShooter; - private TalonFX rightShooter; - - private Limelight limelight; - - private int spinMode = 0; - // 0 = Stop / Coast - // 1 = Idle - // 2 = Idle Near Speaker - // 3 = Spin - // 4 = SingleSpin - - private double smartDashboardShooterSpeed; - - /** Creates a new Shooter. */ - public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) { - leftShooter = leftTalonFX; - rightShooter = rightTalonFX; - - limelight = tmplimelight; - - leftShooter.setNeutralMode(NeutralModeValue.Coast); - rightShooter.setNeutralMode(NeutralModeValue.Coast); - SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); - - } - - public Shooter(TalonFX leftShooter) { - this.leftShooter = leftShooter; - leftShooter.setNeutralMode(NeutralModeValue.Coast); - } - - public void singleSpin() { - leftShooter.set(1.0); - spinMode = 4; - } - - public void singleSpin(double speed) { - leftShooter.set(speed); - spinMode = 4; - } - - public void spin() { - spin(smartDashboardShooterSpeed); - spinMode = 3; - } - - public void spin(double speed) { - leftShooter.set(-speed); - rightShooter.set(-speed); - spinMode = 3; - } - - public void spin(double leftSpeed, double rightSpeed) { - leftShooter.set(leftSpeed); - rightShooter.set(-rightSpeed); - spinMode = 3; - } - - public void stop() { - spin(0.d); - spinMode = 0; - } - - public void idle() { - spin(ShooterConstants.SHOOTER_IDLE); - spinMode = 1; - } - - public int getMode(){ - return spinMode; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); - //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); - //smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); - - // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed. - // Else if the robot is not near the speaker, then set the speed back to idle. - // if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){ - // leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); - // rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); - // spinMode = 2; - // }else if(!limelight.isNearSpeaker() && spinMode == 2){ - // idle(); - // } - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java deleted file mode 100644 index bd35742..0000000 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ /dev/null @@ -1,333 +0,0 @@ -// 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 edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.SwerveDriveConstants.Conversions; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotUnits; - -public class SwerveDrive extends SubsystemBase { - - private SwerveModule leftFront; - private SwerveModule rightFront; - private SwerveModule leftBack; - private SwerveModule rightBack; - - private SwerveModule[] modules; - - private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - - private RobotGyro gyro; - - private int gear_index; - private boolean stopped = false; - - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - - public double rotTarget = 0.0; - public Rotation2d orientRotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { - this.leftFront = leftFront; - this.rightFront = rightFront; - this.leftBack = leftBack; - this.rightBack = rightBack; - - this.gyro = gyro; - reset_index(); - 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()); - // rightStick.getAngle() - double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2)); - // System.out.println(ang); - // module.go(ang); - // Rotation2d rot = Rotation2d.fromRadians(ang); - Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY()); - SwerveModuleState state = new SwerveModuleState(speed, rot); - module.setDesiredState(state); - } - - public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - - double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; - SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); - - if (fieldRelative) { - - double rot = 0; - - // ! drift correction - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot_correction = 0; - // rot = rightStick.getX(); - // SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } - - // SmartDashboard.putBoolean("drift correction", true); - - // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - } - - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - - // Convert field-relative speeds to robot-relative speeds. - // chassisSpeeds = chassisSpeeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (fieldRelative) { - - double rot = 0; - - // ! drift correction - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - // SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } - - // SmartDashboard.putBoolean("drift correction", true); - // double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - - } - - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - - // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); - } - // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - - // Translation2d rightStick = new Translation2d(-rightX, rightY); - double rightX = rightStick.getX(); - double rightY = rightStick.getY(); - - double rot_correction = 0; - - // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - if(fieldRelative) { - double rot = 0; - if(rightStick.getNorm() > 0.5) { - orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); - - Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); - double min = tmp.getDegrees(); - min = Math.max(Math.abs(min), 2); - if(tmp.getDegrees() < 0) - min*=-1; - tmp = new Rotation2d(min * Math.PI / 180); - rot = tmp.getRadians(); // x x - y/x - } - - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - } - // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - - /** - * Set each module of the swerve drive to the corresponding desired state. - * @param desiredStates Array of module states to set. - */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - for (int i = 0; i < desiredStates.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state); - } - } - - public boolean rotateToTarget(double angle) { - double currentAngle = getGyroAngle(); - double error = angle - currentAngle; - - driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); - - if (Math.abs(angle - getGyroAngle()) < 5.0) { - return true; - } - - return false; - } - - public double getGyroAngle() { - return -gyro.getAngle(); - } - - public void add180() { - gyro.reset(gyro.getAngle()+180); - rotTarget = gyro.getAngle(); - - } - - public void resetGyro() { - gyro.reset(); - rotTarget = gyro.getAngle(); - } - - public void resetGyroFlip() { - gyro.resetFlip(); - rotTarget = gyro.getAngle(); - } - - public void resetGyroRightBlue() { - gyro.resetRightSideBlue(); - rotTarget = gyro.getAngle(); - } - - public void resetGyroRightAmp() { - gyro.resetAmpSide(); - rotTarget = gyro.getAngle(); - } - - public void stopModules() { - for (SwerveModule module : this.modules) { - module.stop(); - } - } - - public SwerveDriveKinematics getKinematics() { - return this.kinematics; - } - - public boolean getSpeedState() { - - return false; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); - SmartDashboard.putNumber("RotTartget", rotTarget); - } - - private void reset_index() { - gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?) - } - - public void shiftDown() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index - 1; - if (i == -1) i = 0; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; - } - - public void shiftUp() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index + 1; - if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1; - setPercentOutput(SwerveDriveConstants.GEARS[i]); - gear_index = i; - } - - public void setPercentOutput(double speed) { - speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed; - gear_index = -1; - } - - public void setToSlow() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED; - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - } - - public void setToFast() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED; - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - } - - public void setToTurbo() { - this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED; - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - } - - public void toggleGear(double angle) { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } else { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } - } - - public void shiftUpRot() { - rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; - } - - public void shiftDownRot() { - rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; - } - - - -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java deleted file mode 100644 index b9895fb..0000000 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ /dev/null @@ -1,242 +0,0 @@ -// 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 com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; -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.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.ctre.phoenix6.hardware.CANcoder; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.utility.Gains; - -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; - public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; - - - /** Creates a new SwerveModule. */ - public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) { - this.driveMotor = driveMotor; - this.angleMotor = angleMotor; - this.encoder = encoder; - - var motorCfg = new TalonFXConfiguration() - .withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ).withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(100) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(100) - .withSupplyCurrentLimitEnable(true) - ); - - driveMotor.getConfigurator().apply(motorCfg); - - TalonFXConfiguration angleConfig = new TalonFXConfiguration() - .withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ); - - angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - angleConfig.Slot0.kP = swerveGains.kP; - angleConfig.Slot0.kI = swerveGains.kI; - angleConfig.Slot0.kD = swerveGains.kD; - - angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID(); - angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - angleMotor.getConfigurator().apply(angleConfig); - - CANcoderConfiguration canconfig = new CANcoderConfiguration(); - canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; - canconfig.MagnetSensor.MagnetOffset = offset; - encoder.getConfigurator().apply(canconfig); - - rotateToAngle(0); - } - - // public void go(double ang){ - // // double curang = this.encoder.getAbsolutePosition().getValue(); - // System.out.println(getAngle().getDegrees()); - // rotateToAngle(ang); - // } - - @Override - public void periodic() { - //encoder.configMagnetOffset(offsetGetter.get()); - //SmartDashboard.putString("Error Code: " + selfid, getstuff()); - // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees()); - // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel()); - // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos()); - // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel()); - } - /** - * Get the drive motor of the SwerveModule - * @return the drive motor of the SwerveModule - */ - public TalonFX getDriveMotor() { - return this.driveMotor; - } - - /** - * Get the angle motor of the SwerveModule - * @return the angle motor of the SwerveModule - */ - public TalonFX getAngleMotor() { - return this.angleMotor; - } - - /** - * Get the CANcoder of the SwerveModule - * @return the CANcoder of the SwerveModule - */ - public CANcoder getEncoder() { - return this.encoder; - } - - /** - * Get the angle of a SwerveModule as a Rotation2d - * @return the angle of a SwerveModule as a Rotation2d - */ - public Rotation2d getAngle() { - // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. - // return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); - return Rotation2d.fromRotations(encoder.getPosition().getValue()); - } - - public double getAngularVel() { - // return this.angleMotor.getSelectedSensorVelocity(); - return angleMotor.getVelocity().getValueAsDouble(); - } - - public double getDrivePos() { - // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; - return driveMotor.getPosition().getValueAsDouble(); - } - - public double getDriveVel() { - // return this.driveMotor.getSelectedSensorVelocity(0); - return driveMotor.getVelocity().getValueAsDouble(); - } - - public void stop() { - driveMotor.set(0); - angleMotor.set(0); - } - - public void rotateToAngle(double angle) { - final PositionVoltage m_request = new PositionVoltage(angle); - angleMotor.setControl(m_request); - } - - /** - * Get state of swerve module - * @return speed in m/s and angle in degrees - */ - public SwerveModuleState getState() { - return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getVelocity().getValue() * - SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV * - SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV), - getAngle() - ); - } - - // private SwerveModuleState optimizeState(SwerveModuleState desiredState) { - // Rotation2d curRot = this.getAngle(); - - // } - - /** - * Returns the current position of the SwerveModule - * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. - // */ - // public SwerveModulePosition getPosition() { - // return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); - // } - - /** - * 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(); - - 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); - - double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND; - - rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations()); - - driveMotor.set(Math.max(Math.min(speed, 1.), -1.)); - } - - public void reset() { - // encoder.setPosition(0); - } - - // public double getCurrent() { - // return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); - // } - - // public double getVoltage() { - // return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); - // } - - // public String getstuff() { - // encoder.getPosition(); - // return "" + encoder.getLastError().value; - // } -} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 371f621..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc4388.robot.subsystems; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Vision { - private final NetworkTableEntry m_isTags; - private final NetworkTableEntry m_xPoses; - private final NetworkTableEntry m_yPoses; - private final NetworkTableEntry m_zPoses; - - public Vision() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - m_isTags = tagTable.getEntry("IsTag"); - m_xPoses = tagTable.getEntry("TagPosX"); - m_yPoses = tagTable.getEntry("TagPosY"); - m_zPoses = tagTable.getEntry("TagPosZ"); - } - - public AprilTag[] getAprilTags() { - if (!m_isTags.getBoolean(false)) return new AprilTag[0]; - - double xarr[] = m_xPoses.getDoubleArray(new double[] {}); - double yarr[] = m_yPoses.getDoubleArray(new double[] {}); - double zarr[] = m_zPoses.getDoubleArray(new double[] {}); - - AprilTag tags[] = new AprilTag[xarr.length]; - for (int i = 0; i < tags.length; i++) { - tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); - } - - return tags; - } -} diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java deleted file mode 100644 index c2ad269..0000000 --- a/src/main/java/frc4388/utility/AprilTag.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc4388.utility; - -// This is a seperate class in case I want to encode rotation or other -// information about the tag -public class AprilTag { - public final double x, y, z; - - public AprilTag(double _x, double _y, double _z) { - x = _x; - y = _y; - z = _z; - } -} diff --git a/src/main/java/frc4388/utility/DataUtils.java b/src/main/java/frc4388/utility/DataUtils.java deleted file mode 100644 index 3d998b6..0000000 --- a/src/main/java/frc4388/utility/DataUtils.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc4388.utility; - -import java.nio.ByteBuffer; - -public class DataUtils { - public static byte[] doubleToByteArray(double value) { - byte[] bytes = new byte[8]; - ByteBuffer.wrap(bytes).putDouble(value); - return bytes; - } - - public static double byteArrayToDouble(byte[] bytes) { - return ByteBuffer.wrap(bytes).getDouble(); - } - - public static byte[] intToByteArray(int value) { - byte[] bytes = new byte[4]; - ByteBuffer.wrap(bytes).putInt(value); - return bytes; - } - - public static int byteArrayToInt(byte[] bytes) { - return ByteBuffer.wrap(bytes).getInt(); - } - - public static byte[] shortToByteArray(short value) { - byte[] bytes = new byte[2]; - ByteBuffer.wrap(bytes).putShort(value); - return bytes; - } - - public static short byteArrayToShort(byte[] bytes) { - return ByteBuffer.wrap(bytes).getShort(); - } -} diff --git a/src/main/java/frc4388/utility/DualJoystickButton.java b/src/main/java/frc4388/utility/DualJoystickButton.java deleted file mode 100644 index e4ef5ed..0000000 --- a/src/main/java/frc4388/utility/DualJoystickButton.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc4388.utility; - -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.button.Trigger; - - -/** - * A button binding for two controllers, preferably an {@link frc4388.utility.controller.DeadbandedXboxController Xbox Controller} and {@link frc4388.utility.controller.VirtualController Virtual Xbox Controller} - * @author Zachary Wilke - */ -public class DualJoystickButton extends Trigger { - - /** - * Creates an Button binding on two controllers - * @param joystickA A controller - * @param joystickB A controller - * @param buttonNumber The button to bind to - */ - public DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) { - super(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber))); - } -} diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java deleted file mode 100644 index 7a3a026..0000000 --- a/src/main/java/frc4388/utility/Gains.java +++ /dev/null @@ -1,83 +0,0 @@ -// 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.utility; - -/** Add your docs here. */ -public class Gains { - public double kP; - public double kI; - public double kD; - public double kF; - public int kIZone; - public double kPeakOutput; - public double kMaxOutput; - public double kMinOutput; - - /** - * Creates Gains object for PIDs - * @param kP The P value. - * @param kI The I value. - * @param kD The D value. - * @param kF The F value. - * @param kIZone The zone of the I value. - * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. - */ - public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kF = kF; - this.kIZone = kIZone; - this.kPeakOutput = kPeakOutput; - this.kMaxOutput = kPeakOutput; - this.kMinOutput = -kPeakOutput; - } - - /** - * Creates Gains object for PIDs - * @param kP The P value. - * @param kI The I value. - * @param kD The D value. - * @param kF The F value. - * @param kIZone The zone of the I value. - */ - public Gains(double kP, double kI, double kD, double kF, int kIZone) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kF = kF; - this.kIZone = kIZone; - this.kPeakOutput = 1.0; - this.kMaxOutput = 1.0; - this.kMinOutput = -1.0; - } - - public Gains(double kP, double kI, double kD) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - } - - /** - * Creates Gains object for PIDs - * @param kP The P value. - * @param kI The I value. - * @param kD The D value. - * @param kF The F value. - * @param kIZone The zone of the I value. - * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. - * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. - */ - public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kF = kF; - this.kIZone = kIZone; - this.kMaxOutput = kMaxOutput; - this.kMinOutput = kMinOutput; - this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/LEDPatterns.java deleted file mode 100644 index 6df032c..0000000 --- a/src/main/java/frc4388/utility/LEDPatterns.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc4388.utility; - -/** - * Add your docs here. - */ -public enum LEDPatterns { - /* PALLETTE PATTERNS */ - RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), - RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f), - PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f), - PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f), - RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f), - RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f), - RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f), - BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f), - GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f), - - /* COLOR 1 PATTERNS */ - C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), - C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f), - - /* COLOR 2 PATTERNS */ - C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), - C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f), - - /* COLOR 1 AND 2 PATTERNS */ - C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), - C1C2_WAVES(0.53f), C1C2_SINELON(0.55f), - - /* SOLID COLORS */ - SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), - SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f), - SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f), - SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f), - SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f); - - /* GETTERS/SETTERS */ - private final float id; - LEDPatterns(float id) { - this.id = id; - } - public float getValue() { - return id; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java deleted file mode 100644 index c0384db..0000000 --- a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc4388.utility.configurable; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class ConfigurableDouble { - private double defualtValue; - private String name; - - /** - * Creates an new ConfigurableDouble through Smart Dashboard. - * @param name the name of the Smart Dashboard key. - * @param defualtValue the initilization value - */ - public ConfigurableDouble(String name, double defualtValue) { - this.name = name; - this.defualtValue = defualtValue; - SmartDashboard.putNumber(name, defualtValue); - } - - public double get() { - return SmartDashboard.getNumber(name, defualtValue); - } -} diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java deleted file mode 100644 index 34c0290..0000000 --- a/src/main/java/frc4388/utility/configurable/ConfigurableString.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc4388.utility.configurable; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class ConfigurableString { - private String defualtValue; - private String name; - - /** - * Creates an new ConfigurableString through Smart Dashboard. - * @param name the name of the Smart Dashboard key. - * @param defualtValue the initilization value - */ - public ConfigurableString(String name, String defualtValue) { - this.name = name; - this.defualtValue = defualtValue; - SmartDashboard.putString(name, defualtValue); - } - - public String get() { - return SmartDashboard.getString(name, defualtValue); - } -}