Skip to content

Autonomous #19

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 30 commits into from
Oct 3, 2025
Merged

Autonomous #19

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
30 commits
Select commit Hold shift + click to select a range
430e21f
added auton stuff temporarily. Need the integration to be done to finish
Nummun14 Sep 19, 2025
b49b513
no more craashing
Nummun14 Sep 19, 2025
29d5910
Orangize, add transporter, clean states
Strflightmight09 Sep 19, 2025
b7278fd
Wow
Strflightmight09 Sep 19, 2025
36cce6f
Small stuff
Strflightmight09 Sep 20, 2025
2c2c4f0
Merge branch 'main' into Autonomous
Strflightmight09 Sep 21, 2025
fc5d2aa
Ummmmm
Strflightmight09 Sep 21, 2025
d7fb963
Who did this???
Strflightmight09 Sep 21, 2025
aef696f
@ShmayaR, do the rest 🙏
Strflightmight09 Sep 21, 2025
a87293b
"Theoretically" fine
Strflightmight09 Sep 21, 2025
f112548
Merge branch 'main' into Autonomous
Strflightmight09 Sep 30, 2025
ce5e02c
no erros
Strflightmight09 Sep 30, 2025
269a8aa
crashing is dumb
Strflightmight09 Sep 30, 2025
5c317d0
progress is made
Strflightmight09 Oct 1, 2025
194694d
Update AutonomousCommands.java
Strflightmight09 Oct 1, 2025
732c68b
why it never see coral
Strflightmight09 Oct 1, 2025
be98f79
Update CameraConstants.java
Strflightmight09 Oct 1, 2025
29500df
working on it
Strflightmight09 Oct 1, 2025
afeeddb
Update AutonomousCommands.java
Strflightmight09 Oct 1, 2025
c2afab1
WHY DOES IT EQUAL NULL
Strflightmight09 Oct 1, 2025
e4ca745
temp
Strflightmight09 Oct 1, 2025
2f7bf28
12 coral auto :)
ShmayaR Oct 1, 2025
b53034b
no collect algae during auto
ShmayaR Oct 2, 2025
819293e
no hitting reef
ShmayaR Oct 2, 2025
85fdcb5
made first coral different
ShmayaR Oct 2, 2025
f00fa2f
stuff works better
ShmayaR Oct 3, 2025
cb9f89c
to drives
ShmayaR Oct 3, 2025
1ebfe5b
added camera coordinates
ShmayaR Oct 3, 2025
9e51ad9
Merge branch 'main' into Autonomous
ShmayaR Oct 3, 2025
1668e3f
oops
ShmayaR Oct 3, 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
49 changes: 45 additions & 4 deletions src/main/java/frc/trigon/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,16 +6,18 @@
package frc.trigon.robot;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.trigon.robot.commands.CommandConstants;
import frc.trigon.robot.commands.commandfactories.AutonomousCommands;
import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands;
import frc.trigon.robot.commands.commandfactories.GeneralCommands;
import frc.trigon.robot.constants.AutonomousConstants;
import frc.trigon.robot.constants.CameraConstants;
import frc.trigon.robot.constants.LEDConstants;
import frc.trigon.robot.constants.OperatorConstants;
import frc.trigon.robot.constants.PathPlannerConstants;
import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
Expand All @@ -38,9 +40,11 @@
import lib.utilities.flippable.Flippable;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

import java.util.List;

