Skip to content

Commit

Permalink
aimAtPoint method
Browse files Browse the repository at this point in the history
  • Loading branch information
@viggy96
viggy96 committed Mar 23, 2024
1 parent c26c0ac commit 9be44c6
Show file tree
Hide file tree
Showing 3 changed files with 142 additions and 110 deletions.
227 changes: 119 additions & 108 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@

import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.DriverStation;
Expand Down Expand Up @@ -46,20 +47,20 @@ public class RobotContainer {
Constants.Drive.DRIVE_LOOKAHEAD
);

private static final ShooterSubsystem SHOOTER_SUBSYSTEM = new ShooterSubsystem(
ShooterSubsystem.initializeHardware(),
Constants.Shooter.FLYWHEEL_CONFIG,
Constants.Shooter.ANGLE_CONFIG,
Constants.Shooter.ANGLE_MOTION_CONSTRAINT,
Constants.Shooter.FLYWHEEL_DIAMETER,
Constants.Shooter.SHOOTER_MAP,
DRIVE_SUBSYSTEM::getPose,
() -> speakerSupplier()
);
private static final IntakeSubsystem INTAKE_SUBSYSTEM = new IntakeSubsystem(
IntakeSubsystem.initializeHardware(),
Constants.Intake.ROLLER_VELOCITY
);
// private static final ShooterSubsystem SHOOTER_SUBSYSTEM = new ShooterSubsystem(
// ShooterSubsystem.initializeHardware(),
// Constants.Shooter.FLYWHEEL_CONFIG,
// Constants.Shooter.ANGLE_CONFIG,
// Constants.Shooter.ANGLE_MOTION_CONSTRAINT,
// Constants.Shooter.FLYWHEEL_DIAMETER,
// Constants.Shooter.SHOOTER_MAP,
// DRIVE_SUBSYSTEM::getPose,
// () -> speakerSupplier()
// );
// private static final IntakeSubsystem INTAKE_SUBSYSTEM = new IntakeSubsystem(
// IntakeSubsystem.initializeHardware(),
// Constants.Intake.ROLLER_VELOCITY
// );
private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance();

private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT);
Expand All @@ -82,13 +83,13 @@ public RobotContainer() {
DRIVE_SUBSYSTEM.configureAutoBuilder();

// Register Named Commands
NamedCommands.registerCommand(Constants.NamedCommands.INTAKE_COMMAND_NAME, autoIntakeCommand().withTimeout(7));
NamedCommands.registerCommand(Constants.NamedCommands.PRELOAD_COMMAND_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(1.2));
NamedCommands.registerCommand(Constants.NamedCommands.SHOOT_COMMAND_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(0.7));
NamedCommands.registerCommand(Constants.NamedCommands.SPINUP_COMMAND_NAME, SHOOTER_SUBSYSTEM.spinupCommand());
// NamedCommands.registerCommand(Constants.NamedCommands.INTAKE_COMMAND_NAME, autoIntakeCommand().withTimeout(7));
// NamedCommands.registerCommand(Constants.NamedCommands.PRELOAD_COMMAND_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(1.2));
// NamedCommands.registerCommand(Constants.NamedCommands.SHOOT_COMMAND_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(0.7));
// NamedCommands.registerCommand(Constants.NamedCommands.SPINUP_COMMAND_NAME, SHOOTER_SUBSYSTEM.spinupCommand());

NamedCommands.registerCommand(Constants.NamedCommands.FEEDTHROUGH_COMMAND_NAME, feedThroughCommand().withTimeout(2));
NamedCommands.registerCommand(Constants.NamedCommands.AUTO_SHOOT_COMMAND_NAME, shootCommand().withTimeout(1));
// NamedCommands.registerCommand(Constants.NamedCommands.FEEDTHROUGH_COMMAND_NAME, feedThroughCommand().withTimeout(2));
// NamedCommands.registerCommand(Constants.NamedCommands.AUTO_SHOOT_COMMAND_NAME, shootCommand().withTimeout(1));

VISION_SUBSYSTEM.setPoseSupplier(() -> DRIVE_SUBSYSTEM.getPose());

Expand All @@ -108,37 +109,38 @@ private void configureBindings() {
PRIMARY_CONTROLLER.back().onTrue(DRIVE_SUBSYSTEM.toggleCentriciyCommand());

PRIMARY_CONTROLLER.leftBumper().whileTrue(aimAtObject());
PRIMARY_CONTROLLER.rightBumper().whileTrue(aimAndIntakeObjectCommand());

// Right trigger button - aim and shoot at speaker, shooting only if speaker tag is visible and robot is in range
// Click right stick to override and shoot now
PRIMARY_CONTROLLER.rightTrigger().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.b().getAsBoolean()));
// PRIMARY_CONTROLLER.rightTrigger().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.b().getAsBoolean()));

