diff --git a/src/main/deploy/pathplanner/L Path.path b/src/main/deploy/pathplanner/L Path.path index 15b82fa..de23fbf 100644 --- a/src/main/deploy/pathplanner/L Path.path +++ b/src/main/deploy/pathplanner/L Path.path @@ -1,55 +1 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 2.0, - "y": 2.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 2.0 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false - }, - { - "anchorPoint": { - "x": 5.0, - "y": 2.0 - }, - "prevControl": { - "x": 4.000000000000002, - "y": 2.0 - }, - "nextControl": { - "x": 5.004218182347978, - "y": 2.0 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false - }, - { - "anchorPoint": { - "x": 5.0, - "y": 5.0 - }, - "prevControl": { - "x": 4.991875448297241, - "y": 5.0 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false - } - ], - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "isReversed": null -} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":5.0,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.000000000000002,"y":2.0},"nextControl":{"x":5.004218182347978,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.991875448297241,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path index b63994d..1e2c78c 100644 --- a/src/main/deploy/pathplanner/Move Forward.path +++ b/src/main/deploy/pathplanner/Move Forward.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":3.8761638155965237,"y":2.097178183811552},"prevControl":null,"nextControl":{"x":3.891027717524718,"y":2.097178183811552},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.989837747689554,"y":2.097178183811552},"prevControl":{"x":4.984427304969435,"y":2.097178183811552},"nextControl":{"x":4.984427304969435,"y":2.097178183811552},"holonomicAngle":91.52752544221289,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.8761638155965237,"y":2.140568077269722},"prevControl":{"x":3.3359301436282043,"y":2.140568077269722},"nextControl":null,"holonomicAngle":178.2100893917539,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":3.0,"y":2.0},"prevControl":null,"nextControl":{"x":3.0148639019281944,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.45976632803168,"y":2.0},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Rotate.path b/src/main/deploy/pathplanner/Rotate.path index 0707701..bc807e0 100644 --- a/src/main/deploy/pathplanner/Rotate.path +++ b/src/main/deploy/pathplanner/Rotate.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":0.05,"maxAcceleration":0.05,"isReversed":false} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Test Forward.path b/src/main/deploy/pathplanner/Test Forward.path new file mode 100644 index 0000000..428cb4a --- /dev/null +++ b/src/main/deploy/pathplanner/Test Forward.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":0.0,"y":0.0},"prevControl":null,"nextControl":{"x":1.0,"y":0.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":0.0},"prevControl":{"x":3.0,"y":0.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 650a28d..7101859 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,18 +46,18 @@ public final class Constants { public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant? // IDs - public static final int LEFT_FRONT_STEER_CAN_ID = 2; // * - public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // * - public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // * - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // * - public static final int LEFT_BACK_STEER_CAN_ID = 6; // * - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // * - public static final int RIGHT_BACK_STEER_CAN_ID = 8; // * - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // * - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // * - public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // * - public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // * - public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // * + public static final int LEFT_FRONT_STEER_CAN_ID = 2; + public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; + public static final int RIGHT_FRONT_STEER_CAN_ID = 4; + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; + public static final int LEFT_BACK_STEER_CAN_ID = 6; + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; + public static final int RIGHT_BACK_STEER_CAN_ID = 8; + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; + public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; + public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; public static final int GYRO_ID = 14; // offsets are in degrees @@ -85,8 +85,8 @@ public final class Constants { // swerve auto constants public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0); public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0); - public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3, - new TrapezoidProfile.Constraints(Math.PI, Math.PI)); + public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//0.1, 0.3, + new TrapezoidProfile.Constraints(Math.PI, Math.PI/2)); public static final boolean PATH_RECORD_VELOCITY = true; public static final double PATH_MAX_VELOCITY = 5.0; public static final double PATH_MAX_ACCELERATION = 5.0; @@ -326,8 +326,8 @@ public final class Constants { public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?) public static final double H_FOV = 59.6; public static final double V_FOV = 45.7; - public static final double LIME_HIXELS = 920; - public static final double LIME_VIXELS = 720; + public static final double LIME_HIXELS = 640; + public static final double LIME_VIXELS = 480; public static final double TURRET_kP = 0.1; public static final double POINTS_THRESHOLD = 400; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 4af5fb5..0606fd9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -46,7 +46,7 @@ public class Robot extends TimedRobot { private double current; - private static DesmosServer desmosServer = new DesmosServer(8000); + // private static DesmosServer desmosServer = new DesmosServer(8000); public static Alliance alliance; @@ -121,7 +121,7 @@ public class Robot extends TimedRobot { } }); - desmosServer.start(); + // desmosServer.start(); m_robotContainer.m_robotVisionOdometry.setLEDs(false); // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } @@ -214,8 +214,7 @@ public class Robot extends TimedRobot { * This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() { - } + public void autonomousPeriodic() {} @Override public void teleopInit() { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2182819..e5fdb9d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,24 +4,42 @@ package frc4388.robot; +import java.io.File; +import java.io.IOException; +import java.io.StringWriter; +import java.nio.file.FileSystems; +import java.nio.file.StandardWatchEventKinds; +import java.nio.file.WatchEvent; +import java.nio.file.WatchKey; +import java.time.ZonedDateTime; import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; import java.util.Objects; +import java.util.Optional; import java.util.logging.Logger; +import java.util.stream.Collectors; +import com.diffplug.common.base.Errors; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; import com.pathplanner.lib.commands.PPSwerveControllerCommand; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.NotifierCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -29,9 +47,8 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.PathRecorder; -// import frc4388.robot.commands.ButtonBoxCommands.TurretManual; +import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; -import frc4388.robot.commands.ShooterCommands.Shoot; import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Claws; @@ -44,8 +61,9 @@ import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.PathPlannerUtil; import frc4388.utility.controller.ButtonBox; -import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.utility.controller.DeadbandedRawXboxController; /** * This class is where the bulk of the robot should be declared. Since @@ -78,8 +96,8 @@ public class RobotContainer { /* Autonomous */ private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive); // Controllers - private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID); + private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); public static boolean softLimits = true; @@ -153,9 +171,11 @@ public class RobotContainer { if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); } if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); } }, m_robotClimber)); + + m_robotBoomBoom.setDefaultCommand( + new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom) + ); - // autoInit(); - // recordInit(); } /** @@ -165,104 +185,117 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - // Iterate over all Xbox controller buttons. - for (XboxController.Button binding : XboxController.Button.values()) { - /* ------------------------------------ Driver ------------------------------------ */ - JoystickButton button = new JoystickButton(getDriverController(), binding.value); - if (binding == XboxController.Button.kLeftBumper) - /* Left Bumper > Shift Down */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - else if (binding == XboxController.Button.kRightBumper) - /* Right Bumper > Shift Up */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - else if (binding == XboxController.Button.kLeftStick) - /* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kRightStick) - /* Right Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kA) - /* A > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kB) - /* B > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kX) - /* X > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kY) - /* Y > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kBack) - /* Back > Calibrate Odometry */ button.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - else if (binding == XboxController.Button.kStart) - /* Start > Calibrate Odometry */ button.whenPressed(m_robotSwerveDrive::resetGyro); + //! Driver Buttons - /* ------------------------------------ Operator ------------------------------------ */ - button = new JoystickButton(getDriverController(), binding.value); - if (binding == XboxController.Button.kLeftBumper) - /* Left Bumper > Storage In */ - button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - else if (binding == XboxController.Button.kRightBumper) - /* Right Bumper > Storage Out */ - button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - else if (binding == XboxController.Button.kLeftStick) - /* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kRightStick) - /* Right Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kA) - /* A > Spit Out Ball */ - button.whileHeld(new RunCommand(m_robotTurret::gotoMidpoint, m_robotTurret)) - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); - else if (binding == XboxController.Button.kB) - /* B > Toggle Claws */ button.whenPressed(new InstantCommand(m_robotClaws::toggleClaws, m_robotClaws)); - else if (binding == XboxController.Button.kX) - /* X > TEST BUTTON */ button.whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); - ///* X > Toggles extender in and out */ button.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - else if (binding == XboxController.Button.kY) - /* Y > TEST BUTTON */ button.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); - ///* Y > Full aim command */ button.whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); - else if (binding == XboxController.Button.kBack) - /* Back > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == XboxController.Button.kStart) - /* Start > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - } - // Iterate over all button box buttons. - for (ButtonBox.Button binding : ButtonBox.Button.values()) { - /* ---------------------------------- Button Box ---------------------------------- */ - JoystickButton button = new JoystickButton(getButtonBox(), binding.value); - if (binding == ButtonBox.Button.kRightSwitch) - /* Right Switch > Unbound */ button.whenPressed(new PrintCommand("Unbound")); - else if (binding == ButtonBox.Button.kMiddleSwitch) - /* Middle Switch > Climber and Shooter mode switching */ - button.whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); - else if (binding == ButtonBox.Button.kLeftSwitch) - /* Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) */ - button.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) - .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) - - .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) - .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) - - .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - - .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) - - .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + // Start > Calibrate Odometry + new JoystickButton(getDriverController(), XboxController.Button.kBack.value) + .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + // Start > Calibrate Odometry + new JoystickButton(getDriverController(), XboxController.Button.kStart.value) + .whenPressed(m_robotSwerveDrive::resetGyro); - .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - else if (binding == ButtonBox.Button.kRightButton) - /* Right Button > Extender Out */ - button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - else if (binding == ButtonBox.Button.kLeftButton) - /* Left Button > Extender In */ - button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - } + // Left Bumper > Shift Down + new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + + // Right Bumper > Shift Up + new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + + + //! Operator Buttons + + // Right Bumper > Storage Out + new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + // Left Bumper > Storage In + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + // B > Toggle claws + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); + + // X > Toggles extender in and out + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + + // A > Spit Out Ball + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); + + // Y > Full aim command + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); + + + //! Test Buttons + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, false)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); + + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + + + + //! Button Box Buttons + // Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) + .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) + + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) + + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) + + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + + .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + + // Middle Switch > Climber and Shooter mode switching + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + + // Left Button > Extender In + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + + // Left Button > Extender Out + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } /** @@ -288,8 +321,11 @@ public class RobotContainer { if (inputs[i] instanceof String) { PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel); - PathPlannerState initState = (PathPlannerState) traj.sample(0); - commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation)))); + PathPlannerState initState = traj.getInitialState(); + + Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation); + + commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive)); commands.add(new PPSwerveControllerCommand( traj, m_robotSwerveDrive::getOdometry, @@ -300,12 +336,13 @@ public class RobotContainer { m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive)); } + if (inputs[i] instanceof Command) { commands.add((Command) inputs[i]); } } - commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules())); + commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive)); Command[] ret = new Command[commands.size()]; ret = commands.toArray(ret); return ret; @@ -346,8 +383,38 @@ public class RobotContainer { // * null, // * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")), // * new RunCommand(() -> m_robotIntake.runAtOutput(0.5)))); + + // return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive), + // // new InstantCommand(() -> this.resetOdometry(new Pose2d())), + // new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive), + // "Diamond")); - return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command + // * assume turret is already pointed towards target. + // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), + // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), + // new ParallelRaceGroup( + // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + // new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage) + // )); + + // return new RunCommandForTime(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true), m_robotSwerveDrive), 1.0, true); + + return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), + new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.3, 0.0, 0.0}, 1.0));//, + // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + // new ParallelCommandGroup( + // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0) + //)); + + // * aim with RotateUntilTarget + // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), + // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), + // new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5), + // new ParallelRaceGroup( + // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + // new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage) + // )); } public static XboxController getDriverController() { diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java new file mode 100644 index 0000000..a2ce001 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java @@ -0,0 +1,63 @@ +// 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.DriveCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; + +public class DriveWithInputForTime extends CommandBase { + + // subsystems + SwerveDrive swerve; + + double[] inputs; + + // timing + long start; + long elapsed; + double duration; + + /** + * DriveWithInput for a specified amount of time. + * @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot). + * @param time Time to DriveWithInput for, in seconds. + */ + public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) { + // Use addRequirements() here to declare subsystem dependencies. + + this.swerve = swerve; + this.inputs = inputs; + this.duration = duration * 1000; // ! convert seconds to milliseconds, duh + + addRequirements(this.swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + start = System.currentTimeMillis(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + System.out.println("RUNNING"); + elapsed = System.currentTimeMillis() - start; + this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true); + } + + // 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() { + System.out.println("Duration: " + duration); + System.out.println("Elapsed: " + elapsed); + + return ((double) elapsed >= duration); + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java new file mode 100644 index 0000000..c7e0149 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java @@ -0,0 +1,59 @@ +// 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.DriveCommands; + +import javax.swing.plaf.basic.BasicTreeUI.TreeCancelEditingAction; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.VisionObscuredException; + +public class RotateUntilTarget extends CommandBase { + + // subsystems + SwerveDrive swerve; + VisionOdometry visionOdometry; + + double rotateSpeed; + + /** Creates a new RotateUntilTarget. */ + public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) { + // Use addRequirements() here to declare subsystem dependencies. + + this.swerve = swerve; + this.visionOdometry = visionOdometry; + + this.rotateSpeed = rotateSpeed; + + addRequirements(this.swerve, this.visionOdometry); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.visionOdometry.setLEDs(true); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true); + } + + // 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() { + + try { this.visionOdometry.getTargetPoints(); } catch (VisionObscuredException voe) { return false; } + + return true; + // return this.visionOdometry.m_camera.getLatestResult().hasTargets(); + } +} diff --git a/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java b/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java new file mode 100644 index 0000000..9827145 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java @@ -0,0 +1,156 @@ +// 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.io.File; +import java.io.IOException; +import java.util.Comparator; +import java.util.Map; +import java.util.Optional; +import java.util.function.Function; +import java.util.logging.Level; +import java.util.logging.Logger; +import java.util.regex.Pattern; +import java.util.stream.IntStream; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.CSV; + +public class PlaybackDriveInput extends CommandBase { + private static final Logger LOGGER = Logger.getLogger(PlaybackDriveInput.class.getSimpleName()); + + public static class DriveInputEntry { + public double millis, leftAxisX, leftAxisY, rightAxisX; + } + + private DriveInputEntry[] driveInputEntries; + + private SwerveDrive swerve; + private long startTime; + + /** Creates a new PlaybackDriveInput. */ + public PlaybackDriveInput(SwerveDrive swerve, String autoName) { + this.swerve = swerve; + + try { + // This is a helper class that allows us to read a CSV file into a Java array. + CSV csv = new CSV<>(DriveInputEntry::new) { + // This is a regular expression that removes all parentheses from the header of the CSV file. + private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + + /** + * Removes the parentheses from the CSV header + * + * @param header The header to be sanitized. + * @return The headerSanitizer method is overriding the headerSanitizer method in the parent class. + * The parentheses.matcher(header) is matching the header with the parentheses regular + * expression. The replaceAll method is replacing all of the parentheses with an empty + * string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling + * the parent sanitizer. + */ + @Override + protected String headerSanitizer(final String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + }; + // This is reading the CSV file into a Java array. + driveInputEntries = csv.read(new File(Filesystem.getDeployDirectory(), autoName + ".csv").toPath()); + // This is a running a helper method that is logging the contents of the table to the console on a new thread. + // ! new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(driveInputEntries, RobotBase.isSimulation()))).start(); + } catch (final IOException exception) { + DriveInputEntry dummyEntry = new DriveInputEntry(); + dummyEntry.millis = 0.0; + dummyEntry.leftAxisX = 0.0; + dummyEntry.leftAxisY = 0.0; + dummyEntry.rightAxisX = 0.0; + driveInputEntries = new DriveInputEntry[] { dummyEntry }; + LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception); + } + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + startTime = System.currentTimeMillis(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + long currentTime = System.currentTimeMillis() - startTime; + + double leftAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisX).doubleValue(); + double leftAxisY = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisY).doubleValue(); + double rightAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.rightAxisX).doubleValue(); + + swerve.driveWithInput(leftAxisX, leftAxisY, rightAxisX, true); + } + + /** + * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the + * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the + * interpolation (y) between the two values (y0 and y1) accquired by applying the target getter + * function to the lower and upper bounds entries. + * + * @param table An array of entries to search through. + * @param lookupValue The value to lookup in the table. + * @param lookupGetter A function that takes an entry from the table and returns . + * @param targetGetter A function that takes an E and returns a Number. + * @return The interpolated value. + */ + private static Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { + final Map.Entry closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1])); + final E closestRecord = closestEntry.getValue(); + final int closestRecordIndex = closestEntry.getKey(); + final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))]; + return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord)); + } + + /** + * If the value is in the table, return the entry. Otherwise, return the entry with the closest + * value + * + * @param table The array of values to search. + * @param value The value to search for. + * @param valueGetter A function that takes an element of the table and returns a Number to compare + * with the given value. + * @param exactMatch If true, the lookup will only return a match if the value is exactly equal to + * the value of the entry. If false, the lookup will return a match with a value closest to + * the given value. + * @return The entry with the closest value to the given value. + */ + private static Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { + final Optional> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue()))); + return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty(); + } + + /** + * Given a value x, and two values x0 and x1, and two values y0 and y1, return a value y that is a + * linear interpolation of the two values y0 and y1 + * + * @param x The value to interpolate. + * @param x0 the x coordinate of the lower bound to interpolate to + * @param y0 The value at x0. + * @param x1 the x-coordinate of the upper bound to interpolate to + * @param y1 The value at x1. + * @return The interpolation between y0 and y1 at x. + */ + private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) { + final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue()); + return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue(); + } + + // 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 false; + } +} diff --git a/src/main/java/frc4388/robot/commands/RecordDriveInput.java b/src/main/java/frc4388/robot/commands/RecordDriveInput.java new file mode 100644 index 0000000..cc120fc --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RecordDriveInput.java @@ -0,0 +1,107 @@ +// 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.io.File; +import java.io.FileOutputStream; +import java.io.PrintWriter; +import java.util.HashMap; +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; + +public class RecordDriveInput extends CommandBase { + private HashMap timedInput; + + private SwerveDrive swerve; + + private Supplier leftAxisXSupplier; + private Supplier leftAxisYSupplier; + private Supplier rightAxisXSupplier; + + private String autoName; + + private long startTime; + private long count; + + private int nthRecord; + + /** Creates a new RecordDriveInput. */ + public RecordDriveInput( + SwerveDrive swerve, + Supplier leftAxisXSupplier, + Supplier leftAxisYSupplier, + Supplier rightAxisXSupplier, + String autoName, + int nthRecord) + { + this.swerve = swerve; + this.leftAxisXSupplier = leftAxisXSupplier; + this.leftAxisYSupplier = leftAxisYSupplier; + this.rightAxisXSupplier = rightAxisXSupplier; + this.autoName = autoName; + this.nthRecord = nthRecord; + + timedInput = new HashMap<>(); + + addRequirements(this.swerve); + } + + public RecordDriveInput( + SwerveDrive swerve, + Supplier leftAxisXSupplier, + Supplier leftAxisYSupplier, + Supplier rightAxisXSupplier, + String autoName) + { + this(swerve, leftAxisXSupplier, leftAxisYSupplier, rightAxisXSupplier, autoName, 1); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + startTime = System.currentTimeMillis(); + count = 0; + + timedInput.put((long) 0, new double[] {0, 0, 0}); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + long timeFromStart = System.currentTimeMillis() - startTime; + double[] input = {leftAxisXSupplier.get(), leftAxisYSupplier.get(), rightAxisXSupplier.get()}; + + if(count % (long) nthRecord == 0) + timedInput.put(timeFromStart, input); + + swerve.driveWithInput(input[0], input[1], input[2], true); + count++; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + File csvOutput = new File(autoName + ".csv"); + try(PrintWriter writer = new PrintWriter(csvOutput)) { + writer.println("millis,leftx,lefty,rightx"); + + for(long millis : timedInput.keySet()) + writer.println(millis + "," + timedInput.get(millis)[0] + "," + timedInput.get(millis)[1] + "," + timedInput.get(millis)[2]); + + //// writer.println(( + 1) + ",0,0,0"); + writer.close(); + } catch(Exception 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/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java new file mode 100644 index 0000000..b66947d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunCommandForTime.java @@ -0,0 +1,81 @@ +// 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 edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class RunCommandForTime extends CommandBase { + + // command + Command command; + + // timing + long start; + long elapsed; + double duration; + + // override + boolean override; + + /** + * Runs given command for given time. + * @param command Command to run. + * @param duration Time to run for, in seconds. + * @param override If true: end command when time ends, even if the command isn't finished. If false: end command when it finished and time has ended. + */ + public RunCommandForTime(Command command, double duration, boolean override) { + // Use addRequirements() here to declare subsystem dependencies. + + this.command = command; + this.duration = duration * 1000; // ! convert seconds to milliseconds, duh + this.override = override; + + addRequirements(this.command.getRequirements().toArray(Subsystem[]::new)); + } + + /** + * Runs given command for given time. + * @param command Command to run. + * @param duration Time to run for, in seconds. + */ + public RunCommandForTime(Command command, double duration) { + this(command, duration, false); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.start = System.currentTimeMillis(); + this.command.initialize(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + System.out.println("RUNNING"); + this.elapsed = System.currentTimeMillis() - this.start; + this.command.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.command.end(interrupted); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (this.override) { + System.out.println("Duration: " + duration); + System.out.println("Elapsed: " + elapsed); + return (this.elapsed >= this.duration); + } else { + return (this.command.isFinished() && (this.elapsed >= this.duration)); + } + } +} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index 73f82d0..310b77e 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup { public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(swerve, turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 3c83331..0428899 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -35,6 +35,8 @@ public class TrackTarget extends CommandBase { BoomBoom m_boomBoom; Hood m_hood; + boolean isAuto; + static double velocity; static double hoodPosition; @@ -46,19 +48,35 @@ public class TrackTarget extends CommandBase { boolean isExecuted = false; - public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { - m_swerve = swerve; + // timing + boolean isAimed; + + boolean timerStarted; + long startTime; + private double timeTolerance; + + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) { m_turret = turret; m_boomBoom = boomBoom; m_hood = hood; m_visionOdometry = visionOdometry; + this.isAuto = isAuto; + this.timeTolerance = 1000; + addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); } + public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { + this(turret, boomBoom, hood, visionOdometry, false); + } + // Called when the command is initially scheduled. @Override public void initialize() { + timerStarted = false; + startTime = 0; + velocity = 0; hoodPosition = 0; } @@ -67,32 +85,34 @@ public class TrackTarget extends CommandBase { @Override public void execute() { try { + m_visionOdometry.setDriverMode(false); m_visionOdometry.setLEDs(true); points = m_visionOdometry.getTargetPoints(); - points = filterPoints(points); + // points = getFakePoints(); + //// points = filterPoints(points); Point average = VisionOdometry.averagePoint(points); double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; - output *= 2; + output *= 2.0; m_turret.runTurretWithInput(output); - double position = m_turret.m_boomBoomRotateEncoder.getPosition(); + // double position = m_turret.m_boomBoomRotateEncoder.getPosition(); - if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || - Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) - m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); - else - m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), - RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), - true); + // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || + // Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) + // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); + // else + // m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), + // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), + // true); double regressedDistance = getDistance(average.y); - // ! add 30 to the distance to get in target. May need to be adjusted - velocity = m_boomBoom.getVelocity(regressedDistance + 30); - hoodPosition = m_boomBoom.getHood(regressedDistance + 30); + // ! no longer a +30 lol -aarav + velocity = m_boomBoom.getVelocity(regressedDistance); + hoodPosition = m_boomBoom.getHood(regressedDistance); m_boomBoom.runDrumShooterVelocityPID(velocity); m_hood.runAngleAdjustPID(hoodPosition); @@ -110,6 +130,9 @@ public class TrackTarget extends CommandBase { } catch (Exception e){ e.printStackTrace(); } + + // run storage + } public ArrayList filterPoints(ArrayList points) { @@ -129,7 +152,18 @@ public class TrackTarget extends CommandBase { } final double averageDist = distanceSum / points.size(); - return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); + return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 1.3 * averageDist).collect(Collectors.toList()); + } + + public final ArrayList getFakePoints() { + ArrayList fakePoints = new ArrayList<>(); + + for(int i = 0; i < 10; i++) { + Point p = new Point((Math.random() * 20) - 10 + (VisionConstants.LIME_HIXELS/2), (Math.random() * 20) - 10 + (VisionConstants.LIME_VIXELS/2)); + fakePoints.add(p); + } + + return fakePoints; } public final double getDistance(double averageY) { @@ -142,7 +176,8 @@ public class TrackTarget extends CommandBase { double regressedDistance = distanceRegression(distance); regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; - + SmartDashboard.putNumber("Distance from Lime 123", distance); + SmartDashboard.putNumber("Regressed Distance from Lime 123", regressedDistance); return regressedDistance; } @@ -153,12 +188,25 @@ public class TrackTarget extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_visionOdometry.setLEDs(false); + m_visionOdometry.setDriverMode(true); } // Returns true when the command should end. @Override public boolean isFinished() { + //// if (this.isAuto) { + //// if (targetLocked& !timerStarted) { + //// timerStarted = true; + //// startTime = System.currentTimeMillis(); + //// } + //// return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + //// } else { + //// return false; + //// } + // // return isExecuted && Math.abs(output) < .1; + //// } + return false; - // return isExecuted && Math.abs(output) < .1; } } diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java b/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 7a6cf7b..6ef257c 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -94,9 +94,9 @@ public class BoomBoom extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0); - SmartDashboard.putNumber("Shooter Current", getCurrent()); - SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage()); - SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); + // SmartDashboard.putNumber("Shooter Current", getCurrent()); + // SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage()); + // SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); } public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { @@ -111,9 +111,9 @@ public class BoomBoom extends SubsystemBase { public void runDrumShooter(double speed) { // m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2); m_shooterFalconLeft.set(speed); - SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); - SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent()); - SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); + // SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); + // SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent()); + // SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6843e51..9c0429f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -156,6 +156,13 @@ public class SwerveDrive extends SubsystemBase { // modules[0].setDesiredState(desiredStates[0], false); } + public void setModuleRotationsToAngle(double angle) { + for (int i = 0; i < modules.length; i++) { + SwerveModule module = modules[i]; + module.rotateToAngle(angle); + } + } + @Override public void periodic() { @@ -226,10 +233,16 @@ public class SwerveDrive extends SubsystemBase { * Updates the field relative position of the robot. */ public void updateOdometry() { - m_poseEstimator.update( getRegGyro(), - modules[0].getState(), - modules[1].getState(), - modules[2].getState(), + Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2)); + Rotation2d actual = new Rotation2d(-1 * m_gyro.getRotation2d().getRadians()); + + SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees()); + SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees()); + + m_poseEstimator.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), + modules[0].getState(), + modules[1].getState(), + modules[2].getState(), modules[3].getState()); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index d4905af..59e9e13 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -182,6 +182,10 @@ public class SwerveModule extends SubsystemBase { angleMotor.set(0); } + public void rotateToAngle(double angle) { + this.angleMotor.set(TalonFXControlMode.Position, angle); + } + @Override public void periodic() { diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 315623d..0f8f0bd 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -47,6 +47,9 @@ public class VisionOdometry extends SubsystemBase { m_camera = new PhotonCamera(VisionConstants.NAME); m_drive = drive; m_shooter = shooter; + + setLEDs(false); + setDriverMode(true); } /** Gets the vision points from the limelight @@ -93,6 +96,10 @@ public class VisionOdometry extends SubsystemBase { m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); } + public void setDriverMode(boolean driverMode) { + m_camera.setDriverMode(driverMode); + } + public Point getTargetOffset() throws VisionObscuredException { ArrayList screenPoints = getTargetPoints();