From 40e53305e056cc44431affda04c57dcd08754b35 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Mon, 4 Mar 2024 19:40:54 -0500 Subject: [PATCH] Adding necessary code --- networktables.json | 1 + simgui-ds.json | 97 +++++++++++++++++++ simgui.json | 17 ++++ src/main/java/frc/robot/RobotContainer.java | 47 ++++++--- .../java/frc/robot/subsystems/Drivetrain.java | 2 +- .../frc/robot/utilities/PoseSendable.java | 62 ++++++++++++ 6 files changed, 213 insertions(+), 13 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json create mode 100644 src/main/java/frc/robot/utilities/PoseSendable.java diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..69b1a3c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..72d3ee0 --- /dev/null +++ b/simgui.json @@ -0,0 +1,17 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables": { + "transitory": { + "Shuffleboard": { + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 83193b5..aae2605 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,34 +5,42 @@ package frc.robot; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.constants.OIConstants; import frc.robot.subsystems.Drivetrain; +import frc.robot.utilities.PoseSendable; /* - * The objective for this part of testing is to just make sure swerve is working in a perfect world scenario. - * This is designed to be as close the REV template as possible, the only difference being the naming of certain - * Constants and the positioning of instance variables and there declarations. + * The objective for this part of testing is to see if we can get pose estimation values to work for the gyro angle + * required for Field Oriented AND can we find a way to reset the robot's pose to properly offset using the pose estimator. * * We should try to solve in this branch: - * - Does Field Oriented Drive work in a perfect world scenario? - * - Is our weird module spin behavior still present? - * - If it is, can we do something about it? + * - Does Field Oriented Drive work in a perfect world scenario when using the pose estimator as the rotation source? + * - Can we reset our pose in such a way using the pose estimator that we get consistent behavior of field oriented drive + * regardless of what orientation the robot is booted up in? * - * Any changes we make should be able to be carried forward to part 2 + * Any changes we make should be able to be carried forward to part 3 */ public class RobotContainer { private Drivetrain drivetrain; private CommandXboxController driver; + private PoseSendable poseSendable; + public RobotContainer() { drivetrain = new Drivetrain(new Pose2d()); driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB); + poseSendable = new PoseSendable(); + configureBindings(); + shuffleboardConfig(); } private void configureBindings() { @@ -46,10 +54,25 @@ public class RobotContainer { ); driver.a().onTrue(drivetrain.zeroHeadingCommand()); - - } - public Command getAutonomousCommand() { - return null; - } + driver.b().onTrue( + new InstantCommand(() -> { + System.out.println("X: " + poseSendable.getX()); + System.out.println("Y: " + poseSendable.getY()); + System.out.println("Rotation: " + poseSendable.getRotation()); + drivetrain.resetOdometry(poseSendable.getPose()); + } + )); + } + + private void shuffleboardConfig() { + ShuffleboardTab tab = Shuffleboard.getTab("Testing"); + tab.add("Drivetrain Pose", poseSendable) + .withPosition(0, 0) + .withSize(2, 3); + } + + public Command getAutonomousCommand() { + return null; + } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index c553719..febf440 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -208,7 +208,7 @@ public class Drivetrain extends SubsystemBase { var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle())) + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_poseEstimator.getEstimatedPosition().getRotation()) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); diff --git a/src/main/java/frc/robot/utilities/PoseSendable.java b/src/main/java/frc/robot/utilities/PoseSendable.java new file mode 100644 index 0000000..5792d86 --- /dev/null +++ b/src/main/java/frc/robot/utilities/PoseSendable.java @@ -0,0 +1,62 @@ +package frc.robot.utilities; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public class PoseSendable implements Sendable { + + private double x; + private double y; + private double rot; + + public PoseSendable() { + this(0, 0, 0); + } + + public PoseSendable(double x, double y, double rot) { + this.x = x; + this.y = y; + this.rot = rot; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getRotation() { + return rot; + } + + public Rotation2d getRotation2d() { + return Rotation2d.fromDegrees(rot); + } + + public Pose2d getPose() { + return new Pose2d(x, y, getRotation2d()); + } + + public void setX(double x) { + this.x = x; + } + + public void setY(double y) { + this.y = y; + } + + public void setRotation(double rot) { + this.rot = rot; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addDoubleProperty("X", this::getX, this::setX); + builder.addDoubleProperty("Y", this::getY, this::setY); + builder.addDoubleProperty("Rotation", this::getRotation, this::setRotation); + } +}