// Right bumper button - amp score, also use for outtake
PRIMARY_CONTROLLER.rightBumper().whileTrue(SHOOTER_SUBSYSTEM.scoreAmpCommand());
// PRIMARY_CONTROLLER.rightBumper().whileTrue(SHOOTER_SUBSYSTEM.scoreAmpCommand());

// Left trigger button - intake game piece
PRIMARY_CONTROLLER.leftTrigger().whileTrue(intakeCommand());
// PRIMARY_CONTROLLER.leftTrigger().whileTrue(intakeCommand());

// Left bumper button - intake game piece from source
PRIMARY_CONTROLLER.leftBumper().whileTrue(sourceIntakeCommand());
// PRIMARY_CONTROLLER.leftBumper().whileTrue(sourceIntakeCommand());

// A button - go to amp and score
PRIMARY_CONTROLLER.a().whileTrue(
DRIVE_SUBSYSTEM.goToPoseCommand(
Constants.Field.AMP,
SHOOTER_SUBSYSTEM.prepareForAmpCommand(),
SHOOTER_SUBSYSTEM.scoreAmpCommand()
)
);
// PRIMARY_CONTROLLER.a().whileTrue(
// DRIVE_SUBSYSTEM.goToPoseCommand(
// Constants.Field.AMP,
// SHOOTER_SUBSYSTEM.prepareForAmpCommand(),
// SHOOTER_SUBSYSTEM.scoreAmpCommand()
// )
// );

// B button - go to source and intake game piece
PRIMARY_CONTROLLER.b().whileTrue(
DRIVE_SUBSYSTEM.goToPoseCommand(
Constants.Field.SOURCE,
SHOOTER_SUBSYSTEM.sourceIntakeCommand(),
SHOOTER_SUBSYSTEM.sourceIntakeCommand()
)
);
// PRIMARY_CONTROLLER.b().whileTrue(
// DRIVE_SUBSYSTEM.goToPoseCommand(
// Constants.Field.SOURCE,
// SHOOTER_SUBSYSTEM.sourceIntakeCommand(),
// SHOOTER_SUBSYSTEM.sourceIntakeCommand()
// )
// );
//B
// PRIMARY_CONTROLLER.b().whileTrue(aimAndIntakeObjectCommand());

Expand All @@ -151,13 +153,14 @@ private void configureBindings() {
);

// X button - shoot note into speaker from against the subwoofer
PRIMARY_CONTROLLER.x().whileTrue(SHOOTER_SUBSYSTEM.shootSpeakerCommand());
// PRIMARY_CONTROLLER.x().whileTrue(SHOOTER_SUBSYSTEM.shootSpeakerCommand());

// Y button - aim and shoot at speaker, regardless if shooting if speaker tag is visible
PRIMARY_CONTROLLER.y().whileTrue(outtakeCommand());
// PRIMARY_CONTROLLER.y().whileTrue(outtakeCommand());

PRIMARY_CONTROLLER.povUp().whileTrue(SHOOTER_SUBSYSTEM.shootManualCommand(() -> dashboardStateSupplier()));
PRIMARY_CONTROLLER.povRight().whileTrue(feedThroughCommand());
// PRIMARY_CONTROLLER.povUp().whileTrue(SHOOTER_SUBSYSTEM.shootManualCommand(() -> dashboardStateSupplier()));
// PRIMARY_CONTROLLER.povRight().whileTrue(feedThroughCommand());
PRIMARY_CONTROLLER.povLeft().onTrue(DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));
}

/**
Expand All @@ -169,101 +172,101 @@ private void configureBindings() {
* </ul>
* @return Command that will automatically make the controller rumble based on the above conditions
*/
private Command rumbleCommand() {
return Commands.run(() -> {
if (VISION_SUBSYSTEM.getObjectLocation().isPresent())
PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kLeftRumble, 1.0);
else PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kLeftRumble, 0.0);
if (SHOOTER_SUBSYSTEM.isObjectPresent())
PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kRightRumble, 1.0);
else PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kRightRumble, 0.0);
}).finallyDo(() -> PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kBothRumble, 0.0));
}
// private Command rumbleCommand() {
// return Commands.run(() -> {
// if (VISION_SUBSYSTEM.getObjectLocation().isPresent())
// PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kLeftRumble, 1.0);
// else PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kLeftRumble, 0.0);
// if (SHOOTER_SUBSYSTEM.isObjectPresent())
// PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kRightRumble, 1.0);
// else PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kRightRumble, 0.0);
// }).finallyDo(() -> PRIMARY_CONTROLLER.getHID().setRumble(RumbleType.kBothRumble, 0.0));
// }

/**
* Compose command to intake a note and rumble controller appropriately
* @return Command that will automatically intake a note and prepare it for feeding inside the shooter motor
*/
private Command intakeCommand() {
return Commands.parallel(
rumbleCommand(),
INTAKE_SUBSYSTEM.intakeCommand(),
SHOOTER_SUBSYSTEM.intakeCommand()
);
}
// private Command intakeCommand() {
// return Commands.parallel(
// rumbleCommand(),
// INTAKE_SUBSYSTEM.intakeCommand(),
// SHOOTER_SUBSYSTEM.intakeCommand()
// );
// }

/**
* Compose command to intake a note via the source and rumble controller appropriately
* @return Command that will automatically intake a note from source via the shooter flywheels
*/
private Command sourceIntakeCommand() {
return Commands.parallel(
rumbleCommand(),
SHOOTER_SUBSYSTEM.sourceIntakeCommand()
);
}
// private Command sourceIntakeCommand() {
// return Commands.parallel(
// rumbleCommand(),
// SHOOTER_SUBSYSTEM.sourceIntakeCommand()
// );
// }

/**
* Intake until an object is present in autonomous
* @return Command to intake until an object is present
*/
private Command autoIntakeCommand() {
return Commands.parallel(
INTAKE_SUBSYSTEM.intakeCommand(),
SHOOTER_SUBSYSTEM.intakeCommand()
).until(() -> SHOOTER_SUBSYSTEM.isObjectPresent());
}
// private Command autoIntakeCommand() {
// return Commands.parallel(
// INTAKE_SUBSYSTEM.intakeCommand(),
// SHOOTER_SUBSYSTEM.intakeCommand()
// ).until(() -> SHOOTER_SUBSYSTEM.isObjectPresent());
// }

/**
* Compose command to outtake a note
* @return Command that will spit out a note from ground intake
*/
private Command outtakeCommand() {
return Commands.parallel(
rumbleCommand(),
INTAKE_SUBSYSTEM.outtakeCommand(),
SHOOTER_SUBSYSTEM.outtakeCommand()
);
}
// private Command outtakeCommand() {
// return Commands.parallel(
// rumbleCommand(),
// INTAKE_SUBSYSTEM.outtakeCommand(),
// SHOOTER_SUBSYSTEM.outtakeCommand()
// );
// }

/**
* Compose command to shoot note
* @param override Shoot even if target tag is not visible
* @return Command that will automatically aim and shoot note
*/
private Command shootCommand(BooleanSupplier override) {
return Commands.parallel(
DRIVE_SUBSYSTEM.aimAtPointCommand(
() -> PRIMARY_CONTROLLER.getLeftY(),
() -> PRIMARY_CONTROLLER.getLeftX(),
() -> PRIMARY_CONTROLLER.getRightX(),
() -> speakerSupplier().pose.getTranslation().toTranslation2d(),
true,
true
),
SHOOTER_SUBSYSTEM.shootCommand(() -> DRIVE_SUBSYSTEM.isAimed(), override)
);
}
// private Command shootCommand(BooleanSupplier override) {
// return Commands.parallel(
// DRIVE_SUBSYSTEM.aimAtPointCommand(
// () -> PRIMARY_CONTROLLER.getLeftY(),
// () -> PRIMARY_CONTROLLER.getLeftX(),
// () -> PRIMARY_CONTROLLER.getRightX(),
// () -> speakerSupplier().pose.getTranslation().toTranslation2d(),
// true,
// true
// ),
// SHOOTER_SUBSYSTEM.shootCommand(() -> DRIVE_SUBSYSTEM.isAimed(), override)
// );
// }

/**
* Compose command to shoot note, checking if tag is visible and robot is in range
* @return Command that will automatically aim and shoot note
*/
private Command shootCommand() {
return shootCommand(() -> false);
}
// private Command shootCommand() {
// return shootCommand(() -> false);
// }

/**
* Compose command to feed a note through the robot
* @return Command to feed note through the robot
*/
private Command feedThroughCommand() {
return Commands.parallel(
rumbleCommand(),
INTAKE_SUBSYSTEM.intakeCommand(),
SHOOTER_SUBSYSTEM.feedThroughCommand(() -> DRIVE_SUBSYSTEM.isAimed())
);
}
// private Command feedThroughCommand() {
// return Commands.parallel(
// rumbleCommand(),
// INTAKE_SUBSYSTEM.intakeCommand(),
// SHOOTER_SUBSYSTEM.feedThroughCommand(() -> DRIVE_SUBSYSTEM.isAimed())
// );
// }

/**
* Command to aim at detected game object automatically, driving normally if none is detected
Expand Down Expand Up @@ -295,16 +298,24 @@ private Command aimAndIntakeObjectCommand() {
() -> PRIMARY_CONTROLLER.getLeftX(),
() -> PRIMARY_CONTROLLER.getRightX(),
() -> {
return VISION_SUBSYSTEM.getObjectLocation().isPresent()
? VISION_SUBSYSTEM.getObjectLocation().get()
: null;
return VISION_SUBSYSTEM.getObjectLocation().orElse(null);
},
false,
false).withTimeout(0.5),
false).until(() -> VISION_SUBSYSTEM.shouldIntake()),
Commands.parallel(
Commands.run(() -> {
DRIVE_SUBSYSTEM.autoDrive(new ChassisSpeeds(3, 0, 0));
}, DRIVE_SUBSYSTEM)
DRIVE_SUBSYSTEM.aimAtPointCommand(
() -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getCos(),
() -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getSin(),
() -> 0,
() -> {
return VISION_SUBSYSTEM.getObjectLocation().orElse(null);
},
false,
false).onlyIf(() -> VISION_SUBSYSTEM.shouldIntake())
// Commands.run(() -> {
// DRIVE_SUBSYSTEM.autoDrive(new ChassisSpeeds(3, 0, 0));

// }, DRIVE_SUBSYSTEM)
// INTAKE_SUBSYSTEM.intakeCommand(),
// SHOOTER_SUBSYSTEM.intakeCommand()
)
Expand Down
13 changes: 12 additions & 1 deletion src/main/java/frc/robot/subsystems/vision/ObjectCamera.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,9 @@
* Camera that looks for rings on the ground
*/
public class ObjectCamera implements AutoCloseable {
private static final double TARGET_HEIGHT_METERS = 0.0508;
private static final double TARGET_HEIGHT_METERS = 0;
private static final double MIN_OBJECT_AREA = 0.1;
private static final double INTAKE_YAW_TOLERANCE = 2;

private PhotonCamera m_camera;
private PhotonCameraSim m_cameraSim;
Expand Down Expand Up @@ -80,6 +82,8 @@ public PhotonCameraSim getCameraSim() {
* @return Distance to object, empty if undetected
*/
public Optional<Measure<Distance>> getDistance() {
if (getObjectArea().orElse(0.0) < MIN_OBJECT_AREA) return Optional.empty();

PhotonPipelineResult result = m_camera.getLatestResult();
if (!result.hasTargets()) return Optional.empty();

Expand All @@ -98,6 +102,7 @@ public Optional<Measure<Distance>> getDistance() {
* @return Yaw angle to target, empty if undetected
*/
public Optional<Measure<Angle>> getYaw() {
if (getObjectArea().orElse(0.0) < MIN_OBJECT_AREA) return Optional.empty();
PhotonPipelineResult result = m_camera.getLatestResult();
if (!result.hasTargets()) return Optional.empty();
return Optional.of(Units.Degrees.of(result.getBestTarget().getYaw()));
Expand All @@ -111,6 +116,12 @@ public Transform3d getTransform() {
return m_transform;
}

public Optional<Double> getObjectArea() {
PhotonPipelineResult result = m_camera.getLatestResult();
if (!result.hasTargets()) return Optional.empty();
return Optional.of(result.getBestTarget().getArea());
}

@Override
public void close() {
m_camera.close();
Expand Down
Loading

0 comments on commit 9be44c6

Please sign in to comment.