Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add trims and get reef position

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-16 19:41:05 -07:00
parent 56f3ccf0c5
commit 9db9d25f5b
6 changed files with 64 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -3
View File
@@ -52,6 +52,7 @@ import edu.wpi.first.units.measure.Distance;
import frc4388.utility.CanDevice;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.Trim;
/**
@@ -103,7 +104,7 @@ public final class Constants {
public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
// public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10);
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
private static final class ModuleSpecificConstants {
//Front Left
@@ -199,11 +200,14 @@ public final class Constants {
public static final Gains XY_GAINS = new Gains(3,0,0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
public static final double XY_TOLERANCE = 0.05;
public static final double ROT_TOLERANCE = 1;
public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
// public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
// public static final Pose2d targetpos =
}
@@ -311,7 +315,9 @@ public final class Constants {
public static final class FieldConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2025Reefscape.loadAprilTagLayoutField();
public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(13);
public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);
// Test april tag field layout
src/main/java/frc4388/robot/RobotContainer.java
+13 -2
View File
@@ -42,7 +42,9 @@ import frc4388.robot.subsystems.SwerveDrive;
// Utilites
import frc4388.utility.DeferredBlock;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.Subsystem;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.configurable.ConfigurableString;
/**
@@ -165,6 +167,15 @@ public class RobotContainer {
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
// ! /* Speed */
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
@@ -181,7 +192,7 @@ public class RobotContainer {
// ? /* Operator Buttons */
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos));
.onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision));
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
@@ -237,7 +248,7 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
//return autoPlayback;
return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
}
/**
src/main/java/frc4388/robot/commands/GotoPositionCommand.java
+4 -2
View File
@@ -9,6 +9,8 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
import frc4388.utility.ReefPositionHelper;
import frc4388.utility.ReefPositionHelper.Side;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.VirtualController;
@@ -32,10 +34,9 @@ public class GotoPositionCommand extends Command {
* @param SwerveDrive m_robotSwerveDrive
*/
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) {
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.targetpos = targetpos;
addRequirements(swerveDrive);
}
@@ -43,6 +44,7 @@ public class GotoPositionCommand extends Command {
public void initialize() {
xPID.initialize();
yPID.initialize();
this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get());
}
double xerr;
src/main/java/frc4388/robot/subsystems/Vision.java
+20 -11
View File
@@ -40,7 +40,8 @@ public class Vision extends Subsystem {
private PhotonCamera camera;
private boolean isTag = false;
private boolean isTagDetected = false;
private boolean isTagProcessed = false;
private Pose2d lastVisionPose = new Pose2d();
private Pose2d lastPhysOdomPose = new Pose2d();
@@ -54,10 +55,15 @@ public class Vision extends Subsystem {
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbTag = subsystemLayout
GenericEntry sbTagDetected = subsystemLayout
.add("Tag Detected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbTagProcessed = subsystemLayout
.add("Tag Processed", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbCamConnected = subsystemLayout
.add("Camera Connnected", false)
@@ -78,7 +84,8 @@ public class Vision extends Subsystem {
// var results = camera.getAllUnreadResults();
// if (results.size() == 0) return;
// var result = results.get(results.size()-1);
isTag = result.hasTargets();
isTagDetected = result.hasTargets();
isTagProcessed = false;
// Optional<MultiTargetPNPResult> multitag = result.getMultiTagResult();
@@ -94,8 +101,8 @@ public class Vision extends Subsystem {
// System.out.println(isTag);
if(!isTag){
sbTag.setBoolean(isTag);
if(!isTagDetected){
// sbTagDetected.setBoolean(isTagDetected);
field.setRobotPose(getPose2d());
return;
}
@@ -104,12 +111,13 @@ public class Vision extends Subsystem {
// In case the pose estimator fails to estimate the pose, fallback to physical odometry.
if(EstimatedRobotPose.isEmpty()){
isTag = false;
sbTag.setBoolean(isTag);
field.setRobotPose(getPose2d());
// sbTagProcessed.setBoolean(isTagProcessed);
field.setRobotPose(lastVisionPose);
return;
}
isTagProcessed = true;
lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
// lastVisionPose.rotateBy(Rotation2d.k180deg);
// lastVisionPose = new Pose2d(
@@ -234,7 +242,7 @@ public class Vision extends Subsystem {
}
public Pose2d getPose2d() {
if(isTag)
if(isTagDetected && isTagProcessed)
return lastVisionPose;
else
return lastPhysOdomPose;
@@ -245,7 +253,7 @@ public class Vision extends Subsystem {
}
public boolean isTag(){
return isTag;
return isTagDetected && isTagProcessed;
}
@@ -271,7 +279,8 @@ public class Vision extends Subsystem {
@Override
public void queryStatus() {
sbTag.setBoolean(isTag);
sbTagDetected.setBoolean(isTagDetected);
sbTagProcessed.setBoolean(isTagProcessed);
sbCamConnected.setBoolean(camera.isConnected());
// field.setRobotPose(getPose2d());
}