diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 18c62d2..9f81897 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,6 +12,7 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -27,6 +28,7 @@ import edu.wpi.first.math.numbers.N3; public final class Constants { + public static final double stickDeadband = 0.1; public static final class IntakeArm{ //TODO: This must be tuned to specific robot @@ -56,6 +58,9 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo } public static final class Swerve { + public static final double minimumErrorAligning = 0; // TODO: This must be tuned to specific robot + public static final PIDController aligningPID = new PIDController(0, 0, 0); + public static final int pigeonID = 10; public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- @@ -200,27 +205,27 @@ public static final class AutoConstants { //TODO: The below constants are used i } public static class Vision { - public static final String kRightCameraName = "Arducam_OV9281_USB_Camera"; - public static final String kLeftCameraName = "YOUR CAMERA NAME"; - // Cam mounted facing forward, half a meter forward of center, half a meter up - // from center. - public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); - public static final Transform3d kLeftRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); - - // The layout of the AprilTags on the field - public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); - - // The standard deviations of our vision estimated poses, which affect - // correction rate - // (Fake values. Experiment and determine estimation noise on an actual robot.) - public static final Matrix kRightSingleTagStdDevs = VecBuilder.fill(4, 4, 8); - public static final Matrix kRightMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); - - public static final Matrix kLeftSingleTagStdDevs = VecBuilder.fill(4, 4, 8); - public static final Matrix kLeftMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); - - public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0))); - } + public static final String kRightCameraName = "Arducam_OV9281_USB_Camera"; + public static final String kLeftCameraName = "YOUR CAMERA NAME"; + // Cam mounted facing forward, half a meter forward of center, half a meter up + // from center. + public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + new Rotation3d(0, 0, 0)); + public static final Transform3d kLeftRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + new Rotation3d(0, 0, 0)); + + // The layout of the AprilTags on the field + public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + + // The standard deviations of our vision estimated poses, which affect + // correction rate + // (Fake values. Experiment and determine estimation noise on an actual robot.) + public static final Matrix kRightSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kRightMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + + public static final Matrix kLeftSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kLeftMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + + public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0))); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 92b05e9..f7fe27b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.Utils.PathPlanner.PathPlannerHelper; -import frc.robot.commands.*; +import frc.robot.commands.SwerveCommands.*; import frc.robot.subsystems.*; /** diff --git a/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java new file mode 100644 index 0000000..954d0e3 --- /dev/null +++ b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.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 frc.robot.commands.SwerveCommands; + +import static frc.robot.Constants.Swerve.*; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.Utils.vision.Vision; +import frc.robot.subsystems.SwerveSubsystem; + +public class AlignToSpeaker extends Command { + /** Creates a new TurnToDegree. */ + SwerveSubsystem swerve = SwerveSubsystem.getInstance(); + private PIDController pid = aligningPID; + private Vision vision = Vision.getInstance(); + double angleFromTarget = vision.GetAngleFromTarget().getDegrees(); + + public AlignToSpeaker() { + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + angleFromTarget = vision.GetAngleFromTarget().getDegrees(); + calculateOptimalRotation(); + double rotation = pid.calculate(angleFromTarget, 180); + swerve.drive(new Translation2d(), rotation, true, false, false); + } + + // 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 Math.abs(vision.GetAngleFromTarget().getDegrees() - 180) < minimumErrorAligning ; + } + + public void calculateOptimalRotation() { + double currentRotation = angleFromTarget; + double targetRotation = 180; + + // Calculate the difference between the target and current rotation + double rotationDifference = (targetRotation - currentRotation) % 360; + + // Choose the optimal direction + double optimalRotation = (rotationDifference <= 360 / 2) + ? rotationDifference + : rotationDifference - 360; + + angleFromTarget = optimalRotation; + } +} diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/SwerveCommands/TeleopSwerve.java similarity index 98% rename from src/main/java/frc/robot/commands/TeleopSwerve.java rename to src/main/java/frc/robot/commands/SwerveCommands/TeleopSwerve.java index dcde3b2..75daa60 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/TeleopSwerve.java @@ -1,4 +1,4 @@ -package frc.robot.commands; +package frc.robot.commands.SwerveCommands; import frc.robot.Constants; import frc.robot.subsystems.SwerveSubsystem;