diff --git a/build.gradle b/build.gradle index 13f3130..b8172b7 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.3.1" - id "com.peterabeles.gversion" version "latest.release" + id "com.peterabeles.gversion" version "1.10.2" } java { diff --git a/src/main/deploy/pathplanner/paths/Go to red speaker.path b/src/main/deploy/pathplanner/paths/Go to red speaker.path new file mode 100644 index 0000000..ad900d4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Go to red speaker.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 12.044476897540559, + "y": 2.055995525228976 + }, + "prevControl": null, + "nextControl": { + "x": 12.646589872786187, + "y": 2.276280760074938 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.78, + "y": 2.58 + }, + "prevControl": { + "x": 13.043103295508917, + "y": 2.511251677243963 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -3.453579912405768, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index 5bdff5d..c64503f 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -15,7 +15,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": 108.369, + "absoluteEncoderOffset": 107.842, "location": { "x": -12.3125, "y": 12.3125 diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 1f95f70..eb3172d 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -15,7 +15,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": 175.342, + "absoluteEncoderOffset": 178.330, "location": { "x": -12.3125, "y": -12.3125 diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index 5ae9afb..7ba9774 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -15,7 +15,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": 250.400, + "absoluteEncoderOffset": 250.049, "location": { "x": 12.3125, "y": 12.3125 diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 962e4f2..a815e97 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -15,7 +15,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": 255.850, + "absoluteEncoderOffset": 254.092, "location": { "x": 12.3125, "y": -12.3125 diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index 44b532c..9be10f3 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -2,15 +2,15 @@ "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, "currentLimit": { - "drive": 20, - "angle": 10 + "drive": 40, + "angle": 20 }, "conversionFactor": { "angle": 16.8, "drive": 0.047286787200699704 }, "rampRate": { - "drive": 0.175, - "angle": 0.175 + "drive": 0.25, + "angle": 0.25 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/LocalADStarAK.java b/src/main/java/frc/robot/LocalADStarAK.java new file mode 100644 index 0000000..6f2a86a --- /dev/null +++ b/src/main/java/frc/robot/LocalADStarAK.java @@ -0,0 +1,149 @@ +package frc.robot; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.Pathfinder; +import com.pathplanner.lib.pathfinding.LocalADStar; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 705e907..c4c1d19 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import com.pathplanner.lib.pathfinding.Pathfinding; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.littletonrobotics.junction.LogFileUtil; @@ -13,6 +15,7 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import frc.robot.util.Constants; +import org.littletonrobotics.urcl.URCL; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -27,7 +30,10 @@ public class Robot extends LoggedRobot { @Override public void robotInit() { + DataLogManager.start(); + URCL.start(); // Record metadata + Pathfinding.setPathfinder(new LocalADStarAK()); Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5f89af3..76f633f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,10 +4,13 @@ package frc.robot; +import com.revrobotics.ColorSensorV3; import edu.wpi.first.math.MathUtil; 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.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -17,11 +20,13 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.DriveToShootingPositionCommand; +import frc.robot.commands.AimAndPickUpNoteCommand; import frc.robot.commands.IntakeCommand; import frc.robot.commands.RotateCommand; import frc.robot.commands.ShootCommand; +import frc.robot.commands.drivebase.AimAtLimelightCommand; import frc.robot.commands.drivebase.AimAtTargetCommand; +import frc.robot.subsystems.VisionSubsystem; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIOSparkMax; import frc.robot.subsystems.shooter.Shooter; @@ -42,14 +47,10 @@ public class RobotContainer { private final SwerveSubsystem drivebase; private final Intake intake; private final Shooter shooter; - private final Command intakeCommand, shootCommand, aimAtTargetCommand, driveToShootingPositionCommand, rotateCommand; + private final Command intakeCommand, shootCommand, aimAtLimelightCommand, rotateCommand, driveToPoseCommand, aimAndPickUpNoteCommand; - // CommandJoystick rotationController = new CommandJoystick(1); - // Replace with CommandPS4Controller or CommandJoystick if needed - CommandJoystick driverController = new CommandJoystick(1); - - // CommandJoystick driverController = new CommandJoystick(3);//(OperatorConstants.DRIVER_CONTROLLER_PORT); XboxController driverXbox = new XboxController(0); + XboxController shooterXbox = new XboxController(1); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -58,59 +59,53 @@ public RobotContainer() { drivebase = SwerveSubsystem.getInstance(); intake = new Intake(new IntakeIOSparkMax()); shooter = new Shooter(new ShooterIOSparkFlex()); - intakeCommand = new IntakeCommand(intake, () -> MathUtil.applyDeadband(driverXbox.getLeftTriggerAxis(), 0.1)); - shootCommand = new ShootCommand(shooter, () -> MathUtil.applyDeadband(driverXbox.getRightTriggerAxis(), 0.1) * 4000.0); - aimAtTargetCommand = new AimAtTargetCommand(drivebase); - driveToShootingPositionCommand = new DriveToShootingPositionCommand(drivebase); + intakeCommand = new IntakeCommand(intake, () -> MathUtil.applyDeadband(shooterXbox.getLeftTriggerAxis(), 0.1)); + shootCommand = new ShootCommand(shooter, () -> MathUtil.applyDeadband(shooterXbox.getRightTriggerAxis(), 0.1) * 3250); + aimAtLimelightCommand = new AimAtLimelightCommand(drivebase, VisionSubsystem.getInstance()); rotateCommand = new RotateCommand(drivebase); - - - - AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), - OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), - OperatorConstants.LEFT_X_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getRightX(), - OperatorConstants.RIGHT_X_DEADBAND), - driverXbox::getYButtonPressed, - driverXbox::getAButtonPressed, - driverXbox::getXButtonPressed, - driverXbox::getBButtonPressed); + driveToPoseCommand = new ShootCommand(shooter, () -> 4000).alongWith(drivebase.driveToPose(new Pose2d(new Translation2d(2.720, 2.579), Rotation2d.fromDegrees(180))).andThen(drivebase.driveToPose(new Pose2d(new Translation2d(2.720, 2.579), Rotation2d.fromDegrees(180)))).andThen(new AimAtLimelightCommand(drivebase, VisionSubsystem.getInstance())).andThen(new IntakeCommand(intake, () -> 1))); + aimAndPickUpNoteCommand = new AimAndPickUpNoteCommand(drivebase, VisionSubsystem.getInstance(), intake); + int invert = getInvert(); // Applies deadbands and inverts controls because joysticks // are back-right positive while robot // controls are front-left positive // left stick controls translation // right stick controls the desired angle NOT angular rotation - Command driveFieldOrientedDirectAngle = drivebase.driveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRightX(), - () -> driverXbox.getRightY()); + Command driveFieldOrientedDirectAngle = drivebase.driveCommand(() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND) * invert, () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND) * invert, () -> driverXbox.getRightX() * invert, () -> driverXbox.getRightY() * invert); // Applies deadbands and inverts controls because joysticks // are back-right positive while robot // controls are front-left positive // left stick controls translation // right stick controls the angular velocity of the robot - Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand( - () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRawAxis(4)); + Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand(() -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND) * invert, () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND) * invert, () -> driverXbox.getRawAxis(4) * invert); + + Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand(() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND) * invert, () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND) * invert, () -> driverXbox.getRawAxis(2) * invert); - Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand( - () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverXbox.getRawAxis(2)); + Command sysIdForDrive = drivebase.sysIdDriveMotorCommand(); + Command sysIdForAngle = drivebase.sysIdAngleMotorCommand(); - drivebase.setDefaultCommand( - !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim); + drivebase.setDefaultCommand(!RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim); // Configure the trigger bindings configureBindings(); } + private int getInvert() { + var alliance = DriverStation.getAlliance(); + int invert; + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + invert = -1; + } else { + invert = 1; + } + + + AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, () -> MathUtil.applyDeadband(driverXbox.getLeftY() * invert, OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(driverXbox.getLeftX() * invert, OperatorConstants.LEFT_X_DEADBAND), () -> MathUtil.applyDeadband(driverXbox.getRightX() * invert, OperatorConstants.RIGHT_X_DEADBAND), driverXbox::getYButtonPressed, driverXbox::getAButtonPressed, driverXbox::getXButtonPressed, driverXbox::getBButtonPressed); + return invert; + } + /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary predicate, or via the @@ -122,15 +117,14 @@ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` new JoystickButton(driverXbox, 1).onTrue((new InstantCommand(drivebase::zeroGyro))); - new JoystickButton(driverXbox, XboxController.Button.kX.value).whileTrue(drivebase.driveCommand(() -> 0.0, () -> 0.0, () -> -LimelightHelpers.getTX("limelight-back") / 10)); - new JoystickButton(driverXbox, - 2).whileTrue(driveToShootingPositionCommand); + new JoystickButton(driverXbox, XboxController.Button.kX.value).whileTrue(aimAtLimelightCommand); + new JoystickButton(driverXbox, 2).whileTrue(driveToPoseCommand); new JoystickButton(driverXbox, XboxController.Button.kY.value).whileTrue(rotateCommand); -// new JoystickButton(driverXbox, 3).whileTrue(new RepeatCommand(new InstantCommand(drivebase::lock, drivebase))); - - new JoystickButton(driverXbox, XboxController.Button.kLeftBumper.value).whileTrue(intakeCommand); // independent of speed + new JoystickButton(shooterXbox, XboxController.Button.kLeftBumper.value).whileTrue(intakeCommand); // independent of speed + new JoystickButton(driverXbox, XboxController.Button.kLeftBumper.value).whileTrue(intakeCommand); + new JoystickButton(driverXbox, XboxController.Button.kY.value).whileTrue(aimAndPickUpNoteCommand); shooter.setDefaultCommand(shootCommand); } diff --git a/src/main/java/frc/robot/commands/AimAndPickUpNoteCommand.java b/src/main/java/frc/robot/commands/AimAndPickUpNoteCommand.java new file mode 100644 index 0000000..cc8167d --- /dev/null +++ b/src/main/java/frc/robot/commands/AimAndPickUpNoteCommand.java @@ -0,0 +1,63 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.VisionSubsystem; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.util.LimelightHelpers; + + +public class AimAndPickUpNoteCommand extends Command { + private final SwerveSubsystem swerveSubsystem; + private final VisionSubsystem visionSubsystem; + private final Intake intake; + + + public AimAndPickUpNoteCommand(SwerveSubsystem swerveSubsystem, VisionSubsystem visionSubsystem, Intake intake) { + this.swerveSubsystem = swerveSubsystem; + this.visionSubsystem = visionSubsystem; + this.intake = intake; + + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.swerveSubsystem, this.visionSubsystem, this.intake); + } + + + /** + * The initial subroutine of a command. Called once when the command is initially scheduled. + */ + @Override + public void initialize() { + + } + + @Override + public void execute() { + intake.set(0.5, 0.25); + swerveSubsystem.getSwerveDrive().drive(new Translation2d(-visionSubsystem.limelight_range_proportional() / 5, 0), -visionSubsystem.limelight_aim_proportional_front() / 5, false, false); + + + } + + @Override + public boolean isFinished() { + return !LimelightHelpers.getTV("limelight-front"); + } + + /** + * The action to take when the command ends. Called when either the command + * finishes normally -- that is, it is called when {@link #isFinished()} returns + * true -- or when it is interrupted/canceled. This is where you may want to + * wrap up loose ends, like shutting off a motor that was being used in the command. + * + * @param interrupted whether the command was interrupted/canceled + */ + @Override + public void end(boolean interrupted) { + intake.set(0,0); + swerveSubsystem.getSwerveDrive().drive(new Translation2d(0, 0), 0, false, false); + } +} diff --git a/src/main/java/frc/robot/commands/DriveToShootingPositionCommand.java b/src/main/java/frc/robot/commands/DriveToShootingPositionCommand.java deleted file mode 100644 index 1e7184b..0000000 --- a/src/main/java/frc/robot/commands/DriveToShootingPositionCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.commands; - -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.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.SwerveSubsystem; - - -public class DriveToShootingPositionCommand extends Command { - private final SwerveSubsystem drivebase; - - public DriveToShootingPositionCommand(SwerveSubsystem drivebase) { - this.drivebase = drivebase; - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.drivebase); - } - - @Override - public void initialize() { - CommandScheduler.getInstance().schedule(drivebase.driveToPose( - new Pose2d(new Translation2d(2.720, 2.579), Rotation2d.fromDegrees(-175.312)))); - } - - @Override - public void execute() { - - } - - @Override - public boolean isFinished() { - // TODO: Make this return true when this Command no longer needs to run execute() - return false; - } - - @Override - public void end(boolean interrupted) { - - } -} diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 1027502..54916ba 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -14,12 +14,14 @@ public class IntakeCommand extends Command { private final Intake intake; private final DoubleSupplier speed; + public IntakeCommand(Intake intake, DoubleSupplier speed) { this.intake = intake; this.speed = speed; // each subsystem used by the command must be passed into the // addRequirements() method (which takes a vararg of Subsystem) addRequirements(this.intake); + } @Override @@ -29,8 +31,8 @@ public void initialize() { @Override public void execute() { - intake.setVelocity(1 /*revolutions per minute*/ * 15 /* revolutions per turn */ * 60 /* seconds per minute */ * 2 /* turns per second*/, - 1.25 /*revolutions per minute*/ * 20 /* revolutions per turn */ * 60 /* seconds per minute */ * 2 /* turns per second*/); + + intake.set(0.5, 0.25); } diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java index 47dd650..990d112 100644 --- a/src/main/java/frc/robot/commands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/ShootCommand.java @@ -10,6 +10,7 @@ public class ShootCommand extends Command { private final Shooter shooter; private final DoubleSupplier speed; + public ShootCommand(Shooter shooter, DoubleSupplier speed) { this.shooter = shooter; this.speed = speed; diff --git a/src/main/java/frc/robot/commands/drivebase/AimAtLimelightCommand.java b/src/main/java/frc/robot/commands/drivebase/AimAtLimelightCommand.java new file mode 100644 index 0000000..ec52542 --- /dev/null +++ b/src/main/java/frc/robot/commands/drivebase/AimAtLimelightCommand.java @@ -0,0 +1,68 @@ +package frc.robot.commands.drivebase; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.VisionSubsystem; +import frc.robot.subsystems.intake.Intake; +import frc.robot.util.LimelightHelpers; + + +public class AimAtLimelightCommand extends Command { + private final SwerveSubsystem swerveSubsystem; + private final VisionSubsystem visionSubsystem; + + private boolean isRedAlliance; + double rotateCW, speed; + + public AimAtLimelightCommand(SwerveSubsystem swerveSubsystem, VisionSubsystem visionSubsystem) { + this.swerveSubsystem = swerveSubsystem; + this.visionSubsystem = visionSubsystem; + + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.swerveSubsystem, this.visionSubsystem); + } + + + /** + * The initial subroutine of a command. Called once when the command is initially scheduled. + */ + @Override + public void initialize() { + LimelightHelpers.setPipelineIndex("limelight-back", 1); + isRedAlliance = (DriverStation.getAlliance().get() == DriverStation.Alliance.Red); + + // Take the current angle and see if it's faster to rotate clockwise or counterclockwise to get to 180 degrees + double currentAngle = swerveSubsystem.getHeading().getDegrees(); // (-180, 180] + speed = 2.5; + rotateCW = (currentAngle > 0) ? -speed : speed; + + } + + @Override + public void execute() { + + if (LimelightHelpers.getTV("limelight-back")) { + swerveSubsystem.getSwerveDrive().drive(new Translation2d(0, 0), visionSubsystem.limelight_aim_proportional_back() / 5, false, false); + } else { + if (swerveSubsystem.getHeading().getDegrees() < 140 && swerveSubsystem.getHeading().getDegrees() > -140) + swerveSubsystem.getSwerveDrive().drive(new Translation2d(0, 0), rotateCW, false, false); + else swerveSubsystem.getSwerveDrive().drive(new Translation2d(0, 0), rotateCW / speed / 2, false, false); + } + + } + + @Override + public boolean isFinished() { + return (Math.abs(LimelightHelpers.getTX("limelight-back")) < 1.0) && LimelightHelpers.getTV("limelight-back"); + } + + @Override + public void end(boolean interrupted) { + LimelightHelpers.setPipelineIndex("limelight-back", 0); + swerveSubsystem.getSwerveDrive().drive(new Translation2d(0, 0), 0, false, false); + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 4eafc84..11f4bda 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -6,9 +6,11 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -36,9 +38,11 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; import java.io.File; +import java.util.Optional; import java.util.function.DoubleSupplier; public class SwerveSubsystem extends SubsystemBase { + private boolean lockRotation = false; private static final SwerveSubsystem INSTANCE = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); @@ -103,7 +107,7 @@ public void setupPathPlanner() { // Max module speed, in m/s swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() + new ReplanningConfig(true, true) // Default path replanning config. See the API for the options here ), () -> { @@ -115,6 +119,7 @@ public void setupPathPlanner() { }, this // Reference to this subsystem to set requirements ); + } /** @@ -133,7 +138,7 @@ public Command aimAtTarget(String camera) { }); } - /** + /** * Get the path follower with events. * * @param pathName PathPlanner path name. @@ -151,6 +156,8 @@ public Command getAutonomousCommand(String pathName) { * @return PathFinding command */ public Command driveToPose(Pose2d pose) { + lockRotation = true; + PathPlannerPath path = PathPlannerPath.fromPathFile("Go to red speaker"); // Create the constraints to use while pathfinding PathConstraints constraints = new PathConstraints( swerveDrive.getMaximumVelocity(), 4.0, @@ -248,11 +255,21 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat // Make the robot move swerveDrive.drive(new Translation2d(Math.pow(translationX.getAsDouble(), 3) * swerveDrive.getMaximumVelocity(), Math.pow(translationY.getAsDouble(), 3) * swerveDrive.getMaximumVelocity()), - Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), + angularRotationX.getAsDouble() * swerveDrive.getMaximumAngularVelocity(), true, false); }); } + public Command driveCommandNonRel(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { + return run(() -> { + // Make the robot move + swerveDrive.drive(new Translation2d(Math.pow(translationX.getAsDouble(), 3) * swerveDrive.getMaximumVelocity(), + Math.pow(translationY.getAsDouble(), 3) * swerveDrive.getMaximumVelocity()), + angularRotationX.getAsDouble() * swerveDrive.getMaximumAngularVelocity(), + false, + false); + }); + } /** * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and @@ -295,6 +312,10 @@ public void drive(ChassisSpeeds velocity) { @Override public void periodic() { + if (LimelightHelpers.getTV("limelight-back")) + swerveDrive.addVisionMeasurement(LimelightHelpers.getBotPose2d_wpiRed("limelight-back"), Timer.getFPGATimestamp() - (LimelightHelpers.getLatency_Pipeline("limelight-back") + LimelightHelpers.getLatency_Capture("limelight-back")) / 1000.0); + + PathPlannerLogging.logCurrentPose(getPose()); } @Override @@ -473,6 +494,7 @@ public void addFakeVisionReading() { swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); } + /** * Get the swerve drive object. * diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index c117de7..d73db77 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -1,9 +1,9 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.SwerveSubsystem; import frc.robot.util.LimelightHelpers; +import org.littletonrobotics.junction.Logger; + public class VisionSubsystem extends SubsystemBase { @@ -43,6 +43,65 @@ private VisionSubsystem() { @Override public void periodic() { // This method will be called once per scheduler run - //SwerveSubsystem.addVisionMeasurement(LimelightHelpers.getBotPose2d("back-limelight"), Timer.getFPGATimestamp() - LimelightHelpers.getLatency_Capture("back-limelight") - LimelightHelpers.getLatency_Pipeline("back-limelight") ); + + + Logger.recordOutput("Targets", LimelightHelpers.getTargetPose3d_RobotSpace("limelight-back")); + + } + + // simple proportional turning control with Limelight. + // "proportional control" is a control algorithm in which the output is proportional to the error. + // in this case, we are going to return an angular velocity that is proportional to the + // "tx" value from the Limelight. + public double limelight_aim_proportional_front() { + // kP (constant of proportionality) + // this is a hand-tuned number that determines the aggressiveness of our proportional control loop + // if it is too high, the robot will oscillate. + // if it is too low, the robot will never reach its target + // if the robot never turns in the correct direction, kP should be inverted. + double kP = .035; + + // tx ranges from (-hfov/2) to (hfov/2) in degrees. If your target is on the rightmost edge of + // your limelight 3 feed, tx should return roughly 31 degrees. + double targetingAngularVelocity = LimelightHelpers.getTX("limelight-front") * kP; + + // convert to radians per second for our drive method + targetingAngularVelocity *= SwerveSubsystem.getInstance().getSwerveDrive().getMaximumAngularVelocity(); + + //invert since tx is positive when the target is to the right of the crosshair + targetingAngularVelocity *= -1.0; + + return targetingAngularVelocity; + } + + public double limelight_aim_proportional_back() { + // kP (constant of proportionality) + // this is a hand-tuned number that determines the aggressiveness of our proportional control loop + // if it is too high, the robot will oscillate. + // if it is too low, the robot will never reach its target + // if the robot never turns in the correct direction, kP should be inverted. + double kP = .035; + + // tx ranges from (-hfov/2) to (hfov/2) in degrees. If your target is on the rightmost edge of + // your limelight 3 feed, tx should return roughly 31 degrees. + double targetingAngularVelocity = LimelightHelpers.getTX("limelight-back") * kP; + + // convert to radians per second for our drive method + targetingAngularVelocity *= SwerveSubsystem.getInstance().getSwerveDrive().getMaximumAngularVelocity(); + + //invert since tx is positive when the target is to the right of the crosshair + targetingAngularVelocity *= 1.0; + + return targetingAngularVelocity; + } + // simple proportional ranging control with Limelight's "ty" value + // this works best if your Limelight's mount height and target mount height are different. + // if your limelight and target are mounted at the same or similar heights, use "ta" (area) for target ranging rather than "ty" + public double limelight_range_proportional() { + double kP = .1; + double targetingForwardSpeed = LimelightHelpers.getTY("limelight-front") * kP; + targetingForwardSpeed *= SwerveSubsystem.getInstance().getSwerveDrive().getMaximumVelocity(); + targetingForwardSpeed *= -1.0; + return targetingForwardSpeed; } } diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 0000000..998c261 --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2024.1.0", + "frcYear": "2024", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2024.1.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +} \ No newline at end of file