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());
}
src/main/java/frc4388/utility/ReefPositionHelper.java
+16 -9
View File
@@ -15,7 +15,7 @@ public class ReefPositionHelper {
RIGHT
}
public static Pose2d[] RED_TAGS = {
public static final Pose2d[] RED_TAGS = {
FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(),
@@ -24,7 +24,7 @@ public class ReefPositionHelper {
FieldConstants.kTagLayout.getTagPose(11).get().toPose2d()
};
public static Pose2d[] BLUE_TAGS = {
public static final Pose2d[] BLUE_TAGS = {
FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(),
FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(),
@@ -34,7 +34,7 @@ public class ReefPositionHelper {
};
public static double distanceTo(Pose2d first, Pose2d second){
return Math.sqrt( Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2));
return Math.sqrt(Math.pow(first.getX() - second.getX(),2) + Math.pow(first.getY() - second.getY(),2));
}
@@ -48,13 +48,16 @@ public class ReefPositionHelper {
double minDistance = distanceTo(position,minPos);
for(int i = 1; i < locations.length; i++){
double dist = distanceTo(locations[i],minPos);
double dist = distanceTo(locations[i],position);
if(dist < minDistance){
System.out.println(i);
minPos = locations[i];
minDistance = dist;
}
}
System.out.println(minPos);
return minPos;
}
@@ -73,16 +76,20 @@ public class ReefPositionHelper {
}
public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) {
return getNearestTag(horisontalOffset(position, side == Side.LEFT ? -(xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)));
return offset(getNearestTag(position),
(side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim,
FieldConstants.VERTICAL_SCORING_POSITION_OFFSET);
}
public static Pose2d horisontalOffset(Pose2d oldPose, double xoffset){
public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
Translation2d oldTranslation = oldPose.getTranslation();
Rotation2d rot = oldPose.getRotation();
double rot = oldPose.getRotation().getRadians();
return new Pose2d(new Translation2d(
oldTranslation.getX() + rot.getCos() * xoffset,
oldTranslation.getY() + rot.getSin() * xoffset
oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
), oldPose.getRotation());
}
}