Skip to content

Commit

Permalink
Format
Browse files Browse the repository at this point in the history
  • Loading branch information
@ohowe1
ohowe1 committed Feb 22, 2024
1 parent 801e434 commit a65e361
Show file tree
Hide file tree
Showing 7 changed files with 103 additions and 56 deletions.
6 changes: 2 additions & 4 deletions src/main/java/frc/robot/BuildConstants.java
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@
package frc.robot;

/**
* Automatically generated file containing build version information.
*/
/** Automatically generated file containing build version information. */
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "Competition2024";
Expand All @@ -15,5 +13,5 @@ public final class BuildConstants {
public static final long BUILD_UNIX_TIME = 1708616545688L;
public static final int DIRTY = 1;

private BuildConstants(){}
private BuildConstants() {}
}
7 changes: 6 additions & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,12 @@ public static class SlapdownConstants {
"/slapdown/rotation/trapGains", 25, 40, MiscConstants.TUNING_MODE);
public static final TunableArmElevatorFFGains ROTATION_FF_GAINS =
new TunableArmElevatorFFGains(
"/slapdown/rotation/FFGains", 0.10403, 0.17546, 0.61704, 0.084257, MiscConstants.TUNING_MODE);
"/slapdown/rotation/FFGains",
0.10403,
0.17546,
0.61704,
0.084257,
MiscConstants.TUNING_MODE);
}

public static class ElevatorConstants {
Expand Down
83 changes: 60 additions & 23 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,9 +45,7 @@ public class RobotContainer {
// private final PhotonSubsystem photonSubsystem = new PhotonSubsystem();
// private final SwerveDriveSubsystem driveSubsystem =
// new SwerveDriveSubsystem(photonSubsystem::getEstimatedGlobalPose);
private final SwerveDriveSubsystem driveSubsystem =
new SwerveDriveSubsystem(
(pose) -> List.of());
private final SwerveDriveSubsystem driveSubsystem = new SwerveDriveSubsystem((pose) -> List.of());
private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
private final WristSubsystem wristSubsystem = new WristSubsystem();
Expand Down Expand Up @@ -77,14 +75,30 @@ public RobotContainer() {
}

private void configureAutos() {
autoCommand.addOption("Slapdown Q Forward", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdQuasistatic(SysIdRoutine.Direction.kForward));
autoCommand.addOption("Slapdown Q Back", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
autoCommand.addOption("Slapdown D Forward", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdDynamic(SysIdRoutine.Direction.kForward));
autoCommand.addOption("Slapdown D Back", slapdownSuperstructure.getSlapdownRotationSubsystem().sysIdDynamic(SysIdRoutine.Direction.kReverse));
autoCommand.addOption("slapdown bottom", slapdownSuperstructure.setDownAndRunCommand());
autoCommand.addOption(
"Slapdown Q Forward",
slapdownSuperstructure
.getSlapdownRotationSubsystem()
.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
autoCommand.addOption(
"Slapdown Q Back",
slapdownSuperstructure
.getSlapdownRotationSubsystem()
.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
autoCommand.addOption(
"Slapdown D Forward",
slapdownSuperstructure
.getSlapdownRotationSubsystem()
.sysIdDynamic(SysIdRoutine.Direction.kForward));
autoCommand.addOption(
"Slapdown D Back",
slapdownSuperstructure
.getSlapdownRotationSubsystem()
.sysIdDynamic(SysIdRoutine.Direction.kReverse));
autoCommand.addOption("slapdown bottom", slapdownSuperstructure.setDownAndRunCommand());
autoCommand.addOption("slapdown top", slapdownSuperstructure.setUpCommand());

autoCommand.addOption("Probe Elevator", elevatorSubsystem.probeHomeCommand());
autoCommand.addOption("Probe Elevator", elevatorSubsystem.probeHomeCommand());
autoCommand.addOption(
"Wrist Q Forward", wristSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
autoCommand.addOption(
Expand All @@ -94,8 +108,7 @@ private void configureAutos() {
autoCommand.addOption(
"Wrist D Back", wristSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
autoCommand.addOption(
"Wrist 5 deg",
wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(5))));
"Wrist 5 deg", wristSubsystem.setPositonCommand(new Rotation2d(Units.degreesToRadians(5))));
autoCommand.addOption(
"Elevator Quastatic Backward",
elevatorSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
Expand All @@ -121,9 +134,16 @@ private void configureAutos() {
autoCommand.addOption(
"Shooter Quastatic Forward Command",
shooterSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
autoCommand.addOption("WristElevatorZero", Commands.parallel(
wristSubsystem.setPositonCommand(Constants.WristConstants.WRIST_MIN), elevatorSubsystem.setElevatorPositionCommand(0.0)));
autoCommand.addOption("WristElevatorNot", Commands.parallel(wristSubsystem.setPositonCommand(Rotation2d.fromDegrees(40.0)), elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(8.0))));
autoCommand.addOption(
"WristElevatorZero",
Commands.parallel(
wristSubsystem.setPositonCommand(Constants.WristConstants.WRIST_MIN),
elevatorSubsystem.setElevatorPositionCommand(0.0)));
autoCommand.addOption(
"WristElevatorNot",
Commands.parallel(
wristSubsystem.setPositonCommand(Rotation2d.fromDegrees(40.0)),
elevatorSubsystem.setElevatorPositionCommand(Units.inchesToMeters(8.0))));

autoCommand.addOption(
"Shooter Quastatic Backward Command",
Expand All @@ -149,14 +169,30 @@ private void configureAutos() {
private void configureDriverBindings() {
configureDriving();
driverController
.home()
.onTrue(
RaiderCommands.runOnceAllowDisable(driveSubsystem::zeroHeading)
.withName("ZeroHeading"));
driverController.leftTrigger().whileTrue(transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_VOLTAGE));
.home()
.onTrue(
RaiderCommands.runOnceAllowDisable(driveSubsystem::zeroHeading)
.withName("ZeroHeading"));
driverController
.leftTrigger()
.whileTrue(
transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_VOLTAGE));
driverController.minus().whileTrue(new LockModulesCommand(driveSubsystem).repeatedly());
Command intakeAndFeedUntilDone = Commands.parallel(intakeSubsystem.setIntakeVoltageCommand(Constants.IntakeConstants.INTAKE_VOLTAGE), transportSubsystem.setVoltageCommand(Constants.TransportConstants.TRANSPORT_VOLTAGE)).until(transportSubsystem::atSensor).unless(transportSubsystem::atSensor);
driverController.leftBumper().whileTrue(Commands.parallel(slapdownSuperstructure.setDownAndRunCommand(), intakeAndFeedUntilDone.asProxy(), elevatorSubsystem.setElevatorPositionCommand(0), wristSubsystem.setPositonCommand(new Rotation2d(0))));
Command intakeAndFeedUntilDone =
Commands.parallel(
intakeSubsystem.setIntakeVoltageCommand(Constants.IntakeConstants.INTAKE_VOLTAGE),
transportSubsystem.setVoltageCommand(
Constants.TransportConstants.TRANSPORT_VOLTAGE))
.until(transportSubsystem::atSensor)
.unless(transportSubsystem::atSensor);
driverController
.leftBumper()
.whileTrue(
Commands.parallel(
slapdownSuperstructure.setDownAndRunCommand(),
intakeAndFeedUntilDone.asProxy(),
elevatorSubsystem.setElevatorPositionCommand(0),
wristSubsystem.setPositonCommand(new Rotation2d(0))));
driverController.leftBumper().onFalse(slapdownSuperstructure.setUpCommand());
// TODO: Speaker centric
driverController.rightTrigger().whileTrue(Commands.none());
Expand All @@ -165,11 +201,12 @@ private void configureDriverBindings() {
// TODO: Climb auto align
driverController.a().whileTrue(Commands.none());
driverController.circle().whileTrue(new LockModulesCommand(driveSubsystem).repeatedly());

}

private void configureOperatorBindings() {
operatorController.triangle().onTrue(Commands.sequence(shooterSubsystem.runVelocityCommand(10000.0/60)));
operatorController
.triangle()
.onTrue(Commands.sequence(shooterSubsystem.runVelocityCommand(10000.0 / 60)));
operatorController.x().onTrue(shooterSubsystem.setVoltageCommand(0));
operatorController
.leftTrigger()
Expand Down
28 changes: 17 additions & 11 deletions src/main/java/frc/robot/subsystems/slapdown/SlapdownRotationSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,10 +46,14 @@ public class SlapdownRotationSubsystem extends SubsystemBase {
this));

private final ArmFeedforward rotationFF = ROTATION_FF_GAINS.createArmFeedforward();
private final BooleanTelemetryEntry atLimitEntry = new BooleanTelemetryEntry("/slapdown/rotation/atLimit", true);

private final BooleanTelemetryEntry isHomedEntry = new BooleanTelemetryEntry("/slapdown/rotation/isHomed", true);
private final DoubleTelemetryEntry voltageRequestEntry = new DoubleTelemetryEntry("/slapdown/rotation/voltageReq", Constants.MiscConstants.TUNING_MODE);
private final BooleanTelemetryEntry atLimitEntry =
new BooleanTelemetryEntry("/slapdown/rotation/atLimit", true);

private final BooleanTelemetryEntry isHomedEntry =
new BooleanTelemetryEntry("/slapdown/rotation/isHomed", true);
private final DoubleTelemetryEntry voltageRequestEntry =
new DoubleTelemetryEntry(
"/slapdown/rotation/voltageReq", Constants.MiscConstants.TUNING_MODE);
private final TunableTelemetryProfiledPIDController rotationController =
new TunableTelemetryProfiledPIDController(
"/slapdown/rotation/controller", ROTATION_GAINS, ROTATION_TRAP_GAINS);
Expand Down Expand Up @@ -145,21 +149,24 @@ private boolean isHomed() {
}

public Command setRotationGoalCommand(Rotation2d goal) {
return RaiderCommands.ifCondition(this::isHomed).then(
return RaiderCommands.ifCondition(this::isHomed)
.then(
this.run(
() -> {
double feedbackOutput = rotationController.calculate(getPosition());
TrapezoidProfile.State currentSetpoint = rotationController.getSetpoint();

setRotationVoltage(
feedbackOutput + rotationFF.calculate(currentSetpoint.position, currentSetpoint.velocity));
}).beforeStarting(
feedbackOutput
+ rotationFF.calculate(
currentSetpoint.position, currentSetpoint.velocity));
})
.beforeStarting(
() -> {
rotationController.setGoal(goal.getRadians());
rotationController.reset(getPosition(), rotationEncoder.getVelocity());
}
)
).otherwise(Commands.none());
}))
.otherwise(Commands.none());
}

public Command setVoltageCommand(double voltage) {
Expand All @@ -183,7 +190,6 @@ public void periodic() {
if (atLimit()) {
rotationEncoder.setPosition(ROTATION_UP_ANGLE);
isHomed = true;

}
isHomedEntry.append(isHomed);
atLimitEntry.append(atLimit());
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/subsystems/slapdown/SlapdownSuperstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,8 @@ public Command setDownAndRunCommand() {
feederSubsystem.setVoltageCommand(FEEDER_VOLTAGE),
rotationSubsystem.setRotationGoalCommand(Rotation2d.fromRadians(ROTATION_DOWN_ANGLE)));
}
public SlapdownRotationSubsystem getSlapdownRotationSubsystem(){

public SlapdownRotationSubsystem getSlapdownRotationSubsystem() {
return rotationSubsystem;
}

Expand Down
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@
import frc.robot.Constants;
import frc.robot.Constants.MiscConstants;
import frc.robot.FieldConstants;
import frc.robot.Robot;
import frc.robot.telemetry.types.*;
import frc.robot.telemetry.wrappers.TelemetryPigeon2;
import frc.robot.utils.Alert;
Expand Down Expand Up @@ -166,7 +165,7 @@ public SwerveDriveSubsystem(Function<Pose2d, List<EstimatedRobotPose>> cameraPos
kinematics, getGyroRotation(), getModulePositions(), new Pose2d());

// Start odometry thread
// Robot.getInstance().addPeriodic(this::updateOdometry, 1.0 / ODOMETRY_FREQUENCY);
// Robot.getInstance().addPeriodic(this::updateOdometry, 1.0 / ODOMETRY_FREQUENCY);

stopMovement();
}
Expand Down
29 changes: 15 additions & 14 deletions src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,8 @@ private void configMotor() {

public double getPosition() {

return MathUtil.angleModulus(Units.rotationsToRadians(absoluteEncoder.getAbsolutePosition()) + WRIST_OFFSET);
return MathUtil.angleModulus(
Units.rotationsToRadians(absoluteEncoder.getAbsolutePosition()) + WRIST_OFFSET);
}

public boolean atGoal() {
Expand All @@ -143,19 +144,19 @@ public Command setVotageCommand(double voltage) {

public Command setPositonCommand(Rotation2d desiredPosition) {
return this.run(
() -> {
double feedbackOutput = controller.calculate(getPosition());
TrapezoidProfile.State currentSetpoint = controller.getSetpoint();

setVoltage(
feedbackOutput
+ feedforward.calculate(currentSetpoint.position, currentSetpoint.velocity));
})
.beforeStarting(
() -> {
controller.reset(getPosition(), wristMotor.getEncoder().getVelocity());
controller.setGoal(desiredPosition.getRadians());
});
() -> {
double feedbackOutput = controller.calculate(getPosition());
TrapezoidProfile.State currentSetpoint = controller.getSetpoint();

setVoltage(
feedbackOutput
+ feedforward.calculate(currentSetpoint.position, currentSetpoint.velocity));
})
.beforeStarting(
() -> {
controller.reset(getPosition(), wristMotor.getEncoder().getVelocity());
controller.setGoal(desiredPosition.getRadians());
});
}

public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
Expand Down

0 comments on commit a65e361

Please sign in to comment.