Skip to content

Barge Align #105

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ethanli1337 wants to merge 14 commits into
base: main
Choose a base branch
from
Open

Barge Align #105

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
479b08a
Initial Commit
ethanli1337 Mar 26, 2025
be81f2f
net aim command
ethanli1337 Mar 26, 2025
22d71df
Progress
ethanli1337 Mar 26, 2025
d4ecbe3
binds for net aim
ethanli1337 Mar 27, 2025
7bc1510
drive to net command
ethanli1337 Mar 27, 2025
98b5a87
Merge branch 'main' into net-align
ethanli1337 Apr 11, 2025
19f8c22
net aim command finished
ethanli1337 Apr 11, 2025
e7e4257
barge align progress
SpectrumFRC3847 Apr 14, 2025
c114802
BargeAlign works in sim
SpectrumFRC3847 Apr 14, 2025
66b0bb4
Merge branch 'main' into net-align
marcjuguilon Apr 14, 2025
4212a0b
Commit (in progress)
marcjuguilon Apr 14, 2025
2f401b8
resolve other merge changes
SpectrumFRC3847 Apr 15, 2025
46ea200
Barge align fix
ethanli1337 Apr 15, 2025
0c2451a
barge align state
ethanli1337 Apr 15, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 18 additions & 18 deletions simgui-window.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,18 +1,18 @@
{
"Docking": {
"Data": [
"DockNode ID=0x00000001 Pos=34,976 Size=905,316 Selected=0x48913727"
"DockNode ID=0x00000001 Pos=-64,877 Size=905,316 Selected=0x48913727"
]
},
"MainWindow": {
"GLOBAL": {
"font": "Proggy Dotted",
"fps": "120",
"height": "1415",
"maximized": "1",
"height": "1170",
"maximized": "0",
"style": "0",
"userScale": "2",
"width": "2256",
"width": "1924",
"xpos": "0",
"ypos": "29"
}
Expand All @@ -34,7 +34,7 @@
"###/SmartDashboard/Auto Chooser": {
"Collapsed": "0",
"Pos": "339,253",
"Size": "307,60"
"Size": "361,66"
},
"###/SmartDashboard/Field2d": {
"Collapsed": "0",
Expand All @@ -48,7 +48,7 @@
},
"###/SmartDashboard/Scheduler": {
"Collapsed": "0",
"Pos": "920,644",
"Pos": "414,331",
"Size": "372,548"
},
"###/SmartDashboard/States": {
Expand All @@ -74,21 +74,21 @@
"###Addressable LEDs": {
"Collapsed": "0",
"Pos": "585,29",
"Size": "416,220"
"Size": "491,246"
},
"###FMS": {
"Collapsed": "0",
"Pos": "368,30",
"Size": "336,158"
"Pos": "562,299",
"Size": "244,244"
},
"###Joysticks": {
"Collapsed": "0",
"Pos": "6,384",
"Size": "976,252"
"Pos": "87,735",
"Size": "1156,287"
},
"###Keyboard 0 Settings": {
"Collapsed": "0",
"Pos": "910,548",
"Pos": "981,805",
"Size": "300,560"
},
"###Keyboard 1 Settings": {
Expand All @@ -99,13 +99,13 @@
"###NetworkTables": {
"Collapsed": "0",
"DockId": "0x00000001,0",
"Pos": "34,976",
"Pos": "-64,877",
"Size": "905,316"
},
"###NetworkTables Info": {
"Collapsed": "0",
"DockId": "0x00000001,1",
"Pos": "34,976",
"Pos": "-64,877",
"Size": "905,316"
},
"###Other Devices": {
Expand All @@ -126,13 +126,13 @@
},
"###System Joysticks": {
"Collapsed": "0",
"Pos": "689,662",
"Size": "232,254"
"Pos": "721,421",
"Size": "273,290"
},
"###Timing": {
"Collapsed": "0",
"Pos": "5,187",
"Size": "162,194"
"Size": "188,215"
},
"Debug##Default": {
"Collapsed": "0",
Expand All @@ -142,7 +142,7 @@
"Robot State": {
"Collapsed": "0",
"Pos": "193,186",
"Size": "118,134"
"Size": "137,152"
}
}
}
35 changes: 35 additions & 0 deletions src/main/java/frc/reefscape/Field.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,12 @@ public static class Barge {
public static final Translation2d closeCage =
new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947));

public static final Translation2d bargeXBlue =
new Translation2d(Units.inchesToMeters(325.68), Units.inchesToMeters(241.64));
public static final Translation2d bargeXRed =
new Translation2d(
FieldHelpers.flipXifRed(Units.inchesToMeters(325.68)),
FieldHelpers.flipYifRed(Units.inchesToMeters(241.64)));
// Measured from floor to bottom of cage
public static final double deepHeight = Units.inchesToMeters(3.125);
public static final double shallowHeight = Units.inchesToMeters(30.125);
Expand Down Expand Up @@ -97,6 +103,35 @@ public double getCageToClimb() {
return Robot.getSwerve().getRobotPose().getY();
}
}

public static int indexOfSmallest(double[] array) {
int indexOfSmallest = 0;
double smallestIndex = array[indexOfSmallest];
for (int i = 0; i < array.length; i++) {
if (array[i] <= smallestIndex) {
smallestIndex = array[i];
indexOfSmallest = i;
}
}
return indexOfSmallest;
}

// TODO: Add a method to get which side of the barge the robot is on

public static double bargeAlign(Pose2d robotPose) {
double targetX = bargeXBlue.getX();
double oppositeX = bargeXRed.getX();
if (isRed()) {
targetX = bargeXRed.getX();
oppositeX = bargeXBlue.getX();
}
double robotX = robotPose.getX();

if (robotX >= halfLength) {
targetX = Field.fieldLength - oppositeX;
}
return targetX;
}
}

