From a300529ba350a42d16a21e0889185e536f266e55 Mon Sep 17 00:00:00 2001 From: Cherriae Date: Fri, 26 Jan 2024 13:39:36 -0500 Subject: [PATCH 1/9] comment --- src/main/java/frc/robot/commands/shooter/AutoAim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index 2ac87e4..7d27993 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java @@ -33,7 +33,7 @@ public void execute() { double shooterAngle = 0; if (_vision.isApriltagVisible()) { - shooterAngle = _vision.shooterAnglesToSpeaker()[1]; // ty + shooterAngle = _vision.shooterAnglesToSpeaker()[1]; // ty yappers frfr } else { shooterAngle = _swerve.shooterAngleToSpeaker(); } From 10054a18d78f53bf0ddc8014de68ad8b1845bc33 Mon Sep 17 00:00:00 2001 From: cherriae Date: Fri, 26 Jan 2024 13:55:30 -0500 Subject: [PATCH 2/9] autoaiming shooter --- src/main/java/frc/robot/RobotContainer.java | 49 ++++++++++------- .../frc/robot/commands/shooter/AutoAim.java | 55 ++++++++++++++++--- .../robot/commands/swerve/TeleopDrive.java | 21 ++----- .../subsystems/SwerveDriveSubsystem.java | 27 ++++++--- .../frc/robot/subsystems/VisionSubsystem.java | 13 ++++- .../java/frc/robot/utils/LimelightHelper.java | 2 + 6 files changed, 118 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1b88059..f8fce23 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import frc.robot.commands.elevator.HoldElevator; +import frc.robot.commands.shooter.AutoAim; import frc.robot.commands.shooter.HoldShooter; import frc.robot.commands.shooter.SpinShooter; import frc.robot.commands.swerve.BrakeSwerve; @@ -25,6 +26,7 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.subsystems.VisionSubsystem; +import frc.robot.utils.UtilFuncs; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -35,7 +37,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final VisionSubsystem _visionSubsystem = new VisionSubsystem(); - private final SwerveDriveSubsystem _swerveDrive = new SwerveDriveSubsystem(_visionSubsystem); + private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem); private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem(); private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem(); @@ -54,21 +56,30 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - Command interruptSwerve = new BrakeSwerve(_swerveDrive, 3); + Command interruptSwerve = new BrakeSwerve(_swerveSubsystem, 3); NamedCommands.registerCommand("printHello", new PrintCommand("AUTON HELLO")); NamedCommands.registerCommand("waitCommand", new WaitCommand(3)); NamedCommands.registerCommand("interruptSwerve", interruptSwerve); - _swerveDrive.setDefaultCommand( - new TeleopDrive( - _swerveDrive, - () -> -_driveFilterLeftY.calculate(_driveController.getLeftY()), - () -> -_driveFilterLeftX.calculate(_driveController.getLeftX()), - () -> -_driveFilterRightX.calculate(_driveController.getRightX()))); - - _elevatorSubsystem.setDefaultCommand(new HoldElevator(_elevatorSubsystem)); - _shooterSubsystem.setDefaultCommand(new HoldShooter(_shooterSubsystem)); + _swerveSubsystem.setDefaultCommand( + // new TeleopDrive( + // _swerveDrive, + // () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.1), + // () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.1), + // () -> UtilFuncs.ApplyDeadband(-_driveFilterRightX.calculate(_driveController.getRightX()), 0.1) + // ) + new AutoAim( + _shooterSubsystem, + _visionSubsystem, + _swerveSubsystem, + () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.1), + () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.1) + ) + ); + + // _elevatorSubsystem.setDefaultCommand(new HoldElevator(_elevatorSubsystem)); + // _shooterSubsystem.setDefaultCommand(new HoldShooter(_shooterSubsystem)); // configure trigger bindings configureBindings(); @@ -80,10 +91,10 @@ public RobotContainer() { // to configure button bindings private void configureBindings() { - _driveController.R1().onTrue(new ToggleSwerveOrient(_swerveDrive)); - _driveController.square().onTrue(new ResetPose(_swerveDrive)); + _driveController.R1().onTrue(new ToggleSwerveOrient(_swerveSubsystem)); + _driveController.square().onTrue(new ResetPose(_swerveSubsystem)); _driveController.circle().whileTrue(new SpinShooter(_shooterSubsystem)); - _driveController.cross().whileTrue(new BrakeSwerve(_swerveDrive)); + _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem)); // for testing raw percent output, is it straight? _driveController @@ -91,9 +102,9 @@ private void configureBindings() { .onTrue( Commands.runOnce( () -> { - _swerveDrive.driveTest(0.1); + _swerveSubsystem.driveTest(0.1); }, - _swerveDrive)); + _swerveSubsystem)); // for testing velocity output (forward at 0.3 m/s), is it straight? _driveController @@ -101,16 +112,16 @@ private void configureBindings() { .whileTrue( Commands.run( () -> { - _swerveDrive.driveChassis(new ChassisSpeeds(0.3, 0, 0)); + _swerveSubsystem.driveChassis(new ChassisSpeeds(0.3, 0, 0)); }, - _swerveDrive)); + _swerveSubsystem)); } /** * @return The Command to schedule for auton. */ public Command getAutonCommand() { - _swerveDrive.fieldOriented = + _swerveSubsystem.fieldOriented = false; // make sure swerve is robot-relative for pathplanner to work return _autonChooser.getSelected(); diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index 2ac87e4..d75b24f 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java @@ -3,8 +3,14 @@ package frc.robot.commands.shooter; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.subsystems.VisionSubsystem; @@ -14,13 +20,34 @@ public class AutoAim extends Command { private VisionSubsystem _vision; private SwerveDriveSubsystem _swerve; + private DoubleSupplier _xSpeed; + private DoubleSupplier _ySpeed; + + private PIDController _headingController = new PIDController( + Constants.PID.PP_ROTATION.kP, + 0, + 0 + ); + /** Creates a new AutoAim. */ - public AutoAim(ShooterSubsystem shooter, VisionSubsystem vision, SwerveDriveSubsystem swerve) { + public AutoAim( + ShooterSubsystem shooter, + VisionSubsystem vision, + SwerveDriveSubsystem swerve, + DoubleSupplier xSpeed, + DoubleSupplier ySpeed + ) { // Use addRequirements() here to declare subsystem dependencies. _shooter = shooter; _vision = vision; _swerve = swerve; - addRequirements(_shooter, _vision); // not adding swerve as a requirement to prevent interrupting default command + + _xSpeed = xSpeed; + _ySpeed = ySpeed; + + _headingController.setTolerance(2); + + addRequirements(_shooter, _vision, _swerve); } // Called when the command is initially scheduled. @@ -30,17 +57,31 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double shooterAngle = 0; + double desiredShooterAngle = 0; + double desiredSwerveHeading = 0; + + double currentSwerveHeading = _swerve.getHeading().getDegrees(); if (_vision.isApriltagVisible()) { - shooterAngle = _vision.shooterAnglesToSpeaker()[1]; // ty + desiredShooterAngle = _vision.anglesToSpeaker()[1]; // ty + desiredSwerveHeading = _vision.anglesToSpeaker()[0]; + } else { - shooterAngle = _swerve.shooterAngleToSpeaker(); + desiredShooterAngle = _swerve.anglesToSpeaker()[1]; + desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; } - SmartDashboard.putNumber("ANGLE", shooterAngle); + desiredSwerveHeading += currentSwerveHeading; + + _shooter.setAngle(desiredShooterAngle); - _shooter.setAngle(shooterAngle); + _swerve.driveChassis( + new ChassisSpeeds( + _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + _headingController.calculate(currentSwerveHeading, desiredSwerveHeading) + ) + ); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/swerve/TeleopDrive.java b/src/main/java/frc/robot/commands/swerve/TeleopDrive.java index 749ef6a..b9dfdcd 100644 --- a/src/main/java/frc/robot/commands/swerve/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/swerve/TeleopDrive.java @@ -50,23 +50,14 @@ public void initialize() {} @Override public void execute() { // apply controller deadband - double xSpeed = UtilFuncs.ApplyDeadband(_xSpeed.getAsDouble(), 0.1); - double ySpeed = UtilFuncs.ApplyDeadband(_ySpeed.getAsDouble(), 0.1); - double rotationSpeed = UtilFuncs.ApplyDeadband(_rotationSpeed.getAsDouble(), 0.1); - - SmartDashboard.putNumber( - "DESIRED X SPEED", - xSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF); - SmartDashboard.putNumber( - "DESIRED Y SPEED", - ySpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF); - // drive the swerve chassis subsystem _swerveDrive.driveChassis( - new ChassisSpeeds( - xSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - ySpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - rotationSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED)); + new ChassisSpeeds( + _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + _rotationSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED + ) + ); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 1c3dcd5..3f820b5 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -250,10 +250,10 @@ public void periodic() { if (_visionSubsystem.isApriltagVisible()) { Optional visionBotpose = _visionSubsystem.getBotpose(); - if (visionBotpose.isPresent()) - _estimator.addVisionMeasurement( - _visionSubsystem.getBotpose().get(), Timer.getFPGATimestamp()); - if (visionBotpose.isPresent()) _field.setRobotPose(visionBotpose.get()); + if (visionBotpose.isPresent()) { + _estimator.addVisionMeasurement(_visionSubsystem.getBotpose().get(), _visionSubsystem.getLatency()); + _field.setRobotPose(_estimator.getEstimatedPosition()); + } } SmartDashboard.putData("FIELD", _field); @@ -288,6 +288,10 @@ private void setupOrchestra() { _orchestra.play(); } + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + + } + /** * Set the chassis speed of the swerve drive. * @@ -382,7 +386,7 @@ public Rotation2d getHeadingRaw() { } /** Get the shooter's angle to the speaker hole using the drive's pose estimator. */ - public double shooterAngleToSpeaker() { + public double[] anglesToSpeaker() { int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); @@ -393,7 +397,16 @@ public double shooterAngleToSpeaker() { double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED; - double angle = Math.atan(zDifference / distanceToRobot); - return Math.toDegrees(angle); + double angleY = Math.atan(zDifference / distanceToRobot); + + + Rotation2d currentRotation = getHeading(); + double rotateDifference = Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); + + double angleX = currentRotation.getDegrees() - rotateDifference; + + double[] angles = {angleX, angleY}; + + return angles; } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index f7735c5..782776c 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.utils.LimelightHelper; @@ -38,6 +39,16 @@ public void periodic() { // SmartDashboard.putData("Limelight Field", _field); } + /** + * Returns the latency from the last time data was sent from the limelight. This should be used in the pose estimator. + */ + public double getLatency() { + double tl = _limelight.getEntry("tl").getDouble(0); + double cl = _limelight.getEntry("cl").getDouble(0); + + return Timer.getFPGATimestamp() - (tl/1000.0) - (cl/1000.0); + } + /** * Returns the "wpiblue" botpose of the robot from the limelight. * @@ -117,7 +128,7 @@ public double[] tagAngleOffsets(int ID) { * * @return [tx, ty] or null. */ - public double[] shooterAnglesToSpeaker() { + public double[] anglesToSpeaker() { return tagAngleOffsets(Constants.FIELD_CONSTANTS.SPEAKER_TAG); } } diff --git a/src/main/java/frc/robot/utils/LimelightHelper.java b/src/main/java/frc/robot/utils/LimelightHelper.java index 4ff0fb8..1af35ca 100644 --- a/src/main/java/frc/robot/utils/LimelightHelper.java +++ b/src/main/java/frc/robot/utils/LimelightHelper.java @@ -76,4 +76,6 @@ public JsonNode getTag(int ID) { return null; } + + } From c84ce474aa382a7fc3d17a7a718c14f221f11490 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 26 Jan 2024 13:56:21 -0500 Subject: [PATCH 3/9] pp constants --- .../deploy/pathplanner/autos/New Auto.auto | 12 +-- .../pathplanner/autos/New New Auto.auto | 19 +++++ .../paths/New New New New Path.path | 84 +++++++++++++++++++ src/main/java/frc/robot/Constants.java | 2 +- .../subsystems/SwerveDriveSubsystem.java | 20 +++-- 5 files changed, 118 insertions(+), 19 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New New Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/New New New New Path.path diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto index 2dd6c26..d57a88c 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -1,12 +1,6 @@ { "version": 1.0, - "startingPose": { - "position": { - "x": 1.9099007567375976, - "y": 6.998240302783657 - }, - "rotation": 0 - }, + "startingPose": null, "command": { "type": "sequential", "data": { @@ -14,7 +8,7 @@ { "type": "path", "data": { - "pathName": "Test Path" + "pathName": "New New New New Path" } } ] @@ -22,4 +16,4 @@ }, "folder": null, "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto new file mode 100644 index 0000000..d57a88c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New New Auto.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New New New New Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New Path.path new file mode 100644 index 0000000..f7663d9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New New New Path.path @@ -0,0 +1,84 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.294070804357825 + }, + "prevControl": null, + "nextControl": { + "x": 2.0023477884793603, + "y": 7.294070804357825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 1.7564936030772962 + }, + "prevControl": { + "x": 2.002347788479361, + "y": 1.7657383062513876 + }, + "nextControl": { + "x": 1.9976522115206392, + "y": 1.747248899903205 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 1.7564936030772962 + }, + "prevControl": { + "x": 2.002274563107685, + "y": 1.7380041967291142 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 0.0, + "rotateFast": true + }, + { + "waypointRelativePos": 0.4, + "rotationDegrees": -90.0, + "rotateFast": true + }, + { + "waypointRelativePos": 0.65, + "rotationDegrees": 180.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.67, + "maxAcceleration": 3.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 91.31609348391021, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 90.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7729eed..5a47b4c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -106,7 +106,7 @@ public static class PID { public static final double BACK_LEFT_ROTATE_KP = 0.009; public static final PIDConstants PP_TRANSLATION = new PIDConstants(3.69, 0, 0); - public static final PIDConstants PP_ROTATION = new PIDConstants(1.21893, 0, 0); + public static final PIDConstants PP_ROTATION = new PIDConstants(1.21993, 0, 0); public static final double SHOOTER_FLYWHEEL_KP = 0; diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 1c3dcd5..b43d8b9 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -177,6 +177,15 @@ public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) { return false; }, this); + SmartDashboard.putData( + "Gyro", + new Sendable() { + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Gyro"); + builder.addDoubleProperty("Value", () -> getHeading().getDegrees(), null); + } + }); SmartDashboard.putData( "Swerve Drive", new Sendable() { @@ -205,17 +214,10 @@ public void initSendable(SendableBuilder builder) { "Swerve Speed", () -> Constants.Speeds.SWERVE_DRIVE_COEFF, null); } }); - SmartDashboard.putData( - "Gyro", - new Sendable() { - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Gyro"); - builder.addDoubleProperty("Pose", () -> getHeading().getDegrees(), null); - } - }); SmartDashboard.putData("Swerve/Built-in Accelerometer", new BuiltInAccelerometer()); + + } @Override From ce4ebaf37cb1619dd6ae3e1cf783fcdfddab8d1e Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 26 Jan 2024 13:56:36 -0500 Subject: [PATCH 4/9] Elastic layout --- elastic-layout.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elastic-layout.json b/elastic-layout.json index 69cfc4d..d5568e5 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -1 +1 @@ -{"tabs":[{"name":"Teleoperated","grid_layout":{"layouts":[],"containers":[{"title":"Front Right Velocity","x":640.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Right Velocity","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Left Velocity","x":512.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Left Velocity","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Field Oriented","x":512.0,"y":384.0,"width":256.0,"height":128.0,"type":"Toggle Switch","properties":{"topic":"/SmartDashboard/Field Oriented","period":0.06,"data_type":"boolean"}},{"title":"Back Right Velocity","x":640.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Right Velocity","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Back Left Velocity","x":512.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Left Velocity","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"default","x":1792.0,"y":640.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/AUTON CHOOSER/default","period":0.06,"data_type":"string"}},{"title":"FIELD","x":768.0,"y":0.0,"width":1152.0,"height":640.0,"type":"Field","properties":{"topic":"/SmartDashboard/FIELD","period":0.06,"field_game":"Crescendo","robot_width":0.85,"robot_length":0.85,"show_other_objects":true,"show_trajectories":true}},{"title":"AUTON CHOOSER","x":1536.0,"y":640.0,"width":256.0,"height":128.0,"type":"ComboBox Chooser","properties":{"topic":"/SmartDashboard/AUTON CHOOSER","period":0.06}},{"title":"FMSInfo","x":1536.0,"y":768.0,"width":384.0,"height":128.0,"type":"FMSInfo","properties":{"topic":"/FMSInfo","period":0.06}},{"title":"IsRedAlliance","x":384.0,"y":256.0,"width":128.0,"height":128.0,"type":"Boolean Box","properties":{"topic":"/FMSInfo/IsRedAlliance","period":0.06,"data_type":"boolean","true_color":4283215696,"false_color":4294198070}},{"title":"Back Left Angle","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Left Angle","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Back Right Angle","x":128.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Right Angle","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Left Angle","x":0.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Left Angle","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Right Angle","x":128.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Right Angle","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"limelight_Interface","x":512.0,"y":768.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/limelight_Interface","period":0.06,"data_type":"string"}},{"title":"limelight_Stream","x":512.0,"y":640.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/limelight_Stream","period":0.06,"data_type":"string"}},{"title":"DESIRED X SPEED","x":512.0,"y":256.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/DESIRED X SPEED","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"DESIRED Y SPEED","x":640.0,"y":256.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/DESIRED Y SPEED","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"limelight","x":0.0,"y":384.0,"width":512.0,"height":512.0,"type":"Camera Stream","properties":{"topic":"/CameraPublisher/limelight","period":0.06}},{"title":"Swerve Drive","x":896.0,"y":640.0,"width":640.0,"height":256.0,"type":"SwerveDrive","properties":{"topic":"/SmartDashboard/Swerve Drive","period":0.06,"show_robot_rotation":true}},{"title":"Swerve Speed","x":0.0,"y":256.0,"width":256.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Swerve Drive/Swerve Speed","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Gyro","x":256.0,"y":0.0,"width":256.0,"height":256.0,"type":"Gyro","properties":{"topic":"/SmartDashboard/Gyro","period":0.06,"counter_clockwise_positive":false}},{"title":"Match Time","x":512.0,"y":512.0,"width":256.0,"height":128.0,"type":"Match Time","properties":{"topic":"/SmartDashboard/Match Time","period":0.06,"data_type":"double","time_display_mode":"Minutes and Seconds","red_start_time":15,"yellow_start_time":30}},{"title":"robot Speed","x":256.0,"y":256.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/robot Speed","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}}]}},{"name":"Autonomous","grid_layout":{"layouts":[{"title":"PathPlanner","x":0.0,"y":0.0,"width":256.0,"height":256.0,"type":"List Layout","properties":{"label_position":"TOP"},"children":[{"title":"activePath","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/activePath","period":0.06,"data_type":"double[]"}},{"title":"currentPose","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/currentPose","period":0.06,"data_type":"double[]"}},{"title":"inaccuracy","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/inaccuracy","period":0.06,"data_type":"double"}},{"title":"targetPose","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/targetPose","period":0.06,"data_type":"double[]"}},{"title":"vel","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/vel","period":0.06,"data_type":"double[]"}}]}],"containers":[{"title":"AUTON CHOOSER","x":256.0,"y":0.0,"width":256.0,"height":256.0,"type":"ComboBox Chooser","properties":{"topic":"/SmartDashboard/AUTON CHOOSER","period":0.06}},{"title":"FIELD","x":896.0,"y":0.0,"width":1024.0,"height":512.0,"type":"Field","properties":{"topic":"/SmartDashboard/FIELD","period":0.06,"field_game":"Crescendo","robot_width":0.85,"robot_length":0.85,"show_other_objects":true,"show_trajectories":true}},{"title":"Gyro","x":0.0,"y":256.0,"width":256.0,"height":256.0,"type":"Gyro","properties":{"topic":"/SmartDashboard/Gyro","period":0.06,"counter_clockwise_positive":false}},{"title":"Field Oriented","x":256.0,"y":256.0,"width":256.0,"height":256.0,"type":"Boolean Box","properties":{"topic":"/SmartDashboard/Field Oriented","period":0.06,"data_type":"boolean","true_color":4283215696,"false_color":4294198070}},{"title":"Swerve Drive","x":0.0,"y":512.0,"width":512.0,"height":384.0,"type":"SwerveDrive","properties":{"topic":"/SmartDashboard/Swerve Drive","period":0.06,"show_robot_rotation":true}},{"title":"FMSInfo","x":512.0,"y":0.0,"width":384.0,"height":256.0,"type":"FMSInfo","properties":{"topic":"/FMSInfo","period":0.06}},{"title":"Back Left Angle","x":512.0,"y":256.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/Back Left Angle","period":0.06,"data_type":"double"}},{"title":"Back Right Angle","x":512.0,"y":384.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/Back Right Angle","period":0.06,"data_type":"double"}},{"title":"Front Left Angle","x":512.0,"y":512.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/Front Left Angle","period":0.06,"data_type":"double"}},{"title":"Front Right Angle","x":512.0,"y":640.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/Front Right Angle","period":0.06,"data_type":"double"}},{"title":"IsRedAlliance","x":512.0,"y":768.0,"width":128.0,"height":128.0,"type":"Boolean Box","properties":{"topic":"/FMSInfo/IsRedAlliance","period":0.06,"data_type":"boolean","true_color":4283215696,"false_color":4294198070}},{"title":"StationNumber","x":640.0,"y":768.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/FMSInfo/StationNumber","period":0.06,"data_type":"int"}},{"title":"limelight","x":896.0,"y":512.0,"width":1024.0,"height":384.0,"type":"Camera Stream","properties":{"topic":"/CameraPublisher/limelight","period":0.06}},{"title":"ReplayNumber","x":768.0,"y":768.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/FMSInfo/ReplayNumber","period":0.06,"data_type":"int"}}]}},{"name":"Info","grid_layout":{"layouts":[{"title":"Swerve","x":512.0,"y":768.0,"width":1024.0,"height":256.0,"type":"List Layout","properties":{"label_position":"TOP"},"children":[{"title":"Built-in Accelerometer","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"3-Axis Accelerometer","properties":{"topic":"/SmartDashboard/Swerve/Built-in Accelerometer","period":0.06}}]}],"containers":[{"title":"Back Left Current","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Left Current","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Back Right Current","x":128.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Right Current","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Left Current","x":0.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Left Current","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Right Current","x":128.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Right Current","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"PIDController[1]","x":512.0,"y":0.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[1]","period":0.06}},{"title":"PIDController[3]","x":768.0,"y":0.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[3]","period":0.06}},{"title":"PIDController[5]","x":1024.0,"y":0.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[5]","period":0.06}},{"title":"PIDController[7]","x":1280.0,"y":0.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[7]","period":0.06}},{"title":"PIDController[2]","x":512.0,"y":384.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[2]","period":0.06}},{"title":"PIDController[4]","x":768.0,"y":384.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[4]","period":0.06}},{"title":"PIDController[6]","x":1024.0,"y":384.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[6]","period":0.06}},{"title":"PIDController[8]","x":1280.0,"y":384.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[8]","period":0.06}},{"title":"Swerve Drive","x":1536.0,"y":0.0,"width":384.0,"height":896.0,"type":"SwerveDrive","properties":{"topic":"/SmartDashboard/Swerve Drive","period":0.06,"show_robot_rotation":true}},{"title":"Gyro","x":0.0,"y":512.0,"width":512.0,"height":384.0,"type":"Gyro","properties":{"topic":"/SmartDashboard/Gyro","period":0.06,"counter_clockwise_positive":false}},{"title":"CAN Utilization %","x":0.0,"y":256.0,"width":256.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/CAN Utilization %","period":0.06,"data_type":"double"}},{"title":"CPU Temperature","x":256.0,"y":0.0,"width":256.0,"height":256.0,"type":"Graph","properties":{"topic":"/SmartDashboard/CPU Temperature","period":0.033,"data_type":"double","time_displayed":5.0,"min_value":null,"max_value":null,"color":4278238420,"line_width":2.0}},{"title":"RSL","x":0.0,"y":384.0,"width":256.0,"height":128.0,"type":"Boolean Box","properties":{"topic":"/SmartDashboard/RSL","period":0.06,"data_type":"boolean","true_color":4283215696,"false_color":4294198070}},{"title":"Voltage","x":256.0,"y":256.0,"width":256.0,"height":256.0,"type":"Graph","properties":{"topic":"/SmartDashboard/Voltage","period":0.033,"data_type":"double","time_displayed":5.0,"min_value":null,"max_value":null,"color":4278238420,"line_width":2.0}}]}}]} +{"tabs":[{"name":"Teleoperated","grid_layout":{"layouts":[],"containers":[{"title":"Front Right Velocity","x":640.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Right Velocity","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Left Velocity","x":512.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Left Velocity","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Field Oriented","x":512.0,"y":384.0,"width":256.0,"height":128.0,"type":"Toggle Switch","properties":{"topic":"/SmartDashboard/Field Oriented","period":0.06,"data_type":"boolean"}},{"title":"Back Right Velocity","x":640.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Right Velocity","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Back Left Velocity","x":512.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Left Velocity","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"default","x":1792.0,"y":640.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/AUTON CHOOSER/default","period":0.06,"data_type":"string"}},{"title":"FIELD","x":768.0,"y":0.0,"width":1152.0,"height":640.0,"type":"Field","properties":{"topic":"/SmartDashboard/FIELD","period":0.06,"field_game":"Crescendo","robot_width":0.85,"robot_length":0.85,"show_other_objects":true,"show_trajectories":true}},{"title":"AUTON CHOOSER","x":1536.0,"y":640.0,"width":256.0,"height":128.0,"type":"ComboBox Chooser","properties":{"topic":"/SmartDashboard/AUTON CHOOSER","period":0.06}},{"title":"FMSInfo","x":1536.0,"y":768.0,"width":384.0,"height":128.0,"type":"FMSInfo","properties":{"topic":"/FMSInfo","period":0.06}},{"title":"IsRedAlliance","x":384.0,"y":256.0,"width":128.0,"height":128.0,"type":"Boolean Box","properties":{"topic":"/FMSInfo/IsRedAlliance","period":0.06,"data_type":"boolean","true_color":4283215696,"false_color":4294198070}},{"title":"Back Left Angle","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Left Angle","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Back Right Angle","x":128.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Right Angle","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Left Angle","x":0.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Left Angle","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Right Angle","x":128.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Right Angle","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"limelight_Interface","x":512.0,"y":768.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/limelight_Interface","period":0.06,"data_type":"string"}},{"title":"limelight_Stream","x":512.0,"y":640.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/limelight_Stream","period":0.06,"data_type":"string"}},{"title":"DESIRED X SPEED","x":512.0,"y":256.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/DESIRED X SPEED","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"DESIRED Y SPEED","x":640.0,"y":256.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/DESIRED Y SPEED","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"limelight","x":0.0,"y":384.0,"width":512.0,"height":512.0,"type":"Camera Stream","properties":{"topic":"/CameraPublisher/limelight","period":0.06}},{"title":"Swerve Drive","x":896.0,"y":640.0,"width":640.0,"height":256.0,"type":"SwerveDrive","properties":{"topic":"/SmartDashboard/Swerve Drive","period":0.06,"show_robot_rotation":true}},{"title":"Swerve Speed","x":0.0,"y":256.0,"width":256.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Swerve Drive/Swerve Speed","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Gyro","x":256.0,"y":0.0,"width":256.0,"height":256.0,"type":"Gyro","properties":{"topic":"/SmartDashboard/Gyro","period":0.06,"counter_clockwise_positive":false}},{"title":"Match Time","x":512.0,"y":512.0,"width":256.0,"height":128.0,"type":"Match Time","properties":{"topic":"/SmartDashboard/Match Time","period":0.06,"data_type":"double","time_display_mode":"Minutes and Seconds","red_start_time":15,"yellow_start_time":30}},{"title":"robot Speed","x":256.0,"y":256.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/robot Speed","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}}]}},{"name":"Autonomous","grid_layout":{"layouts":[{"title":"PathPlanner","x":0.0,"y":0.0,"width":256.0,"height":256.0,"type":"List Layout","properties":{"label_position":"TOP"},"children":[{"title":"activePath","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/activePath","period":0.06,"data_type":"double[]"}},{"title":"currentPose","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/currentPose","period":0.06,"data_type":"double[]"}},{"title":"inaccuracy","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/inaccuracy","period":0.06,"data_type":"double"}},{"title":"targetPose","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/targetPose","period":0.06,"data_type":"double[]"}},{"title":"vel","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/PathPlanner/vel","period":0.06,"data_type":"double[]"}}]}],"containers":[{"title":"AUTON CHOOSER","x":256.0,"y":0.0,"width":256.0,"height":256.0,"type":"ComboBox Chooser","properties":{"topic":"/SmartDashboard/AUTON CHOOSER","period":0.06}},{"title":"FIELD","x":896.0,"y":0.0,"width":1024.0,"height":512.0,"type":"Field","properties":{"topic":"/SmartDashboard/FIELD","period":0.06,"field_game":"Crescendo","robot_width":0.85,"robot_length":0.85,"show_other_objects":true,"show_trajectories":true}},{"title":"Field Oriented","x":256.0,"y":256.0,"width":256.0,"height":256.0,"type":"Boolean Box","properties":{"topic":"/SmartDashboard/Field Oriented","period":0.06,"data_type":"boolean","true_color":4283215696,"false_color":4294198070}},{"title":"Swerve Drive","x":0.0,"y":512.0,"width":512.0,"height":384.0,"type":"SwerveDrive","properties":{"topic":"/SmartDashboard/Swerve Drive","period":0.06,"show_robot_rotation":true}},{"title":"FMSInfo","x":512.0,"y":0.0,"width":384.0,"height":256.0,"type":"FMSInfo","properties":{"topic":"/FMSInfo","period":0.06}},{"title":"Back Left Angle","x":512.0,"y":256.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/Back Left Angle","period":0.06,"data_type":"double"}},{"title":"Back Right Angle","x":512.0,"y":384.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/Back Right Angle","period":0.06,"data_type":"double"}},{"title":"Front Left Angle","x":512.0,"y":512.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/Front Left Angle","period":0.06,"data_type":"double"}},{"title":"Front Right Angle","x":512.0,"y":640.0,"width":384.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/Front Right Angle","period":0.06,"data_type":"double"}},{"title":"IsRedAlliance","x":512.0,"y":768.0,"width":128.0,"height":128.0,"type":"Boolean Box","properties":{"topic":"/FMSInfo/IsRedAlliance","period":0.06,"data_type":"boolean","true_color":4283215696,"false_color":4294198070}},{"title":"StationNumber","x":640.0,"y":768.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/FMSInfo/StationNumber","period":0.06,"data_type":"int"}},{"title":"limelight","x":896.0,"y":512.0,"width":1024.0,"height":384.0,"type":"Camera Stream","properties":{"topic":"/CameraPublisher/limelight","period":0.06}},{"title":"ReplayNumber","x":768.0,"y":768.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/FMSInfo/ReplayNumber","period":0.06,"data_type":"int"}},{"title":"Gyro","x":0.0,"y":256.0,"width":256.0,"height":256.0,"type":"Gyro","properties":{"topic":"/SmartDashboard/Gyro","period":0.06,"counter_clockwise_positive":false}}]}},{"name":"Info","grid_layout":{"layouts":[{"title":"Swerve","x":512.0,"y":768.0,"width":1024.0,"height":256.0,"type":"List Layout","properties":{"label_position":"TOP"},"children":[{"title":"Built-in Accelerometer","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"3-Axis Accelerometer","properties":{"topic":"/SmartDashboard/Swerve/Built-in Accelerometer","period":0.06}}]}],"containers":[{"title":"Back Left Current","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Left Current","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Back Right Current","x":128.0,"y":0.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Back Right Current","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Left Current","x":0.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Left Current","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Front Right Current","x":128.0,"y":128.0,"width":128.0,"height":128.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Front Right Current","period":0.06,"data_type":"double","min_value":-1.0,"max_value":1.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"PIDController[1]","x":512.0,"y":0.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[1]","period":0.06}},{"title":"PIDController[3]","x":768.0,"y":0.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[3]","period":0.06}},{"title":"PIDController[5]","x":1024.0,"y":0.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[5]","period":0.06}},{"title":"PIDController[7]","x":1280.0,"y":0.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[7]","period":0.06}},{"title":"PIDController[2]","x":512.0,"y":384.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[2]","period":0.06}},{"title":"PIDController[4]","x":768.0,"y":384.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[4]","period":0.06}},{"title":"PIDController[6]","x":1024.0,"y":384.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[6]","period":0.06}},{"title":"PIDController[8]","x":1280.0,"y":384.0,"width":256.0,"height":384.0,"type":"PIDController","properties":{"topic":"/SmartDashboard/PIDController[8]","period":0.06}},{"title":"Swerve Drive","x":1536.0,"y":0.0,"width":384.0,"height":896.0,"type":"SwerveDrive","properties":{"topic":"/SmartDashboard/Swerve Drive","period":0.06,"show_robot_rotation":true}},{"title":"Gyro","x":0.0,"y":512.0,"width":512.0,"height":384.0,"type":"Gyro","properties":{"topic":"/SmartDashboard/Gyro","period":0.06,"counter_clockwise_positive":false}},{"title":"CAN Utilization %","x":0.0,"y":256.0,"width":256.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/CAN Utilization %","period":0.06,"data_type":"double"}},{"title":"CPU Temperature","x":256.0,"y":0.0,"width":256.0,"height":256.0,"type":"Graph","properties":{"topic":"/SmartDashboard/CPU Temperature","period":0.033,"data_type":"double","time_displayed":5.0,"min_value":null,"max_value":null,"color":4278238420,"line_width":2.0}},{"title":"RSL","x":0.0,"y":384.0,"width":256.0,"height":128.0,"type":"Boolean Box","properties":{"topic":"/SmartDashboard/RSL","period":0.06,"data_type":"boolean","true_color":4283215696,"false_color":4294198070}},{"title":"Voltage","x":256.0,"y":256.0,"width":256.0,"height":256.0,"type":"Graph","properties":{"topic":"/SmartDashboard/Voltage","period":0.033,"data_type":"double","time_displayed":5.0,"min_value":null,"max_value":null,"color":4278238420,"line_width":2.0}}]}}]} \ No newline at end of file From 17dbb613fe081cac3af7423457b9c7b5367e915c Mon Sep 17 00:00:00 2001 From: cherriae Date: Fri, 26 Jan 2024 14:52:37 -0500 Subject: [PATCH 5/9] equations for auto rotate swerve --- .../frc/robot/subsystems/SwerveDriveSubsystem.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 3f820b5..8e53b9d 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -401,10 +401,15 @@ public double[] anglesToSpeaker() { Rotation2d currentRotation = getHeading(); - double rotateDifference = Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); - - double angleX = currentRotation.getDegrees() - rotateDifference; + double angleX = 0; + if (getPose().getY() > Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()){ + angleX = -currentRotation.getDegrees() + Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); + } + else{ + angleX = -currentRotation.getDegrees() - Math.atan((Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY() - getPose().getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); + } + double[] angles = {angleX, angleY}; return angles; From a0e7b266e18b5a5185d75f45db59944bb68b497d Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 26 Jan 2024 14:56:37 -0500 Subject: [PATCH 6/9] debug for auto aim test --- .../frc/robot/commands/shooter/AutoAim.java | 19 ++++++++++++++----- .../subsystems/SwerveDriveSubsystem.java | 2 ++ .../frc/robot/subsystems/VisionSubsystem.java | 2 +- 3 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index d75b24f..b419cac 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java @@ -62,24 +62,33 @@ public void execute() { double currentSwerveHeading = _swerve.getHeading().getDegrees(); - if (_vision.isApriltagVisible()) { - desiredShooterAngle = _vision.anglesToSpeaker()[1]; // ty - desiredSwerveHeading = _vision.anglesToSpeaker()[0]; + SmartDashboard.putBoolean("VISIBLE TAG", _vision.isApriltagVisible()); + + double[] visionAngles = _vision.anglesToSpeaker(); + + if (_vision.isApriltagVisible() && visionAngles != null) { + desiredShooterAngle = visionAngles[1]; + desiredSwerveHeading = visionAngles[0]; + + SmartDashboard.putNumberArray("VISION ANGLES", visionAngles); } else { desiredShooterAngle = _swerve.anglesToSpeaker()[1]; desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; } - desiredSwerveHeading += currentSwerveHeading; + desiredSwerveHeading -= currentSwerveHeading; _shooter.setAngle(desiredShooterAngle); + SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading); + _swerve.driveChassis( new ChassisSpeeds( _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - _headingController.calculate(currentSwerveHeading, desiredSwerveHeading) + // _headingController.calculate(currentSwerveHeading, desiredSwerveHeading), + 0 ) ); } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index d8f2004..4e7055e 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -156,6 +156,8 @@ public ChassisSpeeds getRobotRelativeSpeeds() { public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) { _visionSubsystem = visionSubsystem; + resetPose(_visionSubsystem.getBotpose().get()); + // setupOrchestra(); // pathplannerlib setup diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 782776c..c004377 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -100,7 +100,7 @@ public boolean isApriltagVisible() { public boolean isApriltagVisible(int ID) { if (!isApriltagVisible()) return false; - return _limelight.getTag(ID) != null; + return (_limelight.getTag(ID) != null); } /** From 288efc6d27b3d3ae6362a9d2f467acb45221de6c Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 26 Jan 2024 16:12:17 -0500 Subject: [PATCH 7/9] need to rewrite auto aim --- .../frc/robot/commands/shooter/AutoAim.java | 40 +++++++++----- .../subsystems/SwerveDriveSubsystem.java | 55 +++++++++++++------ .../frc/robot/subsystems/VisionSubsystem.java | 2 +- .../java/frc/robot/utils/LimelightHelper.java | 2 - 4 files changed, 64 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index b419cac..6a2e54d 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java @@ -5,6 +5,7 @@ import java.util.function.DoubleSupplier; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -24,7 +25,7 @@ public class AutoAim extends Command { private DoubleSupplier _ySpeed; private PIDController _headingController = new PIDController( - Constants.PID.PP_ROTATION.kP, + 0.05, 0, 0 ); @@ -46,6 +47,7 @@ public AutoAim( _ySpeed = ySpeed; _headingController.setTolerance(2); + _headingController.enableContinuousInput(-180, 180); addRequirements(_shooter, _vision, _swerve); } @@ -66,29 +68,39 @@ public void execute() { double[] visionAngles = _vision.anglesToSpeaker(); - if (_vision.isApriltagVisible() && visionAngles != null) { - desiredShooterAngle = visionAngles[1]; - desiredSwerveHeading = visionAngles[0]; + // if (_vision.isApriltagVisible() && visionAngles != null) { + // desiredShooterAngle = visionAngles[1]; + // desiredSwerveHeading = visionAngles[0]; - SmartDashboard.putNumberArray("VISION ANGLES", visionAngles); + // SmartDashboard.putNumberArray("VISION ANGLES", visionAngles); - } else { - desiredShooterAngle = _swerve.anglesToSpeaker()[1]; - desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; - } + // } else { + // desiredShooterAngle = _swerve.anglesToSpeaker()[1]; + // desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; + // } - desiredSwerveHeading -= currentSwerveHeading; + // desiredSwerveHeading -= currentSwerveHeading; - _shooter.setAngle(desiredShooterAngle); + // SmartDashboard.putNumber("DESIRED HEADING 360", desiredSwerveHeading); - SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading); + // desiredSwerveHeading = MathUtil.angleModulus(Math.toRadians(desiredSwerveHeading)); + // desiredSwerveHeading = Math.toDegrees(desiredSwerveHeading); + + // _shooter.setAngle(desiredShooterAngle); + + // SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading); + + desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; _swerve.driveChassis( new ChassisSpeeds( _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - // _headingController.calculate(currentSwerveHeading, desiredSwerveHeading), - 0 + MathUtil.clamp( + _headingController.calculate(currentSwerveHeading, desiredSwerveHeading), + -Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED, + Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED + ) ) ); } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 2c6809e..d02e235 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -226,6 +227,9 @@ public void initSendable(SendableBuilder builder) { public void periodic() { publisher.set(states); + SmartDashboard.putNumber("Gyro 180/-180", getHeading().getDegrees()); + anglesToSpeaker(); + // This method will be called once per scheduler run // SmartDashboard.putNumber("Gyro", getHeading().getDegrees()); SmartDashboard.putBoolean("Field Oriented", fieldOriented); @@ -358,7 +362,7 @@ public void resetGyro() { resetPose(new_pose); } - + /** Resets pose estimator's translation of the drive to (0, 0). */ public void resetTranslation() { Pose2d new_pose = new Pose2d(0, 0, _pose.getRotation()); @@ -391,31 +395,46 @@ public Rotation2d getHeadingRaw() { /** Get the shooter's angle to the speaker hole using the drive's pose estimator. */ public double[] anglesToSpeaker() { - int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; - Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); + // int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; + // Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); - double xDifference = tagPose.getX() - _pose.getX(); - double yDifference = tagPose.getY() - _pose.getY(); + // Translation2d tagTranslation = new Translation2d(tagPose.getX(), tagPose.getY()); + // Translation2d botTranslation = _pose.getTranslation(); - double distanceToRobot = Math.sqrt(Math.pow(xDifference, 2) + Math.pow(yDifference, 2)); + // Rotation2d moveAngle = tagTranslation.minus(botTranslation).getAngle(); - double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED; + // double[] a = {1, 2}; - double angleY = Math.atan(zDifference / distanceToRobot); + // SmartDashboard.putNumber("MOVE ANGLE", moveAngle.getDegrees()); + double[] a = {1, 2}; - Rotation2d currentRotation = getHeading(); - double angleX = 0; + return a; - if (getPose().getY() > Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()){ - angleX = -currentRotation.getDegrees() + Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); - } - else{ - angleX = -currentRotation.getDegrees() - Math.atan((Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY() - getPose().getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); - } + // double xDifference = tagPose.getX() - _pose.getX(); + // double yDifference = tagPose.getY() - _pose.getY(); + + // Constants.FIELD_CONSTANTS.SPEAKER_POSE.minus(new Pose2d()); + + // double distanceToRobot = Math.sqrt(Math.pow(xDifference, 2) + Math.pow(yDifference, 2)); + + // double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED; + + // double angleY = Math.atan(zDifference / distanceToRobot); + + + // Rotation2d currentRotation = getHeading(); + // double angleX = 0; + + // if (getPose().getY() > Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()){ + // angleX = -currentRotation.getDegrees() + Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); + // } + // else{ + // angleX = -currentRotation.getDegrees() - Math.atan((Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY() - getPose().getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX())); + // } - double[] angles = {angleX, angleY}; + // double[] angles = {angleX, angleY}; - return angles; + // return angles; } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index c004377..782776c 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -100,7 +100,7 @@ public boolean isApriltagVisible() { public boolean isApriltagVisible(int ID) { if (!isApriltagVisible()) return false; - return (_limelight.getTag(ID) != null); + return _limelight.getTag(ID) != null; } /** diff --git a/src/main/java/frc/robot/utils/LimelightHelper.java b/src/main/java/frc/robot/utils/LimelightHelper.java index 1af35ca..4ff0fb8 100644 --- a/src/main/java/frc/robot/utils/LimelightHelper.java +++ b/src/main/java/frc/robot/utils/LimelightHelper.java @@ -76,6 +76,4 @@ public JsonNode getTag(int ID) { return null; } - - } From 3a91fdd4c37f852fe68fc2f3ba899748dfa90ade Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 26 Jan 2024 17:12:47 -0500 Subject: [PATCH 8/9] made x angles to speaker using swerve drive --- .../deploy/pathplanner/paths/amp path.path | 2 +- src/main/java/frc/robot/RobotContainer.java | 1 + .../subsystems/SwerveDriveSubsystem.java | 28 ++++++++++++++++--- 3 files changed, 26 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/amp path.path b/src/main/deploy/pathplanner/paths/amp path.path index 2068f9a..601a2ee 100644 --- a/src/main/deploy/pathplanner/paths/amp path.path +++ b/src/main/deploy/pathplanner/paths/amp path.path @@ -58,4 +58,4 @@ "folder": null, "previewStartingState": null, "useDefaultConstraints": false -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f8fce23..1ea4151 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index d02e235..03102e4 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -7,6 +7,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -395,6 +397,28 @@ public Rotation2d getHeadingRaw() { /** Get the shooter's angle to the speaker hole using the drive's pose estimator. */ public double[] anglesToSpeaker() { + int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; + Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); + + Translation2d tagTranslation = new Translation2d(tagPose.getX(), tagPose.getY()); + Translation2d botTranslation = _pose.getTranslation(); + + double desiredHeading = MathUtil.inputModulus( + tagTranslation.minus(botTranslation).getAngle().getDegrees(), + -180, + 180 + ); + + double xAngleToSpeaker = desiredHeading - _pose.getRotation().getDegrees(); + double yAngleToSpeaker = 0; + + double[] angles = { + xAngleToSpeaker, + yAngleToSpeaker + }; + + return angles; + // int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; // Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); @@ -407,10 +431,6 @@ public double[] anglesToSpeaker() { // SmartDashboard.putNumber("MOVE ANGLE", moveAngle.getDegrees()); - double[] a = {1, 2}; - - return a; - // double xDifference = tagPose.getX() - _pose.getX(); // double yDifference = tagPose.getY() - _pose.getY(); From 523e8b170d3e1f206c4e65870f97d7646ce886d1 Mon Sep 17 00:00:00 2001 From: PGgit08 <60079012+PGgit08@users.noreply.github.com> Date: Fri, 26 Jan 2024 19:16:37 -0500 Subject: [PATCH 9/9] angle getting works! --- .../paths/Really Short Path pt.1.path | 6 +- .../frc/robot/commands/shooter/AutoAim.java | 38 ++++++----- .../subsystems/SwerveDriveSubsystem.java | 66 ++++++++++--------- 3 files changed, 59 insertions(+), 51 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Really Short Path pt.1.path b/src/main/deploy/pathplanner/paths/Really Short Path pt.1.path index c667cff..16ec760 100644 --- a/src/main/deploy/pathplanner/paths/Really Short Path pt.1.path +++ b/src/main/deploy/pathplanner/paths/Really Short Path pt.1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.9099007567384503, + "x": 1.91, "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 2.2637184360915796, + "x": 2.2638176793531297, "y": 7.0 }, "isLocked": false, @@ -58,4 +58,4 @@ "folder": null, "previewStartingState": null, "useDefaultConstraints": false -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java index 6a2e54d..32cce50 100644 --- a/src/main/java/frc/robot/commands/shooter/AutoAim.java +++ b/src/main/java/frc/robot/commands/shooter/AutoAim.java @@ -59,14 +59,16 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double desiredShooterAngle = 0; - double desiredSwerveHeading = 0; + // TODO: Vision or Pose estimator only? - double currentSwerveHeading = _swerve.getHeading().getDegrees(); + // double desiredShooterAngle = 0; + // double desiredSwerveHeading = 0; - SmartDashboard.putBoolean("VISIBLE TAG", _vision.isApriltagVisible()); + // double currentSwerveHeading = _swerve.getHeading().getDegrees(); - double[] visionAngles = _vision.anglesToSpeaker(); + // SmartDashboard.putBoolean("VISIBLE TAG", _vision.isApriltagVisible()); + + // double[] visionAngles = _vision.anglesToSpeaker(); // if (_vision.isApriltagVisible() && visionAngles != null) { // desiredShooterAngle = visionAngles[1]; @@ -90,19 +92,19 @@ public void execute() { // SmartDashboard.putNumber("DESIRED HEADING", desiredSwerveHeading); - desiredSwerveHeading = _swerve.anglesToSpeaker()[0]; - - _swerve.driveChassis( - new ChassisSpeeds( - _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - MathUtil.clamp( - _headingController.calculate(currentSwerveHeading, desiredSwerveHeading), - -Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED, - Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED - ) - ) - ); + // desiredSwerveHeading = _swerve.speakerAngles()[0]; + + // _swerve.driveChassis( + // new ChassisSpeeds( + // _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + // _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + // MathUtil.clamp( + // _headingController.calculate(currentSwerveHeading, desiredSwerveHeading), + // -Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED, + // Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED + // ) + // ) + // ); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 03102e4..ff7f0bb 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -103,9 +103,6 @@ public class SwerveDriveSubsystem extends SubsystemBase { private final Orchestra _orchestra = new Orchestra(); String song = "output.chrp"; - // estimated pose - private Pose2d _pose = new Pose2d(); - private Field2d _field = new Field2d(); /** A boolean for whether the swerve is field oriented or not. */ @@ -147,7 +144,7 @@ public class SwerveDriveSubsystem extends SubsystemBase { /** Return the estimated pose of the swerve chassis. */ public Pose2d getPose() { - return _pose; + return _estimator.getEstimatedPosition(); } /** Get the drive's chassis speeds (robot relative). */ @@ -159,7 +156,7 @@ public ChassisSpeeds getRobotRelativeSpeeds() { public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) { _visionSubsystem = visionSubsystem; - resetPose(_visionSubsystem.getBotpose().get()); + resetPose(_visionSubsystem.getBotpose().get()); // for testing // setupOrchestra(); @@ -230,7 +227,8 @@ public void periodic() { publisher.set(states); SmartDashboard.putNumber("Gyro 180/-180", getHeading().getDegrees()); - anglesToSpeaker(); + + System.out.println(speakerAngles()[0]); // This method will be called once per scheduler run // SmartDashboard.putNumber("Gyro", getHeading().getDegrees()); @@ -248,15 +246,15 @@ public void periodic() { _backLeft.displayInfo(); // Update the bot's pose - _pose = - _estimator.update( - getHeadingRaw(), - new SwerveModulePosition[] { - _frontLeft.getPosition(), - _frontRight.getPosition(), - _backRight.getPosition(), - _backLeft.getPosition() - }); + _estimator.update( + getHeadingRaw(), + new SwerveModulePosition[] { + _frontLeft.getPosition(), + _frontRight.getPosition(), + _backRight.getPosition(), + _backLeft.getPosition() + } + ); if (_visionSubsystem.isApriltagVisible()) { Optional visionBotpose = _visionSubsystem.getBotpose(); @@ -356,10 +354,11 @@ public SwerveModuleState[] getStates() { /** Resets the pose estimator's heading of the drive to 0. */ public void resetGyro() { + Pose2d current_pose = getPose(); Pose2d new_pose = new Pose2d( - _pose.getTranslation().getX(), - _pose.getTranslation().getY(), + current_pose.getTranslation().getX(), + current_pose.getTranslation().getY(), Rotation2d.fromDegrees(0)); resetPose(new_pose); @@ -367,7 +366,7 @@ public void resetGyro() { /** Resets pose estimator's translation of the drive to (0, 0). */ public void resetTranslation() { - Pose2d new_pose = new Pose2d(0, 0, _pose.getRotation()); + Pose2d new_pose = new Pose2d(0, 0, getPose().getRotation()); resetPose(new_pose); } @@ -395,30 +394,37 @@ public Rotation2d getHeadingRaw() { return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360)); } - /** Get the shooter's angle to the speaker hole using the drive's pose estimator. */ - public double[] anglesToSpeaker() { + /** + * Get the setpoint x and y angles for the drive/shooter for auto-aim. + * + * @return [xSpeakerAngle, ySpeakerAngle] + */ + public double[] speakerAngles() { + double xSpeakerAngle; + double ySpeakerAngle; + int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get(); Translation2d tagTranslation = new Translation2d(tagPose.getX(), tagPose.getY()); - Translation2d botTranslation = _pose.getTranslation(); + Translation2d botTranslation = getPose().getTranslation(); - double desiredHeading = MathUtil.inputModulus( - tagTranslation.minus(botTranslation).getAngle().getDegrees(), - -180, - 180 - ); + Translation2d distanceVec = tagTranslation.minus(botTranslation); - double xAngleToSpeaker = desiredHeading - _pose.getRotation().getDegrees(); - double yAngleToSpeaker = 0; + xSpeakerAngle = MathUtil.inputModulus(distanceVec.getAngle().getDegrees(), -180, 180); + + double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED; // TODO: move to Constants? + ySpeakerAngle = Math.atan(zDifference / distanceVec.getNorm()); double[] angles = { - xAngleToSpeaker, - yAngleToSpeaker + xSpeakerAngle, + ySpeakerAngle }; return angles; + // OLD CODE BELOW + // int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG; // Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get();