From e468ac16b08f589efbf4303c10a661c8d7a46db9 Mon Sep 17 00:00:00 2001 From: Eilon Date: Fri, 19 Jan 2024 08:32:07 +0200 Subject: [PATCH 1/5] branch --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/{ => SwerveCommands}/TeleopSwerve.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename src/main/java/frc/robot/commands/{ => SwerveCommands}/TeleopSwerve.java (98%) 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/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; From 0875847b6f1dc8ac07f3965cddd632c43b9200c8 Mon Sep 17 00:00:00 2001 From: Eilon Date: Fri, 19 Jan 2024 09:12:29 +0200 Subject: [PATCH 2/5] start of turn to a degree command --- src/main/java/frc/robot/Constants.java | 2 ++ .../commands/SwerveCommands/TurnToDegree.java | 35 +++++++++++++++++++ 2 files changed, 37 insertions(+) create mode 100644 src/main/java/frc/robot/commands/SwerveCommands/TurnToDegree.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 18c62d2..8089ea9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,6 +56,8 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo } public static final class Swerve { + public static final double minimumErrorTurn = 0; // TODO: This must be tuned to specific robot + public static final int pigeonID = 10; public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- diff --git a/src/main/java/frc/robot/commands/SwerveCommands/TurnToDegree.java b/src/main/java/frc/robot/commands/SwerveCommands/TurnToDegree.java new file mode 100644 index 0000000..620a926 --- /dev/null +++ b/src/main/java/frc/robot/commands/SwerveCommands/TurnToDegree.java @@ -0,0 +1,35 @@ +// 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 edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.SwerveCommands.*; + +public class TurnToDegree extends Command { + /** Creates a new TurnToDegree. */ + private double degree; + public TurnToDegree(double degree) { + // Use addRequirements() here to declare subsystem dependencies. + this.degree = degree; + } + + // 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() {} + + // 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; + } +} From d3aa85ed2eb44b763ea249bdbe51cd8e150b40c0 Mon Sep 17 00:00:00 2001 From: Eilon Date: Fri, 19 Jan 2024 13:27:40 +0200 Subject: [PATCH 3/5] align to speaker command --- src/main/java/frc/robot/Constants.java | 47 +++++++-------- .../SwerveCommands/AlignToSpeaker.java | 59 +++++++++++++++++++ .../commands/SwerveCommands/TurnToDegree.java | 35 ----------- 3 files changed, 83 insertions(+), 58 deletions(-) create mode 100644 src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java delete mode 100644 src/main/java/frc/robot/commands/SwerveCommands/TurnToDegree.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8089ea9..8fa184a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,6 +27,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 @@ -202,27 +203,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/commands/SwerveCommands/AlignToSpeaker.java b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java new file mode 100644 index 0000000..ad2fc43 --- /dev/null +++ b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.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 frc.robot.commands.SwerveCommands; + +import static frc.robot.Constants.IntakeArm.minimumError; +import static frc.robot.Constants.Swerve.minimumErrorTurn; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.Command; +import frc.Utils.vision.Vision; +import frc.robot.commands.SwerveCommands.*; +import frc.robot.subsystems.SwerveSubsystem; + +public class AlignToSpeaker extends Command { + /** Creates a new TurnToDegree. */ + SwerveSubsystem swerve = SwerveSubsystem.getInstance(); + private PIDController pid = new PIDController(0, 0, 0); + 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(); + optimize(); + 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) < minimumErrorTurn ; + } + + public void optimize() { + double delta = 180 - angleFromTarget; + if (Math.abs(delta) > 90){ + angleFromTarget = delta > 90 ? (angleFromTarget -= 180) : (angleFromTarget += 180); + } + } +} diff --git a/src/main/java/frc/robot/commands/SwerveCommands/TurnToDegree.java b/src/main/java/frc/robot/commands/SwerveCommands/TurnToDegree.java deleted file mode 100644 index 620a926..0000000 --- a/src/main/java/frc/robot/commands/SwerveCommands/TurnToDegree.java +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.SwerveCommands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.commands.SwerveCommands.*; - -public class TurnToDegree extends Command { - /** Creates a new TurnToDegree. */ - private double degree; - public TurnToDegree(double degree) { - // Use addRequirements() here to declare subsystem dependencies. - this.degree = degree; - } - - // 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() {} - - // 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; - } -} From 2b042adc83654318a5e01f332106aa58a0035608 Mon Sep 17 00:00:00 2001 From: Eilon Date: Fri, 19 Jan 2024 14:09:55 +0200 Subject: [PATCH 4/5] change a name and add the pid controller to constans --- src/main/java/frc/robot/Constants.java | 6 ++++-- .../frc/robot/commands/SwerveCommands/AlignToSpeaker.java | 6 +++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8fa184a..d048e41 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,7 +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 @@ -57,7 +58,8 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo } public static final class Swerve { - public static final double minimumErrorTurn = 0; // TODO: This must be tuned to specific robot + public static final double minimumErrorAligning = 0; // TODO: This must be tuned to specific robot + public static final PIDController swervePID = new PIDController(0, 0, 0); public static final int pigeonID = 10; public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- diff --git a/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java index ad2fc43..8260711 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java @@ -5,7 +5,7 @@ package frc.robot.commands.SwerveCommands; import static frc.robot.Constants.IntakeArm.minimumError; -import static frc.robot.Constants.Swerve.minimumErrorTurn; +import static frc.robot.Constants.Swerve.*; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -19,7 +19,7 @@ public class AlignToSpeaker extends Command { /** Creates a new TurnToDegree. */ SwerveSubsystem swerve = SwerveSubsystem.getInstance(); - private PIDController pid = new PIDController(0, 0, 0); + private PIDController pid = swervePID; private Vision vision = Vision.getInstance(); double angleFromTarget = vision.GetAngleFromTarget().getDegrees(); @@ -47,7 +47,7 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return Math.abs(vision.GetAngleFromTarget().getDegrees() - 180) < minimumErrorTurn ; + return Math.abs(vision.GetAngleFromTarget().getDegrees() - 180) < minimumErrorAligning ; } public void optimize() { From f440afdd28d1b932f2f7af3be9505cc1ee63e7f0 Mon Sep 17 00:00:00 2001 From: kully Date: Fri, 19 Jan 2024 22:05:08 +0200 Subject: [PATCH 5/5] fixed optimization of rotation --- src/main/java/frc/robot/Constants.java | 2 +- .../SwerveCommands/AlignToSpeaker.java | 26 +++++++++++-------- 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d048e41..9f81897 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -59,7 +59,7 @@ 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 swervePID = new PIDController(0, 0, 0); + 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- diff --git a/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java index 8260711..954d0e3 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java @@ -4,22 +4,18 @@ package frc.robot.commands.SwerveCommands; -import static frc.robot.Constants.IntakeArm.minimumError; import static frc.robot.Constants.Swerve.*; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj2.command.Command; import frc.Utils.vision.Vision; -import frc.robot.commands.SwerveCommands.*; import frc.robot.subsystems.SwerveSubsystem; public class AlignToSpeaker extends Command { /** Creates a new TurnToDegree. */ SwerveSubsystem swerve = SwerveSubsystem.getInstance(); - private PIDController pid = swervePID; + private PIDController pid = aligningPID; private Vision vision = Vision.getInstance(); double angleFromTarget = vision.GetAngleFromTarget().getDegrees(); @@ -35,7 +31,7 @@ public void initialize() {} @Override public void execute() { angleFromTarget = vision.GetAngleFromTarget().getDegrees(); - optimize(); + calculateOptimalRotation(); double rotation = pid.calculate(angleFromTarget, 180); swerve.drive(new Translation2d(), rotation, true, false, false); } @@ -50,10 +46,18 @@ public boolean isFinished() { return Math.abs(vision.GetAngleFromTarget().getDegrees() - 180) < minimumErrorAligning ; } - public void optimize() { - double delta = 180 - angleFromTarget; - if (Math.abs(delta) > 90){ - angleFromTarget = delta > 90 ? (angleFromTarget -= 180) : (angleFromTarget += 180); - } + 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; } }