public class RobotContainer {
public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator();
public static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = new ObjectPoseEstimator(
public static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = new ObjectPoseEstimator(
CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS,
ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION,
SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE,
Expand Down Expand Up @@ -90,7 +94,7 @@ private void bindDefaultCommands() {
private void initializeGeneralSystems() {
Flippable.init();
LEDConstants.init();
PathPlannerConstants.init();
AutonomousConstants.init();
}

private void bindControllerCommands() {
Expand All @@ -109,7 +113,44 @@ private void configureSysIDBindings(MotorSubsystem subsystem) {
subsystem.setDefaultCommand(Commands.idle(subsystem));
}

@SuppressWarnings("All")
private void buildAutoChooser() {
autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser());
autoChooser = new LoggedDashboardChooser<>("AutoChooser");

final List<String> autoNames = AutoBuilder.getAllAutoNames();
boolean hasDefault = false;

for (String autoName : autoNames) {
final PathPlannerAuto autoNonMirrored = new PathPlannerAuto(autoName);
final PathPlannerAuto autoMirrored = new PathPlannerAuto(autoName, true);

if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName)) {
hasDefault = true;
autoChooser.addDefaultOption(autoNonMirrored.getName(), autoNonMirrored);
autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored);
} else if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName + "Mirrored")) {
hasDefault = true;
autoChooser.addDefaultOption(autoMirrored.getName() + "Mirrored", autoMirrored);
autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored);
} else {
autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored);
autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored);
}
}

if (!hasDefault)
autoChooser.addDefaultOption("None", Commands.none());
else
autoChooser.addOption("None", Commands.none());

addCommandsToChooser(
AutonomousCommands.getFloorAutonomousCommand(true),
AutonomousCommands.getFloorAutonomousCommand(false)
);
}

private void addCommandsToChooser(Command... commands) {
for (Command command : commands)
autoChooser.addOption(command.getName(), command);
}
Comment on lines +162 to 171
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🟠 Major

Restore default selection when DEFAULT_AUTO_NAME targets custom commands.

Because hasDefault is finalized before calling addCommandsToChooser(...), any DEFAULT_AUTO_NAME that points at FloorAutonomousRight/Left (or future manual commands) will never mark the chooser’s default. We always fall through to addDefaultOption("None", …), so the operator loses their configured default autonomous. Please roll the default handling into the helper so these commands can satisfy the same selection logic as the PathPlanner autos.

-        addCommandsToChooser(
-                AutonomousCommands.getFloorAutonomousCommand(true),
-                AutonomousCommands.getFloorAutonomousCommand(false)
-        );
+        hasDefault = addCommandsToChooser(
+                hasDefault,
+                AutonomousCommands.getFloorAutonomousCommand(true),
+                AutonomousCommands.getFloorAutonomousCommand(false)
+        );
+
+        if (!hasDefault)
+            autoChooser.addDefaultOption("None", Commands.none());
+        else
+            autoChooser.addOption("None", Commands.none());
-    }
-
-    private void addCommandsToChooser(Command... commands) {
-        for (Command command : commands)
-            autoChooser.addOption(command.getName(), command);
+    }
+
+    private boolean addCommandsToChooser(boolean hasDefault, Command... commands) {
+        boolean hasDefaultAfter = hasDefault;
+        for (Command command : commands) {
+            final boolean matchesDefault = !AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty()
+                    && AutonomousConstants.DEFAULT_AUTO_NAME.equals(command.getName());
+            if (matchesDefault && !hasDefaultAfter) {
+                autoChooser.addDefaultOption(command.getName(), command);
+                hasDefaultAfter = true;
+            } else {
+                autoChooser.addOption(command.getName(), command);
+            }
+        }
+        return hasDefaultAfter;
     }
📝 Committable suggestion

‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.

Suggested change
addCommandsToChooser(
AutonomousCommands.getFloorAutonomousCommand(true),
AutonomousCommands.getFloorAutonomousCommand(false)
);
}
private void addCommandsToChooser(Command... commands) {
for (Command command : commands)
autoChooser.addOption(command.getName(), command);
}
// Register floor-autonomous commands and update hasDefault if one matches DEFAULT_AUTO_NAME
hasDefault = addCommandsToChooser(
hasDefault,
AutonomousCommands.getFloorAutonomousCommand(true),
AutonomousCommands.getFloorAutonomousCommand(false)
);
// Ensure “None” is added appropriately as default or just as another option
if (!hasDefault)
autoChooser.addDefaultOption("None", Commands.none());
else
autoChooser.addOption("None", Commands.none());
}
private boolean addCommandsToChooser(boolean hasDefault, Command... commands) {
boolean hasDefaultAfter = hasDefault;
for (Command command : commands) {
final boolean matchesDefault = !AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty()
&& AutonomousConstants.DEFAULT_AUTO_NAME.equals(command.getName());
if (matchesDefault && !hasDefaultAfter) {
autoChooser.addDefaultOption(command.getName(), command);
hasDefaultAfter = true;
} else {
autoChooser.addOption(command.getName(), command);
}
}
return hasDefaultAfter;
}

}
4 changes: 2 additions & 2 deletions src/main/java/frc/trigon/robot/commands/CommandConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.commands.commandfactories.GeneralCommands;
import frc.trigon.robot.constants.AutonomousConstants;
import frc.trigon.robot.constants.OperatorConstants;
import frc.trigon.robot.constants.PathPlannerConstants;
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
import lib.commands.CameraPositionCalculationCommand;
import lib.commands.WheelRadiusCharacterizationCommand;
Expand Down Expand Up @@ -45,7 +45,7 @@ public class CommandConstants {
() -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
),
WHEEL_RADIUS_CHARACTERIZATION_COMMAND = new WheelRadiusCharacterizationCommand(
PathPlannerConstants.ROBOT_CONFIG.moduleLocations,
AutonomousConstants.ROBOT_CONFIG.moduleLocations,
RobotContainer.SWERVE::getDriveWheelPositionsRadians,
() -> RobotContainer.SWERVE.getHeading().getRadians(),
(omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null),
Expand Down
80 changes: 0 additions & 80 deletions src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java
View file
Open in desktop

This file was deleted.

22 changes: 11 additions & 11 deletions src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,16 +11,16 @@
import frc.trigon.robot.commands.commandfactories.GeneralCommands;
import frc.trigon.robot.constants.OperatorConstants;
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
import org.littletonrobotics.junction.Logger;
import lib.hardware.RobotHardwareStats;
import org.littletonrobotics.junction.Logger;

import java.util.function.Supplier;

/**
* A command class to assist in the intaking of a game piece.
*/
public class IntakeAssistCommand extends ParallelCommandGroup {
private static final ProfiledPIDController
static final ProfiledPIDController
X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)),
Expand All @@ -43,7 +43,7 @@ public IntakeAssistCommand(AssistMode assistMode) {
GeneralCommands.getContinuousConditionalCommand(
GeneralCommands.getFieldRelativeDriveCommand(),
getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece),
() -> RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null
() -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null
)
);
}
Expand All @@ -53,7 +53,7 @@ public IntakeAssistCommand(AssistMode assistMode) {
* This command is for intaking a game piece with a specific robot-relative position.
* To create an intake assist command that selects the closest game piece automatically, use {@link IntakeAssistCommand#IntakeAssistCommand(AssistMode)} instead.
*
* @param assistMode the type of assistance
* @param assistMode the type of assistance
* @param distanceFromTrackedGamePiece the position of the game piece relative to the robot
* @return the command
*/
Expand All @@ -70,21 +70,21 @@ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Tra

private Command getTrackGamePieceCommand() {
return new RunCommand(() -> {
if (RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null)
distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece();
if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null)
distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece();
});
}

private static Translation2d calculateDistanceFromTrackedCGamePiece() {
public static Translation2d calculateDistanceFromTrackedGamePiece() {
final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
final Translation2d trackedObjectPositionOnField = RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot();
final Translation2d trackedObjectPositionOnField = RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot();
if (trackedObjectPositionOnField == null)
return null;

final Translation2d difference = robotPose.getTranslation().minus(trackedObjectPositionOnField);
final Translation2d robotToTrackedGamepieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus());
Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamepieceDistance);
return robotToTrackedGamepieceDistance;
final Translation2d robotToTrackedGamePieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus());
Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamePieceDistance);
return robotToTrackedGamePieceDistance;
}

private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) {
Expand Down
Loading
Loading