Skip to content

Auto fixes (￣o￣) . z Z #196

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

Merged
merged 12 commits into from
Oct 25, 2025
Merged

Auto fixes (￣o￣) . z Z #196

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
7880e0e
move auto reset to when disabled
Wu-Fan-529 Oct 21, 2025
4fd4695
Update RobotContainer.java
Wu-Fan-529 Oct 21, 2025
dbea6d1
Update Field.java
Wu-Fan-529 Oct 22, 2025
3b17d05
pushing changes
EliUva Oct 23, 2025
10aa7aa
Merge branch 'auto-fixes' of https://github.com/FRCTeam3255/2025_Offb…
EliUva Oct 23, 2025
752d106
Update _all_paths.chor
EliUva Oct 23, 2025
2dfcfa5
tuned branch poses
BrodyKarr Oct 23, 2025
edd6738
10/32, 18:19
EliUva Oct 24, 2025
307fd23
Update Constants.java
EliUva Oct 24, 2025
f581881
Update Constants.java
EliUva Oct 24, 2025
05f8a99
10/23, 20:20
EliUva Oct 24, 2025
52b8bd6
10/24, 19:58
EliUva Oct 25, 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
6 changes: 6 additions & 0 deletions .DataLogTool/datalogtool.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
{
"download": {
"localDir": "C:\\Users\\Student\\Desktop\\logs",
"serverTeam": "3255"
}
}
28 changes: 14 additions & 14 deletions src/main/deploy/choreo/_all_paths.chor
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -234,22 +234,22 @@
"config":{
"frontLeft":{
"x":{
"exp":"11 in",
"val":0.2794
"exp":"14.5 in",
"val":0.3683
},
"y":{
"exp":"11 in",
"val":0.2794
"exp":"14.5 in",
"val":0.3683
}
},
"backLeft":{
"x":{
"exp":"-11 in",
"val":-0.2794
"exp":"-14.5 in",
"val":-0.3683
},
"y":{
"exp":"11 in",
"val":0.2794
"exp":"14.5 in",
"val":0.3683
}
},
"mass":{
Expand Down Expand Up @@ -282,16 +282,16 @@
},
"bumper":{
"front":{
"exp":"16 in",
"val":0.4064
"exp":"18 in",
"val":0.4572
},
"side":{
"exp":"16 in",
"val":0.4064
"exp":"18 in",
"val":0.4572
},
"back":{
"exp":"16 in",
"val":0.4064
"exp":"18 in",
"val":0.4572
}
},
"differentialTrackWidth":{
Expand Down
18 changes: 9 additions & 9 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ public static class constDrivetrain {
public static final AngularVelocity TURN_SPEED = Units.DegreesPerSecond.of(360);
public static final double SLOW_MODE_MULTIPLIER = 0.5;

public static final boolean INVERT_ROTATION = true;
public static final boolean INVERT_ROTATION = !Robot.isSimulation();

// -- Motor Configurations --
static {
Expand Down Expand Up @@ -496,19 +496,19 @@ public static class constMechanismPositions {
static {

CLEAN_HIGH_FORWARDS.wristAngle = Degrees.of(65.81);
CLEAN_HIGH_FORWARDS.liftHeight = Inches.of(19.975);
CLEAN_HIGH_FORWARDS.liftHeight = Inches.of(20.975);
CLEAN_HIGH_FORWARDS.pivotAngle = Degrees.of(71.04);

CLEAN_LOW_FORWARDS.wristAngle = Degrees.of(65.15);
CLEAN_LOW_FORWARDS.liftHeight = Inches.of(13);
CLEAN_LOW_FORWARDS.liftHeight = Inches.of(14);
CLEAN_LOW_FORWARDS.pivotAngle = Degrees.of(54);

CLEAN_LOW_BACKWARDS.wristAngle = Degrees.of(-98.05);
CLEAN_LOW_BACKWARDS.liftHeight = Inches.of(2);
CLEAN_LOW_BACKWARDS.liftHeight = Inches.of(3);
CLEAN_LOW_BACKWARDS.pivotAngle = Degrees.of(94.88);

CLEAN_HIGH_BACKWARDS.wristAngle = Degrees.of(-99.26);
CLEAN_HIGH_BACKWARDS.liftHeight = Inches.of(22);
CLEAN_HIGH_BACKWARDS.liftHeight = Inches.of(23);
CLEAN_HIGH_BACKWARDS.pivotAngle = Degrees.of(95.16);

INTAKE_CORAL_STATION.wristAngle = Degrees.of(13.9);
Expand Down Expand Up @@ -550,7 +550,7 @@ public static class constMechanismPositions {
PREP_CORAL_L2_FORWARDS.pivotAngle = Degrees.of(45.56);

PREP_CORAL_L3_FORWARDS.wristAngle = Degrees.of(-100);
PREP_CORAL_L3_FORWARDS.liftHeight = Inches.of(19); // TODO: Replace with actual height
PREP_CORAL_L3_FORWARDS.liftHeight = Inches.of(17); // TODO: Replace with actual height
PREP_CORAL_L3_FORWARDS.pivotAngle = Degrees.of(61);

PREP_CORAL_L4_FORWARDS.wristAngle = Degrees.of(-35);
Expand All @@ -565,7 +565,7 @@ public static class constMechanismPositions {
PREP_CORAL_L2_BACKWARDS.wristTolerance = Degrees.of(5); // TODO: tune tolerances

PREP_CORAL_L3_BACKWARDS.wristAngle = Degrees.of(-125);
PREP_CORAL_L3_BACKWARDS.liftHeight = Inches.of(17); // TODO: Replace with actual height
PREP_CORAL_L3_BACKWARDS.liftHeight = Inches.of(18); // TODO: Replace with actual height
PREP_CORAL_L3_BACKWARDS.pivotAngle = Degrees.of(84.33);
PREP_CORAL_L3_BACKWARDS.liftTolerance = Inches.of(5); // TODO: tune tolerances
PREP_CORAL_L3_BACKWARDS.pivotTolerance = Degrees.of(5); // TODO: tune tolerances
Expand Down Expand Up @@ -601,7 +601,7 @@ public static class constMechanismPositions {

PREP_CLIMB.wristAngle = Degrees.of(50); // TODO: Replace with actual angle
PREP_CLIMB.liftHeight = Inches.of(0); // TODO: Replace with actual height
PREP_CLIMB.pivotAngle = Degrees.of(108.4); // TODO: Replace with actual angle
PREP_CLIMB.pivotAngle = Degrees.of(109.4); // TODO: Replace with actual angle

NONE.wristAngle = Degrees.of(0); // TODO: Replace with actual angle
NONE.liftHeight = Inches.of(0); // TODO: Replace with actual height
Expand Down Expand Up @@ -676,7 +676,7 @@ public static class PoseDriveGroup {
public boolean lockX;
public boolean lockY;
public boolean backwardsAllowed;
public Distance distanceTolerance = Units.Inches.of(1);
public Distance distanceTolerance = Units.Inches.of(.5);
public Angle rotationTolerance = Units.Degrees.of(2);
}

Expand Down
91 changes: 68 additions & 23 deletions src/main/java/frc/robot/Field.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
Expand All @@ -29,18 +28,43 @@ public class Field {
private static class FieldElements {
private static final Pose2d REEF_CENTER = new Pose2d(4.5, 4.0, Rotation2d.kZero);
// Branch Poses
private static final Pose2d REEF_A = new Pose2d(3.171, 4.189, Rotation2d.kZero);
private static final Pose2d REEF_B = new Pose2d(3.171, 3.863, Rotation2d.kZero);
private static final Pose2d REEF_C = new Pose2d(3.688, 2.968, Rotation2d.fromDegrees(60));
private static final Pose2d REEF_D = new Pose2d(3.975, 2.803, Rotation2d.fromDegrees(60));
private static final Pose2d REEF_E = new Pose2d(5.001, 2.804, Rotation2d.fromDegrees(120));
private static final Pose2d REEF_F = new Pose2d(5.285, 2.964, Rotation2d.fromDegrees(120));
private static final Pose2d REEF_G = new Pose2d(5.805, 3.863, Rotation2d.k180deg);
private static final Pose2d REEF_H = new Pose2d(5.805, 4.189, Rotation2d.k180deg);
private static final Pose2d REEF_I = new Pose2d(5.288, 5.083, Rotation2d.fromDegrees(-120));
private static final Pose2d REEF_J = new Pose2d(5.002, 5.248, Rotation2d.fromDegrees(-120));
private static final Pose2d REEF_K = new Pose2d(3.972, 5.247, Rotation2d.fromDegrees(-60));
private static final Pose2d REEF_L = new Pose2d(3.693, 5.079, Rotation2d.fromDegrees(-60));
// private static final Pose2d REEF_A = new Pose2d(3.171, 4.189,
// Rotation2d.kZero);
// private static final Pose2d REEF_B = new Pose2d(3.171, 3.863,
// Rotation2d.kZero);
// private static final Pose2d REEF_C = new Pose2d(3.688, 2.968,
// Rotation2d.fromDegrees(60));
// private static final Pose2d REEF_D = new Pose2d(3.975, 2.803,
// Rotation2d.fromDegrees(60));
// private static final Pose2d REEF_E = new Pose2d(5.001, 2.804,
// Rotation2d.fromDegrees(120));
// private static final Pose2d REEF_F = new Pose2d(5.285, 2.964,
// Rotation2d.fromDegrees(120));
// private static final Pose2d REEF_G = new Pose2d(5.805, 3.863,
// Rotation2d.k180deg);
// private static final Pose2d REEF_H = new Pose2d(5.805, 4.189,
// Rotation2d.k180deg);
// private static final Pose2d REEF_I = new Pose2d(5.288, 5.083,
// Rotation2d.fromDegrees(-120));
// private static final Pose2d REEF_J = new Pose2d(5.002, 5.248,
// Rotation2d.fromDegrees(-120));
// private static final Pose2d REEF_K = new Pose2d(3.972, 5.247,
// Rotation2d.fromDegrees(-60));
// private static final Pose2d REEF_L = new Pose2d(3.693, 5.079,
// Rotation2d.fromDegrees(-60));

private static final Pose2d REEF_A = new Pose2d(3.1793227195739746 , 4.190391540527344 , Rotation2d.kZero);
private static final Pose2d REEF_B = new Pose2d(3.1793227195739746 , 3.859665632247925 , Rotation2d.kZero);
private static final Pose2d REEF_C = new Pose2d(3.690809488296509 , 2.9746057987213135 , Rotation2d.fromDegrees(60));
private static final Pose2d REEF_D = new Pose2d(3.9746711254119873 , 2.8099122047424316 , Rotation2d.fromDegrees(60));
private static final Pose2d REEF_E = new Pose2d(5.000613689422607 , 2.8110105991363525 , Rotation2d.fromDegrees(120));
private static final Pose2d REEF_F = new Pose2d(5.2899651527404785 , 2.9782919883728027 , Rotation2d.fromDegrees(120));
private static final Pose2d REEF_G = new Pose2d(5.798591136932373 , 3.857649564743042 , Rotation2d.k180deg);
private static final Pose2d REEF_H = new Pose2d(5.798591136932373 , 4.1899518966674805 , Rotation2d.k180deg);
private static final Pose2d REEF_I = new Pose2d(5.291268825531006 , 5.072429180145264 , Rotation2d.fromDegrees(-120));
private static final Pose2d REEF_J = new Pose2d(5.00312614440918 , 5.239398956298828 , Rotation2d.fromDegrees(-120));
private static final Pose2d REEF_K = new Pose2d(3.9745900630950928 , 5.243215560913086 , Rotation2d.fromDegrees(-60));
private static final Pose2d REEF_L = new Pose2d(3.692172050476074 , 5.0762457847595215 , Rotation2d.fromDegrees(-60));

// L2 Backwards
private static final Pose2d REEF_A_L2_BACKWARDS = new Pose2d(2.848, 4.189, Rotation2d.k180deg);
Expand All @@ -65,18 +89,39 @@ private static class FieldElements {
Rotation2d.fromDegrees(180));

// algae poses
private static final Pose2d ALGAE_AB = REEF_A.interpolate(REEF_B, 0.5);
private static final Pose2d ALGAE_CD = REEF_C.interpolate(REEF_D, 0.5);
private static final Pose2d ALGAE_EF = REEF_E.interpolate(REEF_F, 0.5);
private static final Pose2d ALGAE_GH = REEF_G.interpolate(REEF_H, 0.5);
private static final Pose2d ALGAE_IJ = REEF_I.interpolate(REEF_J, 0.5);
private static final Pose2d ALGAE_KL = REEF_K.interpolate(REEF_L, 0.5);
private static final Pose2d ALGAE_A = new Pose2d(3.171, 4.189, Rotation2d.kZero);
private static final Pose2d ALGAE_B = new Pose2d(3.171, 3.863, Rotation2d.kZero);
private static final Pose2d ALGAE_C = new Pose2d(3.688, 2.968, Rotation2d.fromDegrees(60));
private static final Pose2d ALGAE_D = new Pose2d(3.975, 2.803, Rotation2d.fromDegrees(60));
private static final Pose2d ALGAE_E = new Pose2d(5.001, 2.804, Rotation2d.fromDegrees(120));
private static final Pose2d ALGAE_F = new Pose2d(5.285, 2.964, Rotation2d.fromDegrees(120));
private static final Pose2d ALGAE_G = new Pose2d(5.805, 3.863, Rotation2d.k180deg);
private static final Pose2d ALGAE_H = new Pose2d(5.805, 4.189, Rotation2d.k180deg);
private static final Pose2d ALGAE_I = new Pose2d(5.288, 5.083, Rotation2d.fromDegrees(-120));
private static final Pose2d ALGAE_J = new Pose2d(5.002, 5.248, Rotation2d.fromDegrees(-120));
private static final Pose2d ALGAE_K = new Pose2d(3.972, 5.247, Rotation2d.fromDegrees(-60));
private static final Pose2d ALGAE_L = new Pose2d(3.693, 5.079, Rotation2d.fromDegrees(-60));

private static final Pose2d ALGAE_AB = ALGAE_A.interpolate(ALGAE_B, 0.5);
private static final Pose2d ALGAE_CD = ALGAE_C.interpolate(ALGAE_D, 0.5);
private static final Pose2d ALGAE_EF = ALGAE_E.interpolate(ALGAE_F, 0.5);
private static final Pose2d ALGAE_GH = ALGAE_G.interpolate(ALGAE_H, 0.5);
private static final Pose2d ALGAE_IJ = ALGAE_I.interpolate(ALGAE_J, 0.5);
private static final Pose2d ALGAE_KL = ALGAE_K.interpolate(ALGAE_L, 0.5);

// private static final Pose2d ALGAE_AB = REEF_A.interpolate(REEF_B, 0.5);
// private static final Pose2d ALGAE_CD = REEF_C.interpolate(REEF_D, 0.5);
// private static final Pose2d ALGAE_EF = REEF_E.interpolate(REEF_F, 0.5);
// private static final Pose2d ALGAE_GH = REEF_G.interpolate(REEF_H, 0.5);
// private static final Pose2d ALGAE_IJ = REEF_I.interpolate(REEF_J, 0.5);
// private static final Pose2d ALGAE_KL = REEF_K.interpolate(REEF_L, 0.5);

// Coral Station
private static final Pose2d LEFT_CORAL_STATION_FAR = new Pose2d(1.64, 7.33, Rotation2d.fromDegrees(125));
private static final Pose2d LEFT_CORAL_STATION_NEAR = new Pose2d(0.71, 6.68, Rotation2d.fromDegrees(125));
private static final Pose2d RIGHT_CORAL_STATION_FAR = new Pose2d(1.61, 0.70, Rotation2d.fromDegrees(-125));
private static final Pose2d RIGHT_CORAL_STATION_NEAR = new Pose2d(0.64, 1.37, Rotation2d.fromDegrees(-125));
private static final Pose2d LEFT_CORAL_STATION_FAR = new Pose2d(1.5795878171920776 , 7.378190517425537 ,
Rotation2d.fromDegrees(125));
private static final Pose2d LEFT_CORAL_STATION_NEAR = new Pose2d(0.7105790972709656 , 6.752768039703369 , Rotation2d.fromDegrees(125));
private static final Pose2d RIGHT_CORAL_STATION_FAR = new Pose2d(1.545912265777588 , 0.6680838465690613 , Rotation2d.fromDegrees(-125));
private static final Pose2d RIGHT_CORAL_STATION_NEAR = new Pose2d(0.6772652864456177 , 1.297790765762329 , Rotation2d.fromDegrees(-125));

// Processor
private static final Pose2d PROCESSOR = new Pose2d(5.6, 0.896, Rotation2d.fromDegrees(-90));
Expand Down
52 changes: 37 additions & 15 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
Expand Down Expand Up @@ -45,7 +46,7 @@
public class RobotContainer {

@NotLogged
AutoChooser autoChooser = new AutoChooser();
SendableChooser<Command> autoChooser = new SendableChooser<>();

private AutoFactory autoFactory;

Expand Down Expand Up @@ -244,6 +245,18 @@ public RobotContainer() {
configAutos();
configFeedback();

autoChooser.onChange(selectedAuto -> {
String pose = "default_pose"; // Initialize with a default value
if (selectedAuto == nonProcSide4Coral) {
pose = "top_ji";
} else if (selectedAuto == procSide4Coral) {
pose = "proc_ef";
} else if (selectedAuto == mid1Coral || selectedAuto == midAlgae) {
pose = "mid_gh";
}
autoFactory.resetOdometry(pose).ignoringDisable(true).schedule();
});

subDrivetrain.resetModulesToAbsolute();
}

Expand All @@ -257,7 +270,6 @@ public void configAutos() {
);

nonProcSide4Coral = Commands.sequence(
autoFactory.resetOdometry("top_ji").asProxy(),
ScoreAndCollect("top_ji", "ji_cs", REEF_AUTO_DRIVING_RIGHT,
TRY_PREP_CORAL_L4),
ScoreAndCollect("cs_lk", "lk_cs", REEF_AUTO_DRIVING_RIGHT,
Expand All @@ -267,7 +279,6 @@ public void configAutos() {
TRY_PREP_CORAL_L4));

procSide4Coral = Commands.sequence(
autoFactory.resetOdometry("proc_ef").asProxy(),
ScoreAndCollect("proc_ef", "ef_cs", REEF_AUTO_DRIVING_RIGHT,
TRY_PREP_CORAL_L4),
ScoreAndCollect("cs_cd", "cd_cs", REEF_AUTO_DRIVING_RIGHT,
Expand All @@ -277,21 +288,19 @@ public void configAutos() {
TRY_PREP_CORAL_L4));

mid1Coral = Commands.sequence(
autoFactory.resetOdometry("mid_gh").asProxy(),
Score("mid_gh", REEF_AUTO_DRIVING_LEFT, TRY_PREP_CORAL_L4));

midAlgae = Commands.sequence(
autoFactory.resetOdometry("mid_gh").asProxy(),
Score("mid_gh", REEF_AUTO_DRIVING_LEFT, TRY_PREP_CORAL_L4),
CleanAndScore("gh", "gh_net", TRY_CLEAN_LOW),
FirstCleanAndScore("gh_net", TRY_CLEAN_LOW),
CleanAndScore("net_ji", "ji_net", TRY_CLEAN_HIGH),
CleanAndScore("net_ef", "ef_net", TRY_CLEAN_HIGH),
runPath("net_off_startingline").asProxy()); // FORGOT TO DO AS PROXY ON RUNPATH

autoChooser.addCmd("4 Coral - Processor Side", () -> procSide4Coral);
autoChooser.addCmd("4 Coral - Non-Processor Side", () -> nonProcSide4Coral);
autoChooser.addCmd("1 Coral - Mid", () -> mid1Coral);
autoChooser.addCmd("1 Algae - Mid", () -> midAlgae);
autoChooser.addOption("4 Coral - Non-Processor Side", nonProcSide4Coral);
autoChooser.addOption("4 Coral - Processor Side", procSide4Coral);
autoChooser.addOption("1 Coral - Mid", mid1Coral);
autoChooser.addOption("3 Algae - Mid", midAlgae);

SmartDashboard.putData("AutoChooser", autoChooser);
}
Expand Down Expand Up @@ -332,14 +341,28 @@ Command CleanAndScore(String startPath, String endPath, Command try_clean_lv) {
NET_AUTO_DRIVING.asProxy().alongWith(
Commands.waitSeconds(0.3).andThen(
TRY_PREP_ALGAE_NET.asProxy()))
.withTimeout(2),
TRY_SCORING_ALGAE.asProxy().withTimeout(0.75),
.withTimeout(1.5),
TRY_SCORING_ALGAE.asProxy().withTimeout(0.5),
TRY_NONE.asProxy().withTimeout(0.05));
}

Command FirstCleanAndScore(String endPath, Command try_clean_lv) {
return Commands.sequence(
ALGAE_AUTO_DRIVING.asProxy().withTimeout(0.7).andThen(
ALGAE_AUTO_DRIVING.asProxy().withDeadline(
try_clean_lv.asProxy().withTimeout(4))),
Commands.runOnce(() -> subStateMachine.setRobotState(RobotState.HAS_ALGAE)).asProxy(),
runPath(endPath).asProxy(),
NET_AUTO_DRIVING.asProxy().alongWith(
Commands.waitSeconds(0.3).andThen(
TRY_PREP_ALGAE_NET.asProxy()))
.withTimeout(1.5),
TRY_SCORING_ALGAE.asProxy().withTimeout(0.5),
TRY_NONE.asProxy().withTimeout(0.05));
}

Command runPath(String pathName) {
return autoFactory.trajectoryCmd(pathName)
return autoFactory.trajectoryCmd(pathName).asProxy()
.alongWith(Commands.runOnce(() -> subDriverStateMachine.setDriverState(DriverState.CHOREO)));
}

Expand Down Expand Up @@ -417,10 +440,9 @@ private void configDriverBindings() {
}

public Command getAutonomousCommand() {
return autoChooser.selectedCommand();
return autoChooser.getSelected();

}

private void configOperatorBindings() {
// Add operator bindings here if needed
conOperator.btn_LeftTrigger
Expand Down