Skip to content

Commit

Permalink
fixed some bugs in the amp animation in example
Browse files Browse the repository at this point in the history
  • Loading branch information
@catr1xLiu
catr1xLiu committed Sep 12, 2024
1 parent b6c9164 commit bdf1eaf
Show file tree
Hide file tree
Showing 3 changed files with 34 additions and 60 deletions.
21 changes: 11 additions & 10 deletions example/5516-2024-OffSeason/src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,6 @@
import frc.robot.subsystems.vision.apriltags.AprilTagVisionIOReal;
import frc.robot.subsystems.vision.apriltags.ApriltagVisionIOSim;
import frc.robot.utils.CompetitionFieldUtils.CompetitionFieldVisualizer;
import frc.robot.utils.CompetitionFieldUtils.Objects.Crescendo2024FieldObjects;
import frc.robot.utils.CompetitionFieldUtils.Simulations.CompetitionFieldSimulation;
import frc.robot.utils.CompetitionFieldUtils.Simulations.Crescendo2024FieldSimulation;
import frc.robot.utils.CompetitionFieldUtils.Simulations.OpponentRobotSimulation;
Expand Down Expand Up @@ -362,12 +361,12 @@ public void configureButtonBindings() {
).ignoringDisable(true)
);

/* intake commands */
driverXBox.leftTrigger(0.5).whileTrue(intake.executeIntakeNote(
joystickDrive,
ledStatusLight,
driverXBox.getHID()
));
/* drive slowly and intake note, rumble game-pad when note detected */
driverXBox
.leftTrigger(0.5)
.whileTrue(intake.executeIntakeNote(ledStatusLight, driverXBox.getHID()))
.onTrue(Commands.runOnce(() -> joystickDrive.setSensitivity(0.6, 0.4)))
.onFalse(Commands.runOnce(joystickDrive::resetSensitivity));
/* split note from bottom */
driverXBox.a().whileTrue(Commands.run(
() -> {
Expand All @@ -379,9 +378,11 @@ public void configureButtonBindings() {
/* amp command */
driverXBox.y()
.onTrue(new PrepareToAmp(pitch, flyWheels, ledStatusLight))
.whileTrue(Commands.run(() -> joystickDrive.setCurrentRotationalMaintenance(
FieldConstants.toCurrentAllianceRotation(Rotation2d.fromDegrees(-90))
)))
.whileTrue(Commands.run(() -> {
joystickDrive.setCurrentRotationalMaintenance(
FieldConstants.toCurrentAllianceRotation(Rotation2d.fromDegrees(-90)));
joystickDrive.setSensitivity(0.7, 1);
}).finallyDo(joystickDrive::resetSensitivity))
.onFalse(new ScoreAmp(intake, pitch, flyWheels, ledStatusLight)
.withVisualization(visualizerForShooter, fieldSimulation, drive)
);
Expand Down
49 changes: 20 additions & 29 deletions example/5516-2024-OffSeason/src/main/java/frc/robot/commands/shooter/ScoreAmp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,21 +61,25 @@ public boolean isFinished() {

// Visualization
public Command withVisualization(CompetitionFieldVisualizer visualizer, CompetitionFieldSimulation simulation, HolonomicDriveSubsystem driveSubsystem) {
final Command visualizeAmpNote = Commands.run(() -> {
final Command visualizeAmpNote = Commands.runOnce(() -> {
final Transform2d poseError = FieldConstants
.toCurrentAlliancePose(CORRECT_SCORE_AMP_ROBOT_POSE_BLUE)
.minus(driveSubsystem.getPose());
final Translation2d shooterPositionOnField = driveSubsystem.getPose()
.getTranslation()
.plus(new Translation2d(-0.3, 0).rotateBy(driveSubsystem.getFacing()));
if (poseError.getTranslation().getNorm() < 0.25
&& Math.abs(poseError.getRotation().getDegrees()) < 8)
visualizer.addGamePieceOnFly(new AmpSuccessNote(driveSubsystem.getPose().getTranslation()));
visualizer.addGamePieceOnFly(new AmpSuccessNote(shooterPositionOnField));
else
visualizer.addGamePieceOnFly(new AmpFailedNote(
driveSubsystem.getPose(),
driveSubsystem.getPose().getRotation(),
shooterPositionOnField,
simulation
));
});
final Command visualizationCommand = Commands
.waitSeconds(0.5)
.waitSeconds(0.3)
.andThen(visualizeAmpNote)
.onlyIf(() -> hasNote && visualizer != null);
return this.alongWith(visualizationCommand);
Expand All @@ -84,8 +88,9 @@ public Command withVisualization(CompetitionFieldVisualizer visualizer, Competit
private static final double
SHOOTER_HEIGHT_AMP = 0.4,
AMP_HEIGHT = 0.8,
AMP_INITIAL_VELOCITY = 4,
AMP_FLIGHT_TIME_SECONDS = 0.8;
private static final Translation3d AMP_POSITION = new Translation3d(1.81, 8.11, AMP_HEIGHT);
private static final Translation3d AMP_POSITION = new Translation3d(1.81, 8.41, AMP_HEIGHT);
private static final Pose2d CORRECT_SCORE_AMP_ROBOT_POSE_BLUE = new Pose2d(1.81, 7.71, Rotation2d.fromDegrees(-90));
private static final class AmpSuccessNote extends GamePieceOnFlyDisplay {
public AmpSuccessNote(Translation2d robotPosition) {
Expand All @@ -105,14 +110,9 @@ public String getTypeName() {
public Pose3d getPose3d() {
/*
* the amplified note follows the free-fall law
* dx = v * t - 1/2 * t ^ 2 * g;
* solve for v = (dx + 1/2 * t ^ 2 * g) / t
* h = h0 + v*t - 1/2*t^2*g;
* */

final double initialVelocity = (
(AMP_HEIGHT - SHOOTER_HEIGHT_AMP)
+ 1.0/2.0 * AMP_FLIGHT_TIME_SECONDS * AMP_FLIGHT_TIME_SECONDS * 10),
height = initialVelocity * getT()
final double height = SHOOTER_HEIGHT_AMP + AMP_INITIAL_VELOCITY * getT()
- 1.0/2.0 * getT() * getT() * 10;
return new Pose3d(
new Translation3d(super.getPose3d().getX(), super.getPose3d().getY(), height),
Expand All @@ -122,17 +122,17 @@ public Pose3d getPose3d() {
}

private static final class AmpFailedNote extends GamePieceOnFlyDisplay {
private final Rotation2d robotYawWhenLaunching;
public AmpFailedNote(Pose2d robotPoseWhenLaunching, CompetitionFieldSimulation simulation) {
private final Rotation2d robotFacingWhenLaunching;
public AmpFailedNote(Rotation2d robotFacingWhenLaunching, Translation2d shooterPositionOnField, CompetitionFieldSimulation simulation) {
super(
new Translation3d(robotPoseWhenLaunching.getX(), robotPoseWhenLaunching.getY(), SHOOTER_HEIGHT_AMP),
new Translation3d(shooterPositionOnField.getX(), shooterPositionOnField.getY(), SHOOTER_HEIGHT_AMP),
getNoteTouchGroundTranslation3d(
robotPoseWhenLaunching.getTranslation().plus(
new Translation2d(-0.2, robotPoseWhenLaunching.getRotation())
shooterPositionOnField.plus(
new Translation2d(-0.7, 0).rotateBy(robotFacingWhenLaunching)
)),
AMP_FLIGHT_TIME_SECONDS
);
this.robotYawWhenLaunching = robotPoseWhenLaunching.getRotation();
this.robotFacingWhenLaunching = robotFacingWhenLaunching;
if (simulation != null) CommandScheduler.getInstance().schedule(
Commands.waitSeconds(AMP_FLIGHT_TIME_SECONDS).andThen(Commands.runOnce(
() -> simulation.addGamePiece(new Crescendo2024FieldObjects.NoteOnFieldSimulated(
Expand All @@ -157,20 +157,11 @@ public String getTypeName() {

@Override
public Pose3d getPose3d() {
/*
* the amplified note follows the free-fall law
* dx = v * t - 1/2 * t ^ 2 * g;
* solve for v = (dx + 1/2 * t ^ 2 * g) / t
* */

final double initialVelocity = (
(AMP_HEIGHT - SHOOTER_HEIGHT_AMP)
+ 1.0/2.0 * AMP_FLIGHT_TIME_SECONDS * AMP_FLIGHT_TIME_SECONDS * 10),
height = initialVelocity * getT()
final double height = SHOOTER_HEIGHT_AMP + AMP_INITIAL_VELOCITY * getT()
- 1.0/2.0 * getT() * getT() * 10;
return new Pose3d(
new Translation3d(super.getPose3d().getX(), super.getPose3d().getY(), height),
new Rotation3d(Math.toRadians(90), 0, robotYawWhenLaunching.getRadians())
new Rotation3d(0, Math.toRadians(90), robotFacingWhenLaunching.getRadians())
);
}
}
Expand Down
24 changes: 3 additions & 21 deletions example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/intake/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,33 +103,15 @@ public Command executeIntakeNote() {
return Commands.run(() -> {
if (inputs.lowerBeamBreakBlocked)
runMinimumPropellingVoltage();
else
runFullIntakeVoltage();
else runFullIntakeVoltage();
}, this)
.until(() -> inputs.upperBeamBreakerBlocked)
.onlyIf(() -> !inputs.upperBeamBreakerBlocked)
.finallyDo(this::runIdle);
}

public Command executeIntakeNote(JoystickDrive joystickDrive) {
return Commands.run(() -> {
if (inputs.lowerBeamBreakBlocked) {
runMinimumPropellingVoltage();
joystickDrive.resetSensitivity();
}
else {
runFullIntakeVoltage();
joystickDrive.setSensitivity(0.4, 0.4);
}
}, this)
.until(() -> inputs.upperBeamBreakerBlocked)
.onlyIf(() -> !inputs.upperBeamBreakerBlocked)
.finallyDo(joystickDrive::resetSensitivity)
.finallyDo(this::runIdle);
}

public Command executeIntakeNote(JoystickDrive joystickDrive, LEDStatusLight statusLight, XboxController xboxController) {
return executeIntakeNote(joystickDrive)
public Command executeIntakeNote(LEDStatusLight statusLight, XboxController xboxController) {
return executeIntakeNote()
.raceWith(Commands.run(() -> statusLight.setAnimation(RUNNING), statusLight))
.andThen(statusLight.playAnimationAndStop(GRABBED_NOTE, 1.5)
.deadlineWith(rumbleGamepad(xboxController))
Expand Down

0 comments on commit bdf1eaf

Please sign in to comment.