Skip to content

Commit

Permalink
Add operator controller
Browse files Browse the repository at this point in the history 
Add amp scoring
Use buttons to change compensation
Add tuning mode to controls
  • Loading branch information
@suryatho
suryatho committed Feb 18, 2024
1 parent 69b5c0c commit bd770e7
Show file tree
Hide file tree
Showing 5 changed files with 106 additions and 60 deletions.
132 changes: 90 additions & 42 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
Expand All @@ -17,6 +19,9 @@
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import java.util.function.Function;
import java.util.function.Supplier;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization;
import org.littletonrobotics.frc2024.commands.auto.AutoBuilder;
import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision;
Expand Down Expand Up @@ -52,8 +57,10 @@
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
import org.littletonrobotics.frc2024.util.GeomUtil;
import org.littletonrobotics.frc2024.util.NoteVisualizer;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

/**
Expand All @@ -62,6 +69,7 @@
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and button mappings) should be declared here.
*/
@ExtensionMethod({GeomUtil.class})
public class RobotContainer {
// Load robot state
private final RobotState robotState = RobotState.getInstance();
Expand All @@ -73,8 +81,13 @@ public class RobotContainer {
private Rollers rollers;
private final Superstructure superstructure;

// Controller
private final CommandXboxController controller = new CommandXboxController(0);
// Controllers
private final CommandXboxController driverController = new CommandXboxController(0);
private final CommandXboxController operatorController = new CommandXboxController(1);
private final LoggedDashboardBoolean shotTuningMode =
new LoggedDashboardBoolean("Shot Tuning Mode", false);
private final LoggedDashboardBoolean autoAmpScore =
new LoggedDashboardBoolean("Allow Auto Amp Score", false);

// Dashboard inputs
private final LoggedDashboardChooser<Command> autoChooser =
Expand Down Expand Up @@ -227,12 +240,25 @@ private void configureButtonBindings() {
.run(
() ->
drive.acceptTeleopInput(
-controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))
-driverController.getLeftY(),
-driverController.getLeftX(),
-driverController.getRightX()))
.withName("Drive Teleop Input"));

// Set up toggles
Trigger inShotTuningMode = new Trigger(shotTuningMode::get);
Trigger allowingAutoAmpScore = new Trigger(autoAmpScore::get);
operatorController
.start()
.onTrue(Commands.runOnce(() -> shotTuningMode.set(!inShotTuningMode.getAsBoolean())));
operatorController
.x()
.onTrue(Commands.runOnce(() -> autoAmpScore.set(!allowingAutoAmpScore.getAsBoolean())));

// Aim and Rev Flywheels
controller
driverController
.a()
.and(inShotTuningMode.negate())
.whileTrue(
Commands.startEnd(
() ->
Expand All @@ -241,26 +267,49 @@ private void configureButtonBindings() {
drive::clearHeadingGoal)
.alongWith(superstructure.aim(), flywheels.shootCommand())
.withName("Prepare Shot"));
// Tuning mode controls
driverController
.a()
.and(inShotTuningMode)
.whileTrue(
Commands.parallel(
superstructure.diagnoseArm(),
flywheels.shootCommand(),
Commands.startEnd(
() ->
drive.setHeadingGoal(
() -> RobotState.getInstance().getAimingParameters().driveHeading()),
drive::clearHeadingGoal)));
// Shoot
Trigger readyToShoot =
new Trigger(() -> drive.atHeadingGoal() && superstructure.atGoal() && flywheels.atGoal());
new Trigger(() -> drive.atHeadingGoal() && superstructure.atGoal() && flywheels.atGoal())
.and(inShotTuningMode.negate());
readyToShoot
.whileTrue(
Commands.run(
() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0)))
() -> driverController.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0)))
.whileFalse(
Commands.run(
() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
controller
() -> driverController.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
// Shoot at current arm and flywheel setpoint
driverController
.rightTrigger()
.onTrue(
Commands.parallel(
Commands.waitSeconds(0.5),
Commands.waitUntil(controller.rightTrigger().negate()))
.deadlineWith(
rollers.feedShooter(), superstructure.aim(), flywheels.shootCommand()));
Commands.waitUntil(driverController.rightTrigger().negate()))
.deadlineWith(rollers.feedShooter())
.finallyDo(
() -> {
Logger.recordOutput(
"RobotState/ShootingArmAngle", superstructure.getArmAngle().getRadians());
Logger.recordOutput(
"RobotState/ShootingEffectiveDistance",
RobotState.getInstance().getAimingParameters().effectiveDistance());
})
.withName("Shoot"));
// Intake Floor
controller
driverController
.leftTrigger()
.whileTrue(
superstructure
Expand All @@ -269,44 +318,43 @@ private void configureButtonBindings() {
Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake()))
.withName("Floor Intake"));
// Eject Floor
controller
driverController
.leftBumper()
.whileTrue(
superstructure
.intake()
.alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor()))
.withName("Eject To Floor"));
// Test rollers with amp score
controller.b().whileTrue(rollers.ampScore());

// Tune arm look up table
controller
.x()
.whileTrue(
Commands.parallel(
superstructure.diagnoseArm(),
flywheels.shootCommand(),
Commands.startEnd(
() ->
drive.setHeadingGoal(
() -> RobotState.getInstance().getAimingParameters().driveHeading()),
drive::clearHeadingGoal)))
.and(controller.rightBumper())
.onTrue(
Commands.parallel(
Commands.waitSeconds(0.5),
Commands.waitUntil(controller.rightBumper().negate()))
.deadlineWith(rollers.feedShooter())
.finallyDo(
() -> {
Logger.recordOutput(
"RobotState/ShootingArmAngle", superstructure.getArmAngle().getRadians());
Logger.recordOutput(
"RobotState/ShootingEffectiveDistance",
RobotState.getInstance().getAimingParameters().effectiveDistance());
}));
// Amp scoring
operatorController.a().whileTrue(superstructure.amp());
driverController.b().whileTrue(rollers.ampScore());

// Auto amp scoring
Pose2d goalAmpScorePose =
new Pose2d(FieldConstants.ampCenter, new Rotation2d(Math.PI / 2.0))
.transformBy(
new Translation2d(
0.0,
-(DriveConstants.driveConfig.bumperWidthX() / 2.0
+ Units.inchesToMeters(3.0)))
.toTransform2d());
Supplier<Pose2d> ampScoringPoseSupp = () -> AllianceFlipUtil.apply(goalAmpScorePose);

// Operator controls
Function<Double, Command> changeCompensationDegrees =
compensation ->
Commands.runOnce(
() -> {
double currentCompensation =
RobotState.getInstance().getShotCompensationDegrees();
RobotState.getInstance()
.setShotCompensationDegrees(currentCompensation + compensation);
});
operatorController.povDown().onTrue(changeCompensationDegrees.apply(-0.1));
operatorController.povUp().onTrue(changeCompensationDegrees.apply(0.1));

controller
driverController
.y()
.onTrue(
Commands.runOnce(
Expand Down
14 changes: 6 additions & 8 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@
import lombok.Setter;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
import org.littletonrobotics.frc2024.util.GeomUtil;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
Expand Down Expand Up @@ -64,7 +63,10 @@ public record AimingParameters(
armAngleMap.put(4.145, .454);
}

@Setter @Getter private double shotCompensationDegrees = 0.0;
@AutoLogOutput(key = "RobotState/ShotCompensationDegrees")
@Setter
@Getter
private double shotCompensationDegrees = 0.0;

private static RobotState instance;

Expand Down Expand Up @@ -215,15 +217,11 @@ public AimingParameters getAimingParameters() {
robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance
- robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance;

Rotation2d armAngleToSpeaker =
new Rotation2d(
targetDistance - ArmConstants.armOrigin.getX(),
FieldConstants.Speaker.centerSpeakerOpening.getZ() - ArmConstants.armOrigin.getY());

latestParameters =
new AimingParameters(
targetVehicleDirection,
Rotation2d.fromRadians(armAngleMap.get(targetDistance)),
Rotation2d.fromRadians(
armAngleMap.get(targetDistance) + Units.degreesToRadians(shotCompensationDegrees)),
targetDistance,
feedVelocity);
Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading());
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ public void periodic() {
gamepieceState = GamepieceState.NONE;
}
case AMP_SCORE -> {
feeder.setGoal(Feeder.Goal.IDLING);
feeder.setGoal(Feeder.Goal.FLOOR_INTAKING);
indexer.setGoal(Indexer.Goal.EJECTING);
intake.setGoal(Intake.Goal.IDLING);
backpack.setGoal(Backpack.Goal.AMP_SCORING);
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ public void periodic() {
case INTAKE -> arm.setGoal(Arm.Goal.FLOOR_INTAKE);
case STATION_INTAKE -> arm.setGoal(Arm.Goal.STATION_INTAKE);
case DIAGNOSTIC_ARM -> arm.setGoal(Arm.Goal.CUSTOM);
case AMP -> arm.setGoal(Arm.Goal.AMP);
default -> {} // DO NOTHING ELSE
}

Expand Down Expand Up @@ -87,6 +88,10 @@ public Command intake() {
.withName("Superstructure Intaking");
}

public Command amp() {
return startEnd(() -> desiredGoal = Goal.AMP, this::stow).withName("Superstructure Amping");
}

public Command stationIntake() {
return startEnd(() -> desiredGoal = Goal.STATION_INTAKE, this::stow)
.withName("Superstructure Station Intaking");
Expand Down
13 changes: 4 additions & 9 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,19 +48,14 @@ public class Arm {
new LoggedTunableNumber("Arm/LowerLimitDegrees", minAngle.getDegrees());
private static final LoggedTunableNumber upperLimitDegrees =
new LoggedTunableNumber("Arm/UpperLimitDegrees", maxAngle.getDegrees());
private static final LoggedTunableNumber stowDegrees =
new LoggedTunableNumber("Superstructure/ArmStowDegrees", 20.0);
private static final LoggedTunableNumber stationIntakeDegrees =
new LoggedTunableNumber("Superstructure/ArmStationIntakeDegrees", 45.0);
private static final LoggedTunableNumber intakeDegrees =
new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0);

@RequiredArgsConstructor
public enum Goal {
FLOOR_INTAKE(intakeDegrees),
STATION_INTAKE(stationIntakeDegrees),
FLOOR_INTAKE(new LoggedTunableNumber("Arm/IntakeDegrees", 18.0)),
STATION_INTAKE(new LoggedTunableNumber("Arm/StationIntakeDegrees", 45.0)),
AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()),
STOW(stowDegrees),
STOW(new LoggedTunableNumber("Arm/StowDegrees", 10.0)),
AMP(new LoggedTunableNumber("Arm/AmpDegrees", 100.0)),
CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0));

private final DoubleSupplier armSetpointSupplier;
Expand Down

0 comments on commit bd770e7

Please sign in to comment.