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 9929191..80d5759 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -213,8 +213,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 c509da2..9bd41dc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -46,6 +46,8 @@ 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.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.Trajectory.State; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Filesystem; @@ -57,8 +59,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.NotifierCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; @@ -71,7 +75,8 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ClimberCommands.RunClaw; import frc4388.robot.commands.ClimberCommands.RunClimberPath; -// import frc4388.robot.commands.ButtonBoxCommands.TurretManual; +import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; +import frc4388.robot.commands.DriveCommands.RotateUntilTarget; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Seek; @@ -97,6 +102,7 @@ import frc4388.utility.PathPlannerUtil; import frc4388.utility.Vector2D; import frc4388.utility.PathPlannerUtil.Path.Waypoint; import frc4388.utility.controller.ButtonBox; +import frc4388.utility.controller.DeadbandedRawXboxController; import frc4388.utility.controller.DeadbandedXboxController; /** @@ -128,8 +134,8 @@ public class RobotContainer { public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); // 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); // Autonomous @@ -218,6 +224,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)) + ); + // autoInit(); // recordInit(); } @@ -267,8 +278,8 @@ public class RobotContainer { .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)); + 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) @@ -281,11 +292,11 @@ public class RobotContainer { //! 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) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); @@ -366,8 +377,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, @@ -378,12 +392,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; @@ -416,6 +431,13 @@ public class RobotContainer { // }).withName("No Autonomous Path"); // } + PIDController xController = SwerveDriveConstants.X_CONTROLLER; + PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + // thetaController.enableContinuousInput(-Math.PI, Math.PI); + + loadPath("Move Forward"); + // ! this will run each of the specified PathPlanner paths in sequence. // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")); @@ -424,8 +446,30 @@ 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) + )); + + // TODO: we should test TrackTarget timing with my RunCommandForTime thing at some point, same with DriveWithInput timing + + // * 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() { @@ -558,7 +602,7 @@ public class RobotContainer { } private void saveRecording() { - // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + // ! IMPORTANT: Had to chown the pathplanner folder in order to save autos. File outputFile = PATHPLANNER_DIRECTORY .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile(); LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath()); @@ -579,8 +623,7 @@ public class RobotContainer { public void recordPeriodic() { Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation(); Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); - // FIXME: Chassis speeds are created from joystick inputs and do not reflect - // actual robot velocity. + // * FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0], m_robotSwerveDrive.getChassisSpeeds()[1]); Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, 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..6f436b9 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.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 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() { + 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() { + return (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/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java new file mode 100644 index 0000000..5e35682 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunCommandForTime.java @@ -0,0 +1,78 @@ +// 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() { + 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) { + 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..7bf4fd4 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,33 @@ 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 = filterPoints(points); Point average = VisionOdometry.averagePoint(points); double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 2; 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,27 +129,30 @@ public class TrackTarget extends CommandBase { } catch (Exception e){ e.printStackTrace(); } + + // run storage + } - public ArrayList filterPoints(ArrayList points) { - Point average = VisionOdometry.averagePoint(points); + // public ArrayList filterPoints(ArrayList points) { + // Point average = VisionOdometry.averagePoint(points); - HashMap pointDistances = new HashMap<>(); - double distanceSum = 0; + // HashMap pointDistances = new HashMap<>(); + // double distanceSum = 0; - for(Point point : points) { - Vector2D difference = new Vector2D(point); - difference.subtract(new Vector2D(average)); + // for(Point point : points) { + // Vector2D difference = new Vector2D(point); + // difference.subtract(new Vector2D(average)); - double mag = difference.magnitude(); - distanceSum += mag; + // double mag = difference.magnitude(); + // distanceSum += mag; - pointDistances.put(point, mag); - } + // pointDistances.put(point, mag); + // } - final double averageDist = distanceSum / points.size(); - return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); - } + // final double averageDist = distanceSum / points.size(); + // return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); + // } public final double getDistance(double averageY) { double y_rot = averageY / VisionConstants.LIME_VIXELS; @@ -142,7 +164,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 +176,22 @@ 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() { - return false; + 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; } } 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/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ed9c193..f913873 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -158,6 +158,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() { @@ -228,10 +235,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();