public static class CoralStation {
Expand Down
11 changes: 11 additions & 0 deletions src/main/java/frc/reefscape/Zones.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,17 @@ public class Zones {
public static final Trigger isCloseToReef =
new Trigger(() -> Zones.withinReefRange(reefRangeRadius));

// -------------------------------------------------------------
// Barge Align Helper
// -------------------------------------------------------------

public double getBargeAlignX() {
double bargeAlignX = Field.Barge.bargeAlign(Robot.getSwerve().getRobotPose());

SmartDashboard.putNumber("Barge Align X", bargeAlignX);
return bargeAlignX;
}

// -------------------------------------------------------------
// Reef Offsets Helper
// -------------------------------------------------------------
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/pilot/Pilot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ public class Pilot extends Gamepad {
public final Trigger reefVision_A = A.and(teleop); // , tagsInView); // remove tags in view
public final Trigger reefAlignScore_B = B.and(teleop);
// public final Trigger cageAim_B = B.and(teleop);
public final Trigger bargeAlign_A = A.and(teleop);

// Drive Triggers
public final Trigger upReorient = upDpad.and(fn, teleop);
Expand Down
34 changes: 26 additions & 8 deletions src/main/java/frc/robot/swerve/SwerveStates.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
import frc.reefscape.Zones;
import frc.reefscape.offsets.HomeOffsets;
import frc.robot.Robot;
import frc.robot.RobotStates;
import frc.robot.pilot.Pilot;
import frc.spectrumLib.SpectrumState;
import frc.spectrumLib.Telemetry;
Expand Down Expand Up @@ -77,9 +78,12 @@ protected static void setStates() {

// // vision aim
// pilot.reefAim_A.whileTrue(log(reefAimDrive()));
pilot.reefVision_A.and(RobotStates.netAlgae).whileTrue(log(netAimDrive()));
// pilot.reefAim_A.whileTrue(log(netAimDrive()));
// pilot.reefAim_A.whileTrue(log(reefAimDrive()));
// pilot.reefVision_A.whileTrue(log(reefAimDriveVisionTA()));
pilot.reefVision_A.whileTrue(log(reefAimDriveVisionXY()));
pilot.reefAlignScore_B.whileTrue(log(reefAimDriveVisionXY()));
// pilot.reefVision_A.whileTrue(log(reefAimDriveVisionXY()));
// pilot.reefAlignScore_B.whileTrue(log(reefAimDriveVisionXY()));

// Pose2d backReefOffset = Field.Reef.getOffsetPosition(21, Units.inchesToMeters(24));
// pilot.cageAim_B.whileTrue(
Expand All @@ -88,6 +92,7 @@ protected static void setStates() {
// backReefOffset::getY,
// () -> Math.toRadians(180))); // alignToYDrive(() -> Field.fieldWidth /
// 2));

}

/** Pilot Commands ************************************************************************ */
Expand Down Expand Up @@ -121,13 +126,26 @@ public static Command reefAimDrive() {
.withName("Swerve.reefAimDrive");
}

public static Command netAimDrive() {
return alignToXDrive(() -> zones.getBargeAlignX());
}

public static Command alignToXDrive(DoubleSupplier xGoalMeters) {
return resetXController()
.andThen(
drive(
getAlignToX(xGoalMeters),
pilot::getDriveLeftPositive,
pilot::getDriveCCWPositive));
if (Field.isRed()) {
return resetXController()
.andThen(
drive(
() -> -getAlignToX(xGoalMeters).getAsDouble(),
pilot::getDriveLeftPositive,
pilot::getDriveCCWPositive));
} else {
return resetXController()
.andThen(
drive(
() -> getAlignToX(xGoalMeters).getAsDouble(),
pilot::getDriveLeftPositive,
pilot::getDriveCCWPositive));
}
}

public static Command alignToYDrive(DoubleSupplier yGoalMeters) {
Expand Down