Skip to content

Commit

Permalink
Cleanup (#23)
Browse files Browse the repository at this point in the history
* refactor: formatting

* fix: commit intellij code formatting

* bump: path planner

* feat: clean up staging and shoot commands

* feat: hide path diffs by default

* fix: line endings and linguist language

* refactor: consistent method naming for beam break checks
  • Loading branch information
rutmanz authored Feb 5, 2024
1 parent 88ddda1 commit f942fee
Show file tree
Hide file tree
Showing 48 changed files with 335 additions and 215 deletions.
2 changes: 2 additions & 0 deletions .gitattributes
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
src/main/deploy/** linguist-language=JSON linguist-generated=true
**/*.java text=auto
2 changes: 1 addition & 1 deletion .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,7 @@ bin/
*.iml
*.ipr
*.iws
.idea/

out/

# Fleet
Expand Down
3 changes: 3 additions & 0 deletions .idea/.gitignore

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

10 changes: 10 additions & 0 deletions .idea/codeStyles/Project.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

5 changes: 5 additions & 0 deletions .idea/codeStyles/codeStyleConfig.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

10 changes: 6 additions & 4 deletions src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ public enum Mode {
public static final double LOOP_PERIOD_SECS = 0.02;

public static class SwerveConfig {
public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "" : "";
public static final String CAN_BUS = IS_COMPETITION_ROBOT ? "" : "";
public static final int FRONT_LEFT = IS_COMPETITION_ROBOT ? 3 : 0;
public static final int FRONT_RIGHT = IS_COMPETITION_ROBOT ? 4 : 0;
public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 7 : 0;
Expand All @@ -63,6 +63,7 @@ public static class Drivetrain {
public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
}

public static class Indexer {
// TODO: fix these constants
public static final int INTAKE_ID = 11;
Expand Down Expand Up @@ -166,7 +167,7 @@ public static class Elevator {
public static final double ELEVATOR_MAX_HEIGHT = ELEVATOR_MINIMUM_HEIGHT + Units.inchesToMeters(21.0); //TODO: Fix these constants to be more accurate
public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + ELEVATOR_MAX_HEIGHT - ELEVATOR_MINIMUM_HEIGHT;

public static final double GEAR_RATIO = 2.0 / 1.0; //TODO: Get constants right sometime
public static final double GEAR_RATIO = 2.0; //TODO: Get constants right sometime
public static final int LEADER_ID = -1;
public static final int FOLLOWER_ID = -1;
public static final double KS = 0.25;
Expand All @@ -182,7 +183,7 @@ public static class Elevator {
public static final double SPROCKET_RADIUS_M = 0.022;
public static final double SPROCKET_CIRCUMFERENCE_M = 2 * SPROCKET_RADIUS_M * Math.PI;
public static final double MOTOR_ROTS_TO_METERS = GEAR_RATIO * SPROCKET_CIRCUMFERENCE_M;
public static final double ERROR_TOLERANCE = 0.03;
public static final double POS_ERR_TOLERANCE_METERS = 0.03;
public static final double SIM_CARRIAGE_MASS_KG = 1.55; //TODO: check this number :)

public enum ElevatorState {
Expand All @@ -208,14 +209,15 @@ public enum ElevatorState {
AMP(27.0); //TODO: Find these values :D

public final double heightMeters;

ElevatorState(double heightMeters) {
this.heightMeters = heightMeters;
}
}
}

public static class Tramp {
public static final double GEAR_RATIO = 3.0 / 1.0;
public static final double GEAR_RATIO = 3.0;
public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D
public static final int TRAMP_MOTOR_ID = -1; //TODO: Configure this later
public static final int TRAMP_BEAM_BREAK_CHANNEL = -1; //TODO: Configure this later
Expand Down
26 changes: 11 additions & 15 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,35 +8,36 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;

import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.robot2024.Constants.Elevator.ElevatorState;
import org.team1540.robot2024.commands.FeedForwardCharacterization;
import org.team1540.robot2024.commands.SwerveDriveCommand;
import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
import org.team1540.robot2024.commands.indexer.IntakeCommand;
import org.team1540.robot2024.commands.shooter.ShootSequence;
import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
import org.team1540.robot2024.subsystems.drive.*;
import org.team1540.robot2024.subsystems.elevator.Elevator;
import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
import org.team1540.robot2024.subsystems.elevator.ElevatorIOSim;
import org.team1540.robot2024.subsystems.elevator.ElevatorIOTalonFX;
import org.team1540.robot2024.subsystems.indexer.Indexer;
import org.team1540.robot2024.subsystems.indexer.IndexerIO;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.subsystems.tramp.Tramp;
import org.team1540.robot2024.subsystems.tramp.TrampIO;
import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.subsystems.vision.AprilTagVision;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
import org.team1540.robot2024.subsystems.indexer.Indexer;
import org.team1540.robot2024.subsystems.indexer.IndexerIO;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
import org.team1540.robot2024.util.swerve.SwerveFactory;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.team1540.robot2024.util.vision.VisionPoseAcceptor;

import static org.team1540.robot2024.Constants.SwerveConfig;
Expand Down Expand Up @@ -66,8 +67,6 @@ public class RobotContainer {
public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);

// TODO: testing dashboard inputs, remove for comp
public final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
public final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
Expand Down Expand Up @@ -138,13 +137,10 @@ public RobotContainer() {
new AprilTagVisionIO() {},
(ignored) -> {},
() -> 0.0,
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0));

indexer =
new Indexer(
new IndexerIO() {}
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)
);

indexer = new Indexer(new IndexerIO() {});
tramp = new Tramp(new TrampIO() {});

break;
Expand Down Expand Up @@ -187,7 +183,7 @@ private void configureButtonBindings() {
).ignoringDisable(true)
);

copilot.a().onTrue(shooter.spinUpCommand(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get()))
copilot.a().onTrue(new ShootSequence(shooter, indexer))
.onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));

driver.a().onTrue(new IntakeCommand(indexer));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ public void add(double velocity, double voltage) {
}

public void print() {
if (velocityData.size() == 0 || voltageData.size() == 0) {
if (velocityData.isEmpty() || voltageData.isEmpty()) {
return;
}

Expand All @@ -87,10 +87,10 @@ public void print() {
1);

System.out.println("FF Characterization Results:");
System.out.println("\tCount=" + Integer.toString(velocityData.size()) + "");
System.out.println(String.format("\tR2=%.5f", regression.R2()));
System.out.println(String.format("\tkS=%.5f", regression.beta(0)));
System.out.println(String.format("\tkV=%.5f", regression.beta(1)));
System.out.println("\tCount=" + velocityData.size());
System.out.printf("\tR2=%.5f%n", regression.R2());
System.out.printf("\tkS=%.5f%n", regression.beta(0));
System.out.printf("\tkV=%.5f%n", regression.beta(1));
}
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,9 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle

@Override
public void initialize() {
xLimiter.reset(0);
yLimiter.reset(0);
rotLimiter.reset(0);
xLimiter.reset(0);
yLimiter.reset(0);
rotLimiter.reset(0);
}

@Override
Expand All @@ -46,7 +46,7 @@ public void execute() {
// Calculate new linear velocity
Translation2d linearVelocity =
new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();

// Convert to field relative speeds & send command
drivetrain.runVelocity(
Expand Down
33 changes: 0 additions & 33 deletions src/main/java/org/team1540/robot2024/commands/TrampCommand.java

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,9 @@
public class DeclimbSequence extends SequentialCommandGroup {
public DeclimbSequence(Elevator elevator, Hooks hooks) {
addCommands(
new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
hooks.undeployHooksCommand(), //Release hooks
new ElevatorSetpointCommand(elevator, ElevatorState.TOP)
new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
hooks.undeployHooksCommand(), //Release hooks
new ElevatorSetpointCommand(elevator, ElevatorState.TOP)
);
}
}
Original file line number Diff line number Diff line change
@@ -1,16 +1,17 @@
package org.team1540.robot2024.commands.climb;

import org.team1540.robot2024.subsystems.tramp.Tramp;

import edu.wpi.first.wpilibj2.command.Command;
import org.team1540.robot2024.subsystems.tramp.Tramp;

//TODO: Write this command Tramp people :D
public class ScoreInTrap extends Command {
private final Tramp tramp;

public ScoreInTrap(Tramp tramp) {
this.tramp = tramp;
addRequirements(tramp);
}

@Override
public void initialize() {
//TODO: Score in trap :D
Expand Down
Original file line number Diff line number Diff line change
@@ -1,10 +1,9 @@
package org.team1540.robot2024.commands.elevator;

import org.team1540.robot2024.Constants;
import org.team1540.robot2024.subsystems.elevator.Elevator;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.team1540.robot2024.Constants;
import org.team1540.robot2024.subsystems.elevator.Elevator;

public class ElevatorManualCommand extends Command {

Expand All @@ -19,8 +18,7 @@ public ElevatorManualCommand(Elevator elevator, CommandXboxController copilot) {

@Override
public void execute() {
// elevator.setVoltage(copilot.getRightTriggerAxis() - copilot.getLeftTriggerAxis() + Constants.Elevator.KG);
elevator.setElevatorPosition(elevator.getSetpoint() + Constants.Elevator.CRUISE_VELOCITY_MPS*Constants.LOOP_PERIOD_SECS*(copilot.getRightTriggerAxis()-copilot.getLeftTriggerAxis()));
elevator.setElevatorPosition(elevator.getSetpoint() + Constants.Elevator.CRUISE_VELOCITY_MPS * Constants.LOOP_PERIOD_SECS * (copilot.getRightTriggerAxis() - copilot.getLeftTriggerAxis()));
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ public ElevatorSetpointCommand(Elevator elevator, ElevatorState state) {
this.state = state;
addRequirements(elevator);
}

@Override
public void initialize() {
elevator.setElevatorPosition(state.heightMeters);
Expand All @@ -22,6 +23,7 @@ public void initialize() {
public void end(boolean interrupted) {
elevator.stop();
}

@Override
public boolean isFinished() {
return elevator.isAtSetpoint();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public void initialize() {

@Override
public boolean isFinished() {
return indexer.isNotePresent();
return indexer.isNoteStaged();
}

@Override
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
package org.team1540.robot2024.commands.indexer;

import edu.wpi.first.wpilibj2.command.Command;
import org.team1540.robot2024.subsystems.indexer.Indexer;
import org.team1540.robot2024.subsystems.tramp.Tramp;

public class StageTrampCommand extends Command {

private final Tramp tramp;
private final Indexer indexer;

public StageTrampCommand(Tramp tramp, Indexer indexer) {
this.tramp = tramp;
this.indexer = indexer;
addRequirements(tramp, indexer);
}

@Override
public void initialize() {
tramp.setPercent(0.5);
indexer.setFeederVelocity(-600);
indexer.setIntakePercent(0.5);
}

@Override
public boolean isFinished() {
return tramp.isNoteStaged();
}

@Override
public void end(boolean interrupted) {
indexer.stopFeeder();
tramp.stop();
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
package org.team1540.robot2024.commands.shooter;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import org.team1540.robot2024.subsystems.shooter.Shooter;

class PrepareShooterCommand extends Command {
private final Shooter shooter;

public PrepareShooterCommand(Shooter shooter) {
this.shooter = shooter;
addRequirements(shooter);
}
@Override
public void execute() {
// TODO: Make this dynamically update based on estimated pose
shooter.setFlywheelSpeeds(1000, 1000);
shooter.setPivotPosition(Rotation2d.fromDegrees(30));
}

@Override
public boolean isFinished() {
return shooter.areFlywheelsSpunUp() && shooter.isPivotAtSetpoint();
}
}
Loading

0 comments on commit f942fee

Please sign in to comment.