Skip to content

Integrate TrigVisionSubsystem with GlobalVisionSubsystem #3

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
nyoravim wants to merge 8 commits into
base: dev
Choose a base branch
from
Open

Integrate TrigVisionSubsystem with GlobalVisionSubsystem #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
0150b5c
integrate trig vision with 'global vision'
nyoravim Mar 15, 2025
d3b9199
fix robot container
nyoravim Mar 15, 2025
ccbda5f
Merge branch 'dev' into consolidate-vision
nyoravim Mar 15, 2025
f542705
full transform
nyoravim Mar 20, 2025
00f244d
minor syntax errors
Mar 20, 2025
f386387
jonas restored a file we didnt need
nyoravim Mar 21, 2025
b6f5d99
merge dev into vision feature branch
nyoravim Mar 21, 2025
f07f2ee
initial testing on auto align
Mar 21, 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
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ public static class TelemetryConstants {
// this will fully disable logging even when FMS is connected.
public static final boolean killswitch = false;
// If true, data won't be sent over network even when not connected to FMS
public static final boolean disableNetworkLogging = true;
public static final boolean disableNetworkLogging = false;
// ONLY ENABLE IN DEV (this *should* be overwritten when connected to FMS, but that's untested)
public static final boolean disableDatalog = false;
// Prefix in NetworkTables, must end with a '/'
Expand Down
134 changes: 78 additions & 56 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 @@ -68,22 +68,22 @@ public final class RobotContainer {
private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();

private final Telemetry logger = new Telemetry();
private final Telemetry logger = new Telemetry();

private final CommandXboxController m_Controller = new CommandXboxController(0);
private final CommandXboxController m_ScuffedController = new CommandXboxController(2);
private final CommandXboxController m_TuningController = new CommandXboxController(4);
private final CommandJoystick m_OperatorController = new CommandJoystick(1);
private final CommandXboxController m_Controller = new CommandXboxController(0);
private final CommandXboxController m_ScuffedController = new CommandXboxController(2);
private final CommandXboxController m_TuningController = new CommandXboxController(4);
private final CommandJoystick m_OperatorController = new CommandJoystick(1);

public final CommandSwerveDrivetrain drivetrain;
public final CommandSwerveDrivetrain drivetrain;

public final BotType botType = RobotDiscoverer.getRobot();

// private SwerveSubsystem m_Swerve;
// private SwerveSubsystem m_Swerve;
private GlobalVisionSubsystem m_GlobalVision;
private TrigVisionSubsystem m_TrigVision;
private LEDSubsystem m_LedSubsystem = new LEDSubsystem();
private ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
private TrigVisionSubsystem m_TrigVision = new TrigVisionSubsystem(m_LedSubsystem);
private ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(botType);
private ClawSubsystemTurbo m_ClawSubsystem = new ClawSubsystemTurbo(botType, m_ElevatorSubsystem);
public PivotSubsystem m_PivotSubsystem = new PivotSubsystem(botType, m_ElevatorSubsystem.elevator);
Expand All @@ -108,15 +108,17 @@ public RobotContainer(Rev2mDistanceSensor distance) {
DataLogManager.start();
DriverStation.startDataLog(DataLogManager.getLog());
}

if (botType == BotType.ALPHA_BOT) {
drivetrain = TunerConstantsAlpha.createDrivetrain();
} else {
drivetrain = TunerConstants.createDrivetrain();
}

sysIdChooser = new SysIdChooser(drivetrain, m_ElevatorSubsystem, m_PivotSubsystem);

initSubsystems();

m_GlobalVision = GlobalVisionSubsystem.configure(drivetrain);
m_TrigVision = new TrigVisionSubsystem(m_LedSubsystem, m_GlobalVision);

switch (botType) {
case MAIN_BOT:
Expand All @@ -134,7 +136,7 @@ public RobotContainer(Rev2mDistanceSensor distance) {
initManualAutos();

SmartDashboard.putData("Auto Chooser", autoChooser);

// SignalLogger.setPath("/ctre-logs/");
SignalLogger.start();
if (RobotBase.isReal())
Expand All @@ -150,32 +152,33 @@ public double getTravelDir() {

return -1;
}

private void initManualAutos() {
autoChooser.setDefaultOption("None", Commands.none());
autoChooser.addOption("Travel", drivetrain.applyRequest(() -> drive.withVelocityY(getTravelDir())).withTimeout(2));
autoChooser.addOption("L1", CoralAutoBuilder.build(AutoType.One, distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision));
autoChooser.addOption("L1 + Intake Algae", CoralAutoBuilder.build(AutoType.OneAndHalf, distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision));
autoChooser.addOption("L1 + Shoot Algae", CoralAutoBuilder.build(AutoType.Two, distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision));

}

private void initSubsystems() {
autoChooser.addOption("L1", CoralAutoBuilder.build(AutoType.One, distance, drivetrain, m_PivotSubsystem,
m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision));
autoChooser.addOption("L1 + Intake Algae", CoralAutoBuilder.build(AutoType.OneAndHalf, distance, drivetrain,
m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision));
autoChooser.addOption("L1 + Shoot Algae", CoralAutoBuilder.build(AutoType.Two, distance, drivetrain,
m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision));
}

private void changeLevel(boolean moveUp) {
if(moveUp)
if (moveUp)
algaeLevel++;
else
algaeLevel--;
if(algaeLevel > 4)

if (algaeLevel > 4)
algaeLevel = 4;
if(algaeLevel < 0)
if (algaeLevel < 0)
algaeLevel = 0;

levelLog.update((double)algaeLevel);
// ParallelCommandGroup parallelLevelCommands = new ParallelCommandGroup(m_PivotSubsystem.goToAngle(algaeLevel), m_ElevatorSubsystem.goToHeight(algaeLevel));
levelLog.update((double) algaeLevel);
// ParallelCommandGroup parallelLevelCommands = new
// ParallelCommandGroup(m_PivotSubsystem.goToAngle(algaeLevel),
// m_ElevatorSubsystem.goToHeight(algaeLevel));
// parallelLevelCommands.schedule();
}

Expand All @@ -189,7 +192,7 @@ public SwerveRequest.FieldCentric getFieldCentricDriveReq() {
if (m_ElevatorSubsystem.getElevatorHeight() > 15) {
multiplier /= 2;
}

if (m_ElevatorSubsystem.getElevatorHeight() > 30) {
multiplier /= 2;
}
Expand All @@ -211,12 +214,12 @@ private void configureSwerveBindings() {
// and Y is defined as to the left according to WPILib convention.
drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically
drivetrain.applyRequest(this::getDriveReq)
);
drivetrain.applyRequest(this::getDriveReq));

// m_Controller.a().whileTrue(drivetrain.applyRequest(() -> brake));
// m_Controller.b().whileTrue(drivetrain.applyRequest(() ->
// point.withModuleDirection(new Rotation2d(-m_Controller.getLeftY(), -m_Controller.getLeftX()))
// point.withModuleDirection(new Rotation2d(-m_Controller.getLeftY(),
// -m_Controller.getLeftX()))
// ));

// // reset the field-centric heading on left bumper press
Expand All @@ -234,8 +237,10 @@ private void configureBindings() {
configureTuningBindings();
configureScuffedBindings();
configureOperatorBindings();

// new Trigger(m_ClawSubsystem.beambreak::get).negate().whileTrue(m_LedSubsystem.solidColor(new Color(0, 155, 255)));

// new
// Trigger(m_ClawSubsystem.beambreak::get).negate().whileTrue(m_LedSubsystem.solidColor(new
// Color(0, 155, 255)));

// m_Controller.rightBumper().whileTrue(m_ClawSubsystem.intakeWithBeambreak());
// m_Controller.rightTrigger().whileTrue(m_PivotSubsystem.setAngle(Rotations.of(0.3)).until(m_PivotSubsystem::closeEnough).andThen(m_ClawSubsystem.shoot()));
Expand All @@ -254,37 +259,46 @@ private void configureBindings() {
m_Controller.leftBumper().whileTrue(m_ClimberSubsystem.runClimberup());

m_Controller.povDown().whileTrue(m_ElevatorSubsystem.autoHonePose().withName("Elevator Hone Command"));

// m_Controller.rightBumper().onTrue(
// (
// (
// scuffedElevator(ElevatorConstantb s.stowHeight).alongWith(scuffedPivot(Rotations.of(0.055), false))
// ).andThen(new WaitCommand(1))
// )
// .until(() -> m_PivotSubsystem.closeEnough())
// .andThen(() -> { m_PivotSubsystem.setDefaultCommand(m_PivotSubsystem.stop());})
// .andThen(m_PivotSubsystem.stop()));
// (
// (
// scuffedElevator(ElevatorConstantb
// s.stowHeight).alongWith(scuffedPivot(Rotations.of(0.055), false))
// ).andThen(new WaitCommand(1))
// )
// .until(() -> m_PivotSubsystem.closeEnough())
// .andThen(() -> {
// m_PivotSubsystem.setDefaultCommand(m_PivotSubsystem.stop());})
// .andThen(m_PivotSubsystem.stop()));
}

public Command scuffedElevator(double rotations) {
return Commands.runOnce(() -> m_ElevatorSubsystem.extendArm(rotations)).andThen(Commands.none().withTimeout(0.05));
}

public void configureScuffedBindings() {
m_ScuffedController.y().onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.bargeAngle)));
m_ScuffedController.a().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.floorAngle)));
m_ScuffedController.x().onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.stowAngle)));
m_ScuffedController.b().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.processorAngle)));
m_ScuffedController.povDown().onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefOneAngle)));
m_ScuffedController.povUp().onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefTwoAngle)));
m_ScuffedController.y().onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.bargeAngle)));
m_ScuffedController.a().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.floorAngle)));
m_ScuffedController.x().onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.stowAngle)));
m_ScuffedController.b().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.processorAngle)));
m_ScuffedController.povDown().onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefOneAngle)));
m_ScuffedController.povUp().onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefTwoAngle)));
// m_ScuffedController.povLeft().onTrue(scuffedElevator(Constants.ElevatorConstants.onCoralHeight).andThen(scuffedPivot(Constants.PivotConstants.onCoralAngle)));

m_ScuffedController.leftTrigger().whileTrue(m_ClawSubsystem.intakeWithBeambreak());
m_ScuffedController.leftBumper().whileTrue(m_ClawSubsystem.intake());
m_ScuffedController.rightTrigger().whileTrue(m_ClawSubsystem.shootWithBeambreak());
m_ScuffedController.rightBumper().whileTrue(m_ClawSubsystem.shoot());

m_ScuffedController.povRight().whileTrue(scuffedElevator(Constants.ElevatorConstants.coralIntake).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.coralIntake)));
m_ScuffedController.povRight().whileTrue(scuffedElevator(Constants.ElevatorConstants.coralIntake)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.coralIntake)));
m_ScuffedController.povLeft().whileTrue(m_ElevatorSubsystem.autoHonePose().withName("Elevator Hone Command"));
}

Expand All @@ -302,12 +316,18 @@ private void configureSimBindings() {
}

private void configureOperatorBindings() {
m_OperatorController.button(7).onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.bargeAngle)));
m_OperatorController.button(5).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.floorAngle)));
m_OperatorController.button(8).onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.stowAngle)));
m_OperatorController.button(2).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.processorAngle)));
m_OperatorController.button(4).onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefOneAngle)));
m_OperatorController.button(1).onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefTwoAngle)));
m_OperatorController.button(7).onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.bargeAngle)));
m_OperatorController.button(5).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.floorAngle)));
m_OperatorController.button(8).onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.stowAngle)));
m_OperatorController.button(2).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.processorAngle)));
m_OperatorController.button(4).onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefOneAngle)));
m_OperatorController.button(1).onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight)
.andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefTwoAngle)));
m_OperatorController.button(6).whileTrue(m_ClawSubsystem.intakeWithBeambreak());
m_OperatorController.button(9).whileTrue(m_ClawSubsystem.intake());
// m_OperatorController.button().whileTrue(m_ClawSubsystem.shootWithBeambreak());
Expand All @@ -324,7 +344,7 @@ private void configureTuningBindings() {

public Command getAutonomousCommand() {
final double sign;

if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) {
sign = -1;
} else {
Expand All @@ -337,7 +357,9 @@ public Command getAutonomousCommand() {
m_PivotSubsystem.reseedEncoder();
}, drivetrain)).andThen(m_ElevatorSubsystem.autoHonePose().asProxy().raceWith(m_ClimberSubsystem.runClimberupAuto()));
// if (Robot.isReal()) {
// base = base.andThen(Commands.run(() -> {}).until(m_PivotSubsystem::closeEnough)); //.andThen(m_ElevatorSubsystem.autoHonePose().asProxy());
// base = base.andThen(Commands.run(() ->
// {}).until(m_PivotSubsystem::closeEnough));
// //.andThen(m_ElevatorSubsystem.autoHonePose().asProxy());
// }
Command cmd = autoChooser.getSelected().asProxy();
cmd.addRequirements(drivetrain);
Expand Down
33 changes: 21 additions & 12 deletions src/main/java/frc/robot/commands/AlignBarge.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,8 @@ public class AlignBarge extends Command {

private final ProfiledPIDController profiledPIDController = new ProfiledPIDController(AutoConstants.Turbo.kTranslationP, AutoConstants.Turbo.kTranslationI, AutoConstants.Turbo.kTranslationD, new TrapezoidProfile.Constraints(AutoConstants.MaxSpeed, AutoConstants.MaxAcceleration));

private final SwerveRequest.FieldCentricFacingAngle driveRequest = new SwerveRequest.FieldCentricFacingAngle().withForwardPerspective(ForwardPerspectiveValue.BlueAlliance);
private final SwerveRequest.FieldCentricFacingAngle driveRequest = new SwerveRequest.FieldCentricFacingAngle()
.withForwardPerspective(ForwardPerspectiveValue.BlueAlliance);

private final UltraDoubleLog logger = new UltraDoubleLog("Auto/Barge Auto Align Pid Output");
private DoublePublisher posePublisher = NetworkTableInstance.getDefault()
Expand All @@ -58,25 +59,34 @@ public AlignBarge(TrigVisionSubsystem vision, CommandSwerveDrivetrain drivetrain

addRequirements(drivetrain);

driveRequest.HeadingController.setPID(AutoConstants.Turbo.kRotationP, AutoConstants.Turbo.kRotationI, AutoConstants.Turbo.kRotationD);
driveRequest.HeadingController.setPID(AutoConstants.Turbo.kRotationP, AutoConstants.Turbo.kRotationI,
AutoConstants.Turbo.kRotationD);
}

private double sign = 1;

@Override
public void execute() {
Optional<Distance> distanceOptional = vision.getLateralDistanceToBarge();
var offset = vision.getRobotToTag();

double pidOutput = 0;
if (!distanceOptional.isEmpty()) {
pidOutput = -profiledPIDController.calculate(distanceOptional.get().in(Meters), AutoConstants.targetDistanceFromBarge.in(Meters));
posePublisher.set(distanceOptional.get().in(Meters));
setPosePublisher.set(AutoConstants.targetDistanceFromBarge.in(Meters));
if (offset.isPresent()) {
var parallelDistance = offset.get().getMeasureX();
double bargeDistance = parallelDistance.in(Meters);
double goalDistance = AutoConstants.targetDistanceFromBarge.in(Meters);

pidOutput = -profiledPIDController.calculate(bargeDistance, goalDistance);

posePublisher.set(bargeDistance);
setPosePublisher.set(goalDistance);
}

logger.update(pidOutput);

Optional<Long> tagID = vision.getTagID();
if (tagID.isPresent()) {
if (tagID.get() == 4 || tagID.get() == 5) {
var tag = vision.getBestTag();
if (tag != null) {
int id = tag.ID;
if (id == 4 || id == 5) {
sign = -1;
} else {
sign = 1;
Expand All @@ -89,7 +99,6 @@ public void execute() {
if (profiledPIDController.atGoal()) {
vision.isAlignedTimestamp = RobotController.getFPGATime();
}


driveRequest.withVelocityX(Meters.of(sign * pidOutput).per(Second));
driveRequest.withVelocityY(horizontalInputSupplier.getAsDouble());
Expand All @@ -102,5 +111,5 @@ public void execute() {
public boolean isFinished() {
return super.isFinished();
}

}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ public class ElevatorSubsystem extends SubsystemBase {
private final UltraSupplierLog rightPosePub = new UltraSupplierLog("Elevator/Right Pose", followerMotor.getPosition());
private final Alert estopAlert = new Alert("Elevator E-Stopped", AlertType.kError);

private boolean estop = false;
private boolean estop = true;

// there will be at least one limit switch and an encoder to track the position of the elevator
public ElevatorSubsystem(BotType bot) {
Expand Down
Loading