From e7d2c7b5cd4ea7221a974c575fbb524d8c9636c3 Mon Sep 17 00:00:00 2001 From: Iron Golem Date: Tue, 21 Feb 2023 20:20:39 -0600 Subject: [PATCH 01/12] changed all occurances of onTrue in robot container to whileTrue. --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d4062bb..f9347d3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,16 +64,16 @@ private void configureBindings() { * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND) ); - driverControllerBackButton.onTrue(new InstantCommand(drivetrainSubsystem::zeroGyroscope)); - driverControllerAButton.onTrue(new SetDriveTarget(drivetrainSubsystem, 0)); - driverControllerYButton.onTrue(new SetDriveTarget(drivetrainSubsystem, 180)); + driverControllerBackButton.whileTrue(new InstantCommand(drivetrainSubsystem::zeroGyroscope)); + driverControllerAButton.whileTrue(new SetDriveTarget(drivetrainSubsystem, 0)); + driverControllerYButton.whileTrue(new SetDriveTarget(drivetrainSubsystem, 180)); - operatorControllerXButton.onTrue(new FloorPickup(floorIntake)); - operatorControllerAButton.onTrue(new FloorDrop(floorIntake)); - operatorControllerAButton.onTrue(new DropCone(intake)); - operatorControllerBButton.onTrue(new DropCube(intake)); - operatorControllerXButton.onTrue(new IntakeCone(intake)); - operatorControllerYButton.onTrue(new IntakeCube(intake)); + operatorControllerXButton.whileTrue(new FloorPickup(floorIntake)); + operatorControllerAButton.whileTrue(new FloorDrop(floorIntake)); + operatorControllerAButton.whileTrue(new DropCone(intake)); + operatorControllerBButton.whileTrue(new DropCube(intake)); + operatorControllerXButton.whileTrue(new IntakeCone(intake)); + operatorControllerYButton.whileTrue(new IntakeCube(intake)); } public Command getAutonomousCommand() { From 32b8e75bebd713e0d172254fcaaab3c447fc42ed Mon Sep 17 00:00:00 2001 From: Iron Golem Date: Tue, 21 Feb 2023 20:22:44 -0600 Subject: [PATCH 02/12] changed driverController buttons to use onTrue --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9347d3..783923b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,9 +64,9 @@ private void configureBindings() { * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND) ); - driverControllerBackButton.whileTrue(new InstantCommand(drivetrainSubsystem::zeroGyroscope)); - driverControllerAButton.whileTrue(new SetDriveTarget(drivetrainSubsystem, 0)); - driverControllerYButton.whileTrue(new SetDriveTarget(drivetrainSubsystem, 180)); + driverControllerBackButton.onTrue(new InstantCommand(drivetrainSubsystem::zeroGyroscope)); + driverControllerAButton.onTrue(new SetDriveTarget(drivetrainSubsystem, 0)); + driverControllerYButton.onTrue(new SetDriveTarget(drivetrainSubsystem, 180)); operatorControllerXButton.whileTrue(new FloorPickup(floorIntake)); operatorControllerAButton.whileTrue(new FloorDrop(floorIntake)); From 9725cdcd86394dce6b52bfeb6e9254c36305f890 Mon Sep 17 00:00:00 2001 From: Iron Golem Date: Tue, 14 Mar 2023 19:18:21 -0500 Subject: [PATCH 03/12] Created AutoAlign command, mapped to DriverDPad Up and added required constants --- simgui.json | 4 ++ src/main/java/frc/robot/Constants.java | 7 +++ src/main/java/frc/robot/RobotContainer.java | 8 +++ .../vision/PlayerStationAutoAlign.java | 49 +++++++++++++++++++ .../java/frc/robot/subsystems/Vision.java | 10 ++++ 5 files changed, 78 insertions(+) create mode 100644 src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java diff --git a/simgui.json b/simgui.json index 482812c..a7b56d9 100644 --- a/simgui.json +++ b/simgui.json @@ -2,6 +2,10 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", + "/Shuffleboard/Auto/Auto Location": "String Chooser", + "/Shuffleboard/Auto/Auto Path Action": "String Chooser", + "/Shuffleboard/Auto/Auto Start Action": "String Chooser", + "/Shuffleboard/Auto/Current Auto Trajectory (Always appears on blue side)": "Field2d", "/SmartDashboard/Field Sim": "Field2d" }, "windows": { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e4f0f8e..b63794a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,9 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + public final class Constants { //auto constants public static final double AUTO_MAX_VELOCITY = 3; @@ -21,6 +24,10 @@ public final class Constants { public static final double FIELD_WIDTH_METERS = 16; public static final double FIELD_HEIGHT_METERS = 7.85; + public static final Pose2d RED_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(3.3, 5.5, Rotation2d.fromDegrees(180)); + public static final Pose2d BLUE_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(13.0, 5.5, Rotation2d.fromDegrees(0)); + + //sim constants public static final double RED_ORIGIN_POS_X_METERS = 16.541748984; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af433a3..7ab2931 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.commands.vision.VisionPoseUpdate; import frc.robot.commands.arm.MoveArmToPosition; import frc.robot.commands.drive.DefaultDriveUpdatePose; import frc.robot.commands.drive.SetDriveRotationTarget; @@ -21,6 +22,7 @@ import frc.robot.commands.intake.armIntake.IntakeCube; import frc.robot.commands.intake.floor.FloorDrop; import frc.robot.commands.intake.floor.FloorPickup; +import frc.robot.commands.vision.PlayerStationAutoAlign; import frc.robot.commands.vision.ToggleLED; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.DrivetrainSubsystem; @@ -66,6 +68,11 @@ public class RobotContainer { private final DPadButton operatorDPadLeft = new DPadButton(operatorController, DPadButton.Direction.LEFT); private final DPadButton operatorDPadDown = new DPadButton(operatorController, DPadButton.Direction.DOWN); + private final DPadButton driverDPadUp = new DPadButton(operatorController, DPadButton.Direction.UP); + private final DPadButton driverDPadRight = new DPadButton(operatorController, DPadButton.Direction.RIGHT); + private final DPadButton driverDPadLeft = new DPadButton(operatorController, DPadButton.Direction.LEFT); + private final DPadButton driverDPadDown = new DPadButton(operatorController, DPadButton.Direction.DOWN); + public RobotContainer() { configureBindings(); } @@ -183,6 +190,7 @@ private void configureBindings() { ); //vision operatorControllerStartButton.toggleOnTrue(new ToggleLED(vision)); + driverDPadUp.toggleOnTrue(new PlayerStationAutoAlign(drivetrainSubsystem, vision)); } } diff --git a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java new file mode 100644 index 0000000..593686f --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java @@ -0,0 +1,49 @@ +// 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.vision; +import frc.robot.Constants; +import frc.robot.subsystems.DrivetrainSubsystem; +import frc.robot.subsystems.Vision; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class PlayerStationAutoAlign extends CommandBase { + /** Creates a new PlayerStationAutoAlign. */ + private final Vision vision; + private final DrivetrainSubsystem drivetrainSubsystem; + public PlayerStationAutoAlign(DrivetrainSubsystem drivetrainSubsystem, Vision vision) { + // Use addRequirements() here to declare subsystem dependencies. + this.vision = vision; + this.drivetrainSubsystem = drivetrainSubsystem; + addRequirements(this.vision, this.drivetrainSubsystem); + } + + // 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() { + if(vision.getAlliance()){ + if(vision.getBotpose().getX() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getX() && vision.getBotpose().getY() > Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ + drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); + } + }else{ + if(vision.getBotpose().getX() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX() && vision.getBotpose().getY() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX()){ + drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); + } + } + } + + // 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index f4e82ee..5435435 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -3,6 +3,8 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; +import javax.lang.model.element.ModuleElement.DirectiveKind; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTable; @@ -72,6 +74,14 @@ public boolean hasTarget() { return targetValid.getDouble(0) == 1; } + public boolean getAlliance(){ + if(DriverStation.getAlliance() == Alliance.Red){ + return true; + }else{ + return false; + } + } + public void setMode(CamMode mode) { if(mode == CamMode.VISION) { table.getEntry("camMode").setNumber(0); From 282f1580d66ad87c1f0f9efe3eec343ef4c05dce Mon Sep 17 00:00:00 2001 From: Iron Golem Date: Tue, 14 Mar 2023 20:20:46 -0500 Subject: [PATCH 04/12] redefined x and y values for the human player stations and added rotation override for the player station april tags --- src/main/java/frc/robot/Constants.java | 4 ++-- .../frc/robot/commands/vision/PlayerStationAutoAlign.java | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b63794a..a7839b9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,8 +24,8 @@ public final class Constants { public static final double FIELD_WIDTH_METERS = 16; public static final double FIELD_HEIGHT_METERS = 7.85; - public static final Pose2d RED_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(3.3, 5.5, Rotation2d.fromDegrees(180)); - public static final Pose2d BLUE_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(13.0, 5.5, Rotation2d.fromDegrees(0)); + public static final Pose2d RED_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(3.7, 5.5, Rotation2d.fromDegrees(180)); + public static final Pose2d BLUE_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(12.6, 5.5, Rotation2d.fromDegrees(0)); diff --git a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java index 593686f..0b239ac 100644 --- a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java +++ b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java @@ -26,7 +26,11 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(vision.getAlliance()){ + if(vision.getTargetID() == 4){ + drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); + }else if(vision.getTargetID() == 5){ + drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); + }else if(vision.getAlliance()){ if(vision.getBotpose().getX() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getX() && vision.getBotpose().getY() > Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); } From ef17fc154b36155183f3c211b37944f49ff74729 Mon Sep 17 00:00:00 2001 From: jagernet-ops Date: Wed, 15 Mar 2023 16:51:06 +0000 Subject: [PATCH 05/12] fixed some merging errors and added alliance checking in the PlayerStationAutoAlign command --- src/main/java/frc/robot/RobotContainer.java | 4 ---- .../commands/vision/PlayerStationAutoAlign.java | 13 ++++++++----- src/main/java/frc/robot/subsystems/Vision.java | 9 --------- 3 files changed, 8 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f555094..95203eb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,11 +11,7 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -<<<<<<< HEAD -import frc.robot.commands.vision.VisionPoseUpdate; -======= import frc.robot.commands.arm.IncrementArm; ->>>>>>> main import frc.robot.commands.arm.MoveArmToPosition; import frc.robot.commands.drive.DefaultDriveUpdatePose; import frc.robot.commands.drive.SetDriveRotationTarget; diff --git a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java index 0b239ac..2795212 100644 --- a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java +++ b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java @@ -3,9 +3,12 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands.vision; + import frc.robot.Constants; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.Vision; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.CommandBase; public class PlayerStationAutoAlign extends CommandBase { @@ -16,7 +19,7 @@ public PlayerStationAutoAlign(DrivetrainSubsystem drivetrainSubsystem, Vision vi // Use addRequirements() here to declare subsystem dependencies. this.vision = vision; this.drivetrainSubsystem = drivetrainSubsystem; - addRequirements(this.vision, this.drivetrainSubsystem); + addRequirements(this.drivetrainSubsystem); } // Called when the command is initially scheduled. @@ -26,15 +29,15 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(vision.getTargetID() == 4){ + if(vision.getTargetID() == 4){ // Override if Blue Alliance Human Player April Tag is seen drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); - }else if(vision.getTargetID() == 5){ + }else if(vision.getTargetID() == 5){ // Override if RED Alliance Human Player April Tag is seen drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); - }else if(vision.getAlliance()){ + }else if(DriverStation.getAlliance() == Alliance.Red){ // For Red Alliance if(vision.getBotpose().getX() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getX() && vision.getBotpose().getY() > Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); } - }else{ + }else{ // For Blue Alliance if(vision.getBotpose().getX() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX() && vision.getBotpose().getY() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX()){ drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 5435435..e5a94a5 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; -import javax.lang.model.element.ModuleElement.DirectiveKind; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -74,14 +73,6 @@ public boolean hasTarget() { return targetValid.getDouble(0) == 1; } - public boolean getAlliance(){ - if(DriverStation.getAlliance() == Alliance.Red){ - return true; - }else{ - return false; - } - } - public void setMode(CamMode mode) { if(mode == CamMode.VISION) { table.getEntry("camMode").setNumber(0); From 98e03c094d57d98c5c2ebdc26094443be4e217d2 Mon Sep 17 00:00:00 2001 From: jager Date: Fri, 17 Mar 2023 13:51:45 -0700 Subject: [PATCH 06/12] merged with main, updated the AprilTag overrides in the command to also update the robot's position, and changed command conditional from toggleOnTrue to onTrue --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 10 +++++----- .../robot/commands/vision/PlayerStationAutoAlign.java | 2 ++ .../java/frc/robot/subsystems/DrivetrainSubsystem.java | 1 + 4 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b28ebd9..200783c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,6 +24,8 @@ public final class Constants { public static final double FIELD_WIDTH_METERS = 16; public static final double FIELD_HEIGHT_METERS = 7.85; + + // public static final Pose2d UNIFIED_HUMAN_BOUNDARY = new Pose2d(3.7, 5.5, Rotation2d.fromDegrees(180)); public static final Pose2d RED_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(3.7, 5.5, Rotation2d.fromDegrees(180)); public static final Pose2d BLUE_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(12.6, 5.5, Rotation2d.fromDegrees(0)); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 95203eb..c3f3632 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,10 +68,10 @@ public class RobotContainer { private final DPadButton operatorDPadLeft = new DPadButton(operatorController, DPadButton.Direction.LEFT); private final DPadButton operatorDPadDown = new DPadButton(operatorController, DPadButton.Direction.DOWN); - private final DPadButton driverDPadUp = new DPadButton(operatorController, DPadButton.Direction.UP); - private final DPadButton driverDPadRight = new DPadButton(operatorController, DPadButton.Direction.RIGHT); - private final DPadButton driverDPadLeft = new DPadButton(operatorController, DPadButton.Direction.LEFT); - private final DPadButton driverDPadDown = new DPadButton(operatorController, DPadButton.Direction.DOWN); + private final DPadButton driverDPadUp = new DPadButton(driverController, DPadButton.Direction.UP); + private final DPadButton driverDPadRight = new DPadButton(driverController, DPadButton.Direction.RIGHT); + private final DPadButton driverDPadLeft = new DPadButton(driverController, DPadButton.Direction.LEFT); + private final DPadButton driverDPadDown = new DPadButton(driverController, DPadButton.Direction.DOWN); public RobotContainer() { configureBindings(); @@ -194,7 +194,7 @@ private void configureBindings() { ); //vision operatorControllerStartButton.toggleOnTrue(new ToggleLED(vision)); - driverDPadUp.toggleOnTrue(new PlayerStationAutoAlign(drivetrainSubsystem, vision)); + driverDPadUp.onTrue(new PlayerStationAutoAlign(drivetrainSubsystem, vision)); } } diff --git a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java index 2795212..ae9972c 100644 --- a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java +++ b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java @@ -30,8 +30,10 @@ public void initialize() {} @Override public void execute() { if(vision.getTargetID() == 4){ // Override if Blue Alliance Human Player April Tag is seen + drivetrainSubsystem.resetPose(vision.getBotpose()); drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); }else if(vision.getTargetID() == 5){ // Override if RED Alliance Human Player April Tag is seen + drivetrainSubsystem.resetPose(vision.getBotpose()); drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); }else if(DriverStation.getAlliance() == Alliance.Red){ // For Red Alliance if(vision.getBotpose().getX() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getX() && vision.getBotpose().getY() > Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index a4b84f6..16e25c8 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -198,6 +198,7 @@ public SwerveDriveKinematics getKinematics() { return kinematics; } + // Should I be using this method? public Pose2d getPose() { return robotPose; } From 35add96f8ca229b97a04672006cf1ed98b158063 Mon Sep 17 00:00:00 2001 From: jager Date: Fri, 17 Mar 2023 14:43:54 -0700 Subject: [PATCH 07/12] swapped from using vision.getBotPose() to drivetrainSubsystem.getPose() in command --- .../frc/robot/commands/vision/PlayerStationAutoAlign.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java index ae9972c..ad077a7 100644 --- a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java +++ b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java @@ -36,11 +36,11 @@ public void execute() { drivetrainSubsystem.resetPose(vision.getBotpose()); drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); }else if(DriverStation.getAlliance() == Alliance.Red){ // For Red Alliance - if(vision.getBotpose().getX() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getX() && vision.getBotpose().getY() > Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ + if(drivetrainSubsystem.getPose().getX() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() > Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); } }else{ // For Blue Alliance - if(vision.getBotpose().getX() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX() && vision.getBotpose().getY() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX()){ + if(drivetrainSubsystem.getPose().getX() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX()){ drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); } } From 61f276d1fbb632857ea5f49f3a7f91faa04c8c81 Mon Sep 17 00:00:00 2001 From: jager Date: Sat, 18 Mar 2023 15:01:58 -0700 Subject: [PATCH 08/12] removed comments --- src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 16e25c8..a4b84f6 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -198,7 +198,6 @@ public SwerveDriveKinematics getKinematics() { return kinematics; } - // Should I be using this method? public Pose2d getPose() { return robotPose; } From 62c9f1572aa9de9d2b99809aa8da982a1e7a2b71 Mon Sep 17 00:00:00 2001 From: Iron Golem Date: Tue, 21 Mar 2023 19:42:01 -0500 Subject: [PATCH 09/12] XY Axis are relative to alliance refactor code later --- simgui-ds.json | 6 ++++++ .../commands/vision/PlayerStationAutoAlign.java | 15 ++++++++------- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java index ad077a7..989b7f3 100644 --- a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java +++ b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java @@ -7,6 +7,7 @@ import frc.robot.Constants; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.Vision; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -24,11 +25,7 @@ public PlayerStationAutoAlign(DrivetrainSubsystem drivetrainSubsystem, Vision vi // 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() { + public void initialize() { if(vision.getTargetID() == 4){ // Override if Blue Alliance Human Player April Tag is seen drivetrainSubsystem.resetPose(vision.getBotpose()); drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); @@ -36,7 +33,7 @@ public void execute() { drivetrainSubsystem.resetPose(vision.getBotpose()); drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); }else if(DriverStation.getAlliance() == Alliance.Red){ // For Red Alliance - if(drivetrainSubsystem.getPose().getX() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() > Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ + if(drivetrainSubsystem.getPose().getX() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); } }else{ // For Blue Alliance @@ -46,6 +43,10 @@ public void execute() { } } + // 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) {} @@ -53,6 +54,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } From d28e732846cdfdb2e49d50a2c82eb7f57cb99aa1 Mon Sep 17 00:00:00 2001 From: jagernet-ops Date: Wed, 22 Mar 2023 12:21:16 -0700 Subject: [PATCH 10/12] changed to use one Pose2d object --- simgui.json | 3 +++ src/main/java/frc/robot/Constants.java | 4 +--- .../commands/vision/PlayerStationAutoAlign.java | 13 ++++++------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/simgui.json b/simgui.json index a7b56d9..e4d360e 100644 --- a/simgui.json +++ b/simgui.json @@ -26,6 +26,9 @@ "NetworkTables": { "Persistent Values": { "open": false + }, + "Retained Values": { + "open": false } } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0327cbd..4cec44e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,9 +25,7 @@ public final class Constants { public static final double FIELD_WIDTH_METERS = 16; public static final double FIELD_HEIGHT_METERS = 7.85; - // public static final Pose2d UNIFIED_HUMAN_BOUNDARY = new Pose2d(3.7, 5.5, Rotation2d.fromDegrees(180)); - public static final Pose2d RED_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(3.7, 5.5, Rotation2d.fromDegrees(180)); - public static final Pose2d BLUE_ALLIANCE_HUMAN_BOUNDARY = new Pose2d(12.6, 5.5, Rotation2d.fromDegrees(0)); + public static final Pose2d HUMAN_STATION_BOUNDARY = new Pose2d(12.6, 2.5, Rotation2d.fromDegrees(0)); diff --git a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java index 989b7f3..4df0023 100644 --- a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java +++ b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java @@ -7,7 +7,6 @@ import frc.robot.Constants; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.Vision; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -28,17 +27,17 @@ public PlayerStationAutoAlign(DrivetrainSubsystem drivetrainSubsystem, Vision vi public void initialize() { if(vision.getTargetID() == 4){ // Override if Blue Alliance Human Player April Tag is seen drivetrainSubsystem.resetPose(vision.getBotpose()); - drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); + drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation()); }else if(vision.getTargetID() == 5){ // Override if RED Alliance Human Player April Tag is seen drivetrainSubsystem.resetPose(vision.getBotpose()); - drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); + drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation()); }else if(DriverStation.getAlliance() == Alliance.Red){ // For Red Alliance - if(drivetrainSubsystem.getPose().getX() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() < Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getY()){ - drivetrainSubsystem.setRotationTarget(Constants.RED_ALLIANCE_HUMAN_BOUNDARY.getRotation()); + if(drivetrainSubsystem.getPose().getX() > Constants.HUMAN_STATION_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() < Constants.HUMAN_STATION_BOUNDARY.getY()){ + drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation()); } }else{ // For Blue Alliance - if(drivetrainSubsystem.getPose().getX() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() > Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getX()){ - drivetrainSubsystem.setRotationTarget(Constants.BLUE_ALLIANCE_HUMAN_BOUNDARY.getRotation()); + if(drivetrainSubsystem.getPose().getX() > Constants.HUMAN_STATION_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() > Constants.HUMAN_STATION_BOUNDARY.getY()){ + drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation()); } } } From 98073813a8926b5d6b860ef23c7eda13647e2b1e Mon Sep 17 00:00:00 2001 From: "Michael J. Shalewski" Date: Thu, 23 Mar 2023 13:23:22 +0000 Subject: [PATCH 11/12] Commented the XY position code out of the auto align code --- .../frc/robot/commands/vision/PlayerStationAutoAlign.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java index 4df0023..86bfb2c 100644 --- a/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java +++ b/src/main/java/frc/robot/commands/vision/PlayerStationAutoAlign.java @@ -31,7 +31,7 @@ public void initialize() { }else if(vision.getTargetID() == 5){ // Override if RED Alliance Human Player April Tag is seen drivetrainSubsystem.resetPose(vision.getBotpose()); drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation()); - }else if(DriverStation.getAlliance() == Alliance.Red){ // For Red Alliance + }/*else if(DriverStation.getAlliance() == Alliance.Red){ // For Red Alliance if(drivetrainSubsystem.getPose().getX() > Constants.HUMAN_STATION_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() < Constants.HUMAN_STATION_BOUNDARY.getY()){ drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation()); } @@ -39,7 +39,7 @@ public void initialize() { if(drivetrainSubsystem.getPose().getX() > Constants.HUMAN_STATION_BOUNDARY.getX() && drivetrainSubsystem.getPose().getY() > Constants.HUMAN_STATION_BOUNDARY.getY()){ drivetrainSubsystem.setRotationTarget(Constants.HUMAN_STATION_BOUNDARY.getRotation()); } - } + }*/ } // Called every time the scheduler runs while the command is scheduled. From 3620079235ebc8a87778484062a810bbc42fe158 Mon Sep 17 00:00:00 2001 From: Golden Experience Date: Sat, 1 Apr 2023 12:53:05 -0500 Subject: [PATCH 12/12] Player station auto align updates --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4983a39..48f9f21 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -30,7 +30,7 @@ public final class Constants { public static final double FIELD_WIDTH_METERS = 16; public static final double FIELD_HEIGHT_METERS = 7.85; - public static final Pose2d HUMAN_STATION_BOUNDARY = new Pose2d(12.6, 2.5, Rotation2d.fromDegrees(0)); + public static final Pose2d HUMAN_STATION_BOUNDARY = new Pose2d(12.6, 2.5, Rotation2d.fromDegrees(180)); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 1e21e57..2f95800 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -161,7 +161,7 @@ public void resetPose(Pose2d pose) { } odometry.resetPosition( - getGyroscopeRotation(), + getGyroscopeRotation().times(-1), getModulePositions(), pose ); @@ -220,7 +220,7 @@ private Rotation2d getGyroscopeRotation() { return Rotation2d.fromDegrees(navx.getFusedHeading()); } - return Rotation2d.fromDegrees(360.0 - navx.getYaw()); + return navx.getRotation2d(); } public Rotation2d getGyroscopePitch() {