Skip to content
This repository was archived by the owner on Dec 18, 2025. It is now read-only.

Auto benchmark speed #163

Closed
ElliotScher wants to merge 6 commits into from
Closed

Auto benchmark speed #163

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
0a17c48
other speed autos completed with rishi g
AbhiraamVenigalla Nov 25, 2025
02345b7
Merge branch 'feature-v3' into auto-benchmark-speed
ElliotScher Dec 7, 2025
eb3a0ca
Merge branch 'feature-v3' into auto-benchmark-speed
ElliotScher Dec 7, 2025
5e69894
edit auto path and use correct thetaStdev
Aradig26 Dec 8, 2025
311e871
autolog fieldrelativevolocity
Aradig26 Dec 8, 2025
6e784b0
vision subsystem done
ElliotScher Dec 11, 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
4 changes: 2 additions & 2 deletions src/main/deploy/choreo/Choreo.chor
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,8 @@
"val":0.048641
},
"vmax":{
"exp":"5557 RPM",
"val":581.9276791999494
"exp":"6000 RPM",
"val":628.3185307179587

@Aradig26 Aradig26 Dec 15, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

should this still be 6000RPM?

},
"tmax":{
"exp":"0.75 N * m",
Expand Down
251 changes: 251 additions & 0 deletions src/main/deploy/choreo/G_BENCHMARK_SPEED_1.traj
View file
Open in desktop

Large diffs are not rendered by default.

169 changes: 169 additions & 0 deletions src/main/deploy/choreo/G_BENCHMARK_SPEED_2.traj
View file
Open in desktop

Large diffs are not rendered by default.

211 changes: 211 additions & 0 deletions src/main/deploy/choreo/G_BENCHMARK_SPEED_3.traj
View file
Open in desktop

Large diffs are not rendered by default.

61 changes: 61 additions & 0 deletions src/main/java/frc/robot/commands/AutonomousCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,10 @@ public static void loadAutoTrajectories(Drive drive) {
drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_5");
drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_6");
drive.getAutoFactory().cache().loadTrajectory("E_LEFT_PATH_7");

drive.getAutoFactory().cache().loadTrajectory("G_BENCHMARK_SPEED_1");
drive.getAutoFactory().cache().loadTrajectory("G_BENCHMARK_SPEED_2");
drive.getAutoFactory().cache().loadTrajectory("G_BENCHMARK_SPEED_3");
}

public static final Command autoALeft(
Expand Down Expand Up @@ -2167,4 +2171,61 @@ public static final LoggedAutoRoutine autoFLeftMinimal(

return routine;
}

public static final LoggedAutoRoutine speedBenchOne(
Drive drive,
V3_PootSuperstructure superstructure,
V3_PootIntake intake,
V3_PootManipulator manipulator,
Camera... cameras) {

LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("speedBench");
LoggedAutoTrajectory path1 = (routine.trajectory("G_BENCHMARK_SPEED_1"));

routine
.active()
.onTrue(
Commands.sequence(
path1.resetOdometry(), path1.cmd(), Commands.runOnce(() -> drive.stop())));

return routine;
}

public static final LoggedAutoRoutine speedBenchTwo(
Drive drive,
V3_PootSuperstructure superstructure,
V3_PootIntake intake,
V3_PootManipulator manipulator,
Camera... cameras) {

LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("speedBench");
LoggedAutoTrajectory path1 = (routine.trajectory("G_BENCHMARK_SPEED_2"));

routine
.active()
.onTrue(
Commands.sequence(
path1.resetOdometry(), path1.cmd(), Commands.runOnce(() -> drive.stop())));

return routine;
}

public static final LoggedAutoRoutine speedBenchThree(
Drive drive,
V3_PootSuperstructure superstructure,
V3_PootIntake intake,
V3_PootManipulator manipulator,
Camera... cameras) {

LoggedAutoRoutine routine = drive.getAutoFactory().newRoutine("speedBench");
LoggedAutoTrajectory path1 = (routine.trajectory("G_BENCHMARK_SPEED_3"));

routine
.active()
.onTrue(
Commands.sequence(
path1.resetOdometry(), path1.cmd(), Commands.runOnce(() -> drive.stop())));

return routine;
}
}
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/subsystems/shared/drive/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ public class Drive extends SubsystemBase {

private final SwerveDriveKinematics kinematics;
@Getter private Rotation2d rawGyroRotation;
private SwerveModulePosition[] lastModulePositions;
private final SwerveModulePosition[] lastModulePositions;
@Getter private ChassisSpeeds measuredChassisSpeeds;

@Getter private final LoggedAutoFactory autoFactory;
Expand Down Expand Up @@ -118,8 +118,7 @@ public void periodic() {
for (int i = 0; i < 4; i++) {
InternalLoggedTracer.reset();
modules[i].updateInputs();
InternalLoggedTracer.record(
"Module" + Integer.toString(i) + "Update Inputs", "Drive/Periodic");
InternalLoggedTracer.record("Module" + i + "Update Inputs", "Drive/Periodic");
}

InternalLoggedTracer.reset();
Expand All @@ -133,8 +132,7 @@ public void periodic() {
for (int i = 0; i < 4; i++) {
InternalLoggedTracer.reset();
modules[i].periodic();
InternalLoggedTracer.record(
"Module" + Integer.toString(i) + "Periodic Total", "Drive/Periodic");
InternalLoggedTracer.record("Module" + i + "Periodic Total", "Drive/Periodic");
}

// Stop moving when disabled
Expand Down Expand Up @@ -331,6 +329,7 @@ public double getMaxAngularSpeedRadPerSec() {
}

/** Returns the field relative velocity in X and Y. */
@AutoLogOutput(key = "FieldRelativeVelocity")
public Translation2d getFieldRelativeVelocity() {
return new Translation2d(filteredX, filteredY);
}
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/subsystems/shared/vision/CameraIOGompeiVision.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.ConnectionInfo;
import edu.wpi.first.networktables.DoubleArraySubscriber;
import edu.wpi.first.networktables.IntegerSubscriber;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
Expand Down Expand Up @@ -41,7 +42,7 @@ public class CameraIOGompeiVision implements CameraIO {
// private final CameraType cameraType;

private final DoubleArraySubscriber observationSubscriber;
// private final IntegerSubscriber fpsAprilTagSubscriber;
private final IntegerSubscriber fpsAprilTagSubscriber;
private final BooleanSubscriber cameraConnectedSubscriber;

private final Map<Integer, Double> lastTagDetectionTimes;
Expand Down Expand Up @@ -96,7 +97,7 @@ public CameraIOGompeiVision(
PubSubOption.sendAll(true),
PubSubOption.pollStorage(5),
PubSubOption.periodic(0.01));
// this.fpsAprilTagSubscriber = outputTable.getIntegerTopic("fps_apriltags").subscribe(0);
this.fpsAprilTagSubscriber = outputTable.getIntegerTopic("fps_apriltags").subscribe(0);
this.cameraConnectedSubscriber = outputTable.getBooleanTopic("connected").subscribe(false);

lastTagDetectionTimes = new HashMap<>();
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/shared/vision/VisionConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -389,7 +389,7 @@ public static class RobotCameras {
new Camera(new CameraIOLimelight(V2_REDUNDANCY_LEFT)),
new Camera(new CameraIOLimelight(V2_REDUNDANCY_RIGHT))
};
public static final Camera[] V3_POOT_CAMS = {
public static final Camera[] V3_EPSILON_CAMS = {
new Camera(
new CameraIOGompeiVision(
BACK_BOTTOM_LEFT,
Expand Down
4 changes: 2 additions & 2 deletions ...main/java/frc/robot/subsystems/v0_GompeivisionTest/v0_GompeivisionTestRobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ public v0_GompeivisionTestRobotContainer() {
vision =
new Vision(
() -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded),
RobotCameras.V3_POOT_CAMS);
RobotCameras.V3_EPSILON_CAMS);
break;
default:
break;
Expand All @@ -38,7 +38,7 @@ public v0_GompeivisionTestRobotContainer() {
vision =
new Vision(
() -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded),
RobotCameras.V3_POOT_CAMS);

@Aradig26 Aradig26 Dec 15, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

undo these changes

RobotCameras.V3_EPSILON_CAMS);
}

configureButtonBindings();
Expand Down
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/subsystems/v3_Poot/V3_PootRobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,6 @@
import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeConstants.IntakePivotState;
import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeIO;
import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeIOSim;
import frc.robot.subsystems.v3_Poot.superstructure.intake.V3_PootIntakeIOTalonFX;
import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulator;
import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorConstants.ManipulatorRollerState;
import frc.robot.subsystems.v3_Poot.superstructure.manipulator.V3_PootManipulatorIO;
Expand Down Expand Up @@ -82,15 +81,15 @@ public V3_PootRobotContainer() {
new ModuleIOTalonFX(2, DriveConstants.BACK_LEFT),
new ModuleIOTalonFX(3, DriveConstants.BACK_RIGHT));
elevator = new Elevator(new ElevatorIOTalonFX()).getFSM();
intake = new V3_PootIntake(new V3_PootIntakeIOTalonFX());
// intake = new V3_PootIntake(new V3_PootIntakeIOTalonFX());
manipulator = new V3_PootManipulator(new V3_PootManipulatorIOTalonFX());
climber = new V3_PootClimber(new V3_PootClimberIOTalonFX());
superstructure = new V3_PootSuperstructure(elevator, intake, manipulator);
// superstructure = new V3_PootSuperstructure(elevator, intake, manipulator);
leds = new V3_PootLEDs();
vision =
new Vision(
() -> AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded),
RobotCameras.V3_POOT_CAMS);

@Aradig26 Aradig26 Dec 15, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

we should still use V3_POOT_CAMS

RobotCameras.V3_LL_TEST);
break;
case V3_POOT_SIM:
drive =
Expand Down Expand Up @@ -368,6 +367,6 @@ public void robotPeriodic() {
*/
@Override
public Command getAutonomousCommand() {
return autoChooser.selectedCommand();
return AutonomousCommands.speedBenchThree(drive, superstructure, intake, manipulator).cmd();

@Aradig26 Aradig26 Dec 15, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

this change was for benchmarking only

}
}
Loading