diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 00000000..f3c87b90
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,2 @@
+src/main/deploy/** linguist-language=JSON linguist-generated=true
+**/*.java text=auto
\ No newline at end of file
diff --git a/.gitignore b/.gitignore
index 96a2b6b3..c541e0a7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -162,7 +162,7 @@ bin/
*.iml
*.ipr
*.iws
-.idea/
+
out/
# Fleet
diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 00000000..ebe0eedc
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,3 @@
+
+/!codeStyles
+/*.xml
\ No newline at end of file
diff --git a/.idea/codeStyles/Project.xml b/.idea/codeStyles/Project.xml
new file mode 100644
index 00000000..52cec35e
--- /dev/null
+++ b/.idea/codeStyles/Project.xml
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/codeStyles/codeStyleConfig.xml b/.idea/codeStyles/codeStyleConfig.xml
new file mode 100644
index 00000000..79ee123c
--- /dev/null
+++ b/.idea/codeStyles/codeStyleConfig.xml
@@ -0,0 +1,5 @@
+
+
+
+
+
\ No newline at end of file
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
index 0065f7a6..4d632b8c 100644
--- a/src/main/java/org/team1540/robot2024/Constants.java
+++ b/src/main/java/org/team1540/robot2024/Constants.java
@@ -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;
@@ -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;
@@ -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;
@@ -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 {
@@ -208,6 +209,7 @@ public enum ElevatorState {
AMP(27.0); //TODO: Find these values :D
public final double heightMeters;
+
ElevatorState(double heightMeters) {
this.heightMeters = heightMeters;
}
@@ -215,7 +217,7 @@ public enum ElevatorState {
}
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
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
index 0237920f..8c9d79d7 100644
--- a/src/main/java/org/team1540/robot2024/RobotContainer.java
+++ b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -8,7 +8,7 @@
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;
@@ -16,27 +16,28 @@
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;
@@ -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.
@@ -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;
@@ -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));
diff --git a/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java b/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java
index fc07d569..fd6f4e11 100644
--- a/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java
+++ b/src/main/java/org/team1540/robot2024/commands/FeedForwardCharacterization.java
@@ -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;
}
@@ -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));
}
}
}
diff --git a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java
index a595f2a0..e6589593 100644
--- a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java
+++ b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java
@@ -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
@@ -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(
diff --git a/src/main/java/org/team1540/robot2024/commands/TrampCommand.java b/src/main/java/org/team1540/robot2024/commands/TrampCommand.java
deleted file mode 100644
index a9cd6307..00000000
--- a/src/main/java/org/team1540/robot2024/commands/TrampCommand.java
+++ /dev/null
@@ -1,33 +0,0 @@
-package org.team1540.robot2024.commands;
-
-import org.team1540.robot2024.subsystems.tramp.Tramp;
-
-import edu.wpi.first.wpilibj2.command.Command;
-
-public class TrampCommand extends Command {
-
- private final Tramp tramp;
-
- public TrampCommand(Tramp tramp) {
- this.tramp = tramp;
- addRequirements(tramp);
- }
-
- @Override
- public void initialize() {
- tramp.setPercent(0.5);
- }
-
- @Override
- public void execute() {}
-
- @Override
- public boolean isFinished() {
-return tramp.isBeamBreakBlocked();
- }
-
- @Override
- public void end(boolean interrupted) {
- tramp.stopTramp();
- }
-}
diff --git a/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java
index aac564ec..f1a1d33b 100644
--- a/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java
+++ b/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java
@@ -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)
);
}
}
diff --git a/src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java b/src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java
index 03ec8429..9e92ad93 100644
--- a/src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java
+++ b/src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java
@@ -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
diff --git a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java
index af06e527..7b91bf67 100644
--- a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java
+++ b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java
@@ -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 {
@@ -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
diff --git a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java
index ba5c3c39..836345ba 100644
--- a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java
+++ b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorSetpointCommand.java
@@ -13,6 +13,7 @@ public ElevatorSetpointCommand(Elevator elevator, ElevatorState state) {
this.state = state;
addRequirements(elevator);
}
+
@Override
public void initialize() {
elevator.setElevatorPosition(state.heightMeters);
@@ -22,6 +23,7 @@ public void initialize() {
public void end(boolean interrupted) {
elevator.stop();
}
+
@Override
public boolean isFinished() {
return elevator.isAtSetpoint();
diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java
index 72df4da5..22e6f86c 100644
--- a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java
+++ b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java
@@ -20,7 +20,7 @@ public void initialize() {
@Override
public boolean isFinished() {
- return indexer.isNotePresent();
+ return indexer.isNoteStaged();
}
@Override
diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java
new file mode 100644
index 00000000..0112797d
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/indexer/StageTrampCommand.java
@@ -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();
+ }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java
new file mode 100644
index 00000000..8d757fb8
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java
@@ -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();
+ }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java
new file mode 100644
index 00000000..d8a969da
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/shooter/ShootSequence.java
@@ -0,0 +1,26 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+
+public class ShootSequence extends SequentialCommandGroup {
+ public ShootSequence(Shooter shooter, Indexer indexer) {
+ addCommands(
+ Commands.parallel(
+ indexer.feedToShooter(),
+ new PrepareShooterCommand(shooter)
+ ),
+ Commands.runOnce(() -> indexer.setIntakePercent(1), indexer)
+ // TODO: Add a wait for having completed the shot (steady then current spike/velocity dip and then back down?)
+ );
+ }
+
+
+ @Override
+ public InterruptionBehavior getInterruptionBehavior() {
+ return InterruptionBehavior.kCancelIncoming;
+ }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java
new file mode 100644
index 00000000..b963ab5e
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/shooter/TuneShooterCommand.java
@@ -0,0 +1,28 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+
+public class TuneShooterCommand extends Command {
+ private final LoggedDashboardNumber leftFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/leftSetpoint", 6000);
+ private final LoggedDashboardNumber rightFlywheelSetpoint = new LoggedDashboardNumber("Shooter/Flywheels/rightSetpoint", 6000);
+ private final LoggedDashboardNumber angleSetpoint = new LoggedDashboardNumber("Shooter/Pivot/angleSetpoint", 30);
+ private final Shooter shooter;
+
+ public TuneShooterCommand(Shooter shooter) {
+ this.shooter = shooter;
+ addRequirements(shooter);
+ }
+
+ public void execute() {
+ shooter.setFlywheelSpeeds(leftFlywheelSetpoint.get(), rightFlywheelSetpoint.get());
+ shooter.setPivotPosition(Rotation2d.fromDegrees(angleSetpoint.get()));
+ }
+
+ public void end() {
+ shooter.stopFlywheels();
+ shooter.stopPivot();
+ }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
index 91333edc..373d06fa 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
@@ -16,9 +16,9 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import org.team1540.robot2024.util.LocalADStarAK;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.util.LocalADStarAK;
import org.team1540.robot2024.util.vision.TimestampedVisionPose;
import static org.team1540.robot2024.Constants.Drivetrain.*;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIONavx.java b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIONavx.java
index 5a7b8721..5cf0b7ec 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIONavx.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIONavx.java
@@ -5,11 +5,11 @@
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Timer;
-public class GyroIONavx implements GyroIO{
+public class GyroIONavx implements GyroIO {
private final AHRS navx = new AHRS(SPI.Port.kMXP);
private Rotation2d lastAngle;
- public GyroIONavx(){
+ public GyroIONavx() {
lastAngle = navx.getRotation2d();
}
@@ -20,7 +20,7 @@ public void updateInputs(GyroIOInputs inputs) {
inputs.time = Timer.getFPGATimestamp();
inputs.connected = navx.isConnected();
inputs.yawPosition = angle;
- inputs.yawVelocityRadPerSec = (angle.minus(lastAngle).getRadians())/(inputs.time - lastTime);
+ inputs.yawVelocityRadPerSec = (angle.minus(lastAngle).getRadians()) / (inputs.time - lastTime);
lastAngle = angle;
}
}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java
index 37add182..7510f41e 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/GyroIOPigeon2.java
@@ -8,7 +8,7 @@
import edu.wpi.first.math.util.Units;
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
-import static org.team1540.robot2024.Constants.SwerveConfig.*;
+import static org.team1540.robot2024.Constants.SwerveConfig.PIGEON_ID;
/**
* IO implementation for Pigeon2
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
index 4f8cdb36..c401b1b5 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
@@ -5,10 +5,11 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
-import org.team1540.robot2024.Constants;
import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.Constants;
-import static org.team1540.robot2024.Constants.Drivetrain.*;
+import static org.team1540.robot2024.Constants.Drivetrain.MAX_LINEAR_SPEED;
+import static org.team1540.robot2024.Constants.Drivetrain.WHEEL_RADIUS;
public class Module {
private final ModuleIO io;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java
index 2e261ea3..a77662ac 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java
@@ -5,8 +5,9 @@
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
-import static org.team1540.robot2024.Constants.Drivetrain.*;
-import static org.team1540.robot2024.Constants.*;
+import static org.team1540.robot2024.Constants.Drivetrain.DRIVE_GEAR_RATIO;
+import static org.team1540.robot2024.Constants.Drivetrain.TURN_GEAR_RATIO;
+import static org.team1540.robot2024.Constants.LOOP_PERIOD_SECS;
/**
* Physics sim implementation of module IO.
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java
index bd3a4cba..f80d1aef 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java
@@ -13,7 +13,8 @@
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
import org.team1540.robot2024.util.swerve.SwerveFactory;
-import static org.team1540.robot2024.Constants.Drivetrain.*;
+import static org.team1540.robot2024.Constants.Drivetrain.DRIVE_GEAR_RATIO;
+import static org.team1540.robot2024.Constants.Drivetrain.IS_TURN_MOTOR_INVERTED;
/**
* Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java
index 28616a6a..e63e0d78 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java
@@ -1,19 +1,14 @@
package org.team1540.robot2024.subsystems.elevator;
import edu.wpi.first.math.MathUtil;
-
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
import org.team1540.robot2024.Constants;
import org.team1540.robot2024.util.MechanismVisualiser;
import org.team1540.robot2024.util.math.AverageFilter;
-import com.ctre.phoenix6.Utils;
-
-import static org.team1540.robot2024.Constants.Elevator.*;
+import static org.team1540.robot2024.Constants.Elevator.POS_ERR_TOLERANCE_METERS;
public class Elevator extends SubsystemBase {
private final ElevatorIO io;
@@ -27,14 +22,14 @@ public Elevator(ElevatorIO elevatorIO) {
}
// periodic
- public void periodic(){
+ public void periodic() {
io.updateInputs(inputs);
Logger.processInputs("Elevator", inputs);
MechanismVisualiser.setElevatorPosition(inputs.positionMeters);
positionFilter.add(inputs.positionMeters);
}
-
+
public void setElevatorPosition(double positionMeters) {
positionMeters = MathUtil.clamp(positionMeters, Constants.Elevator.ELEVATOR_MINIMUM_HEIGHT, Constants.Elevator.ELEVATOR_MAX_HEIGHT);
setpointMeters = positionMeters;
@@ -44,7 +39,7 @@ public void setElevatorPosition(double positionMeters) {
}
public boolean isAtSetpoint() {
- return MathUtil.isNear(setpointMeters, positionFilter.getAverage(), ERROR_TOLERANCE);
+ return MathUtil.isNear(setpointMeters, positionFilter.getAverage(), POS_ERR_TOLERANCE_METERS);
}
public void setVoltage(double voltage) {
diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java
index f0c242bb..84117a11 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java
@@ -8,7 +8,7 @@ class ElevatorIOInputs {
public double positionMeters = 0.0;
public double velocityMPS = 0.0;
public double voltage = 0.0;
- public double[] current = new double[] {};
+ public double[] current = new double[]{};
public boolean atUpperLimit = false;
public boolean atLowerLimit = false;
}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java
index 55cd8d1a..07a27024 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java
@@ -6,16 +6,17 @@
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+
import static org.team1540.robot2024.Constants.Elevator.*;
import static org.team1540.robot2024.Constants.LOOP_PERIOD_SECS;
-public class ElevatorIOSim implements ElevatorIO{
+public class ElevatorIOSim implements ElevatorIO {
// fields
private final ElevatorSim elevatorSim =
new ElevatorSim(
- DCMotor.getFalcon500Foc(2),
+ DCMotor.getFalcon500Foc(2),
GEAR_RATIO, SIM_CARRIAGE_MASS_KG,
- SPROCKET_RADIUS_M,ELEVATOR_MINIMUM_HEIGHT,
+ SPROCKET_RADIUS_M, ELEVATOR_MINIMUM_HEIGHT,
ELEVATOR_MAX_HEIGHT,
true,
ELEVATOR_MINIMUM_HEIGHT);
@@ -31,7 +32,7 @@ public class ElevatorIOSim implements ElevatorIO{
private TrapezoidProfile.State setpoint;
@Override
- public void updateInputs(ElevatorIOInputs inputs){
+ public void updateInputs(ElevatorIOInputs inputs) {
if (isClosedLoop) {
elevatorAppliedVolts =
controller.calculate(elevatorSim.getPositionMeters(), setpoint)
@@ -55,6 +56,7 @@ public void setVoltage(double volts) {
isClosedLoop = false;
elevatorAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
}
+
@Override
public void setSetpointMeters(double position) {
isClosedLoop = true;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java
index fcecef4a..862a3135 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java
@@ -1,7 +1,5 @@
package org.team1540.robot2024.subsystems.elevator;
-import org.team1540.robot2024.Constants;
-
import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.MotionMagicConfigs;
@@ -14,6 +12,7 @@
import com.ctre.phoenix6.signals.GravityTypeValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.ReverseLimitValue;
+import org.team1540.robot2024.Constants;
public class ElevatorIOTalonFX implements ElevatorIO {
@@ -41,8 +40,14 @@ public ElevatorIOTalonFX() {
// TODO: this might not actually be inverted, so fix it if neccesary
follower.setControl(new Follower(leader.getDeviceID(), true));
- BaseStatusSignal.setUpdateFrequencyForAll(
- 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent, topLimitStatus, bottomLimitStatus);
+ BaseStatusSignal.setUpdateFrequencyForAll(50.0,
+ leaderPosition,
+ leaderVelocity,
+ leaderAppliedVolts,
+ leaderCurrent,
+ followerCurrent,
+ topLimitStatus,
+ bottomLimitStatus);
leader.optimizeBusUtilization();
follower.optimizeBusUtilization();
@@ -84,7 +89,7 @@ public void updateInputs(ElevatorIOInputs inputs) {
inputs.positionMeters = leaderPosition.getValue();
inputs.velocityMPS = leaderVelocity.getValue();
inputs.voltage = leaderAppliedVolts.getValue();
- inputs.current = new double[] {leaderCurrent.getValue(), followerCurrent.getValue()};
+ inputs.current = new double[]{leaderCurrent.getValue(), followerCurrent.getValue()};
inputs.atUpperLimit = topLimitStatus.getValue() == ForwardLimitValue.ClosedToGround;
inputs.atLowerLimit = bottomLimitStatus.getValue() == ReverseLimitValue.ClosedToGround;
}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java
index f5a8804d..b9853233 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java
@@ -3,6 +3,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
//TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D
public class Hooks extends SubsystemBase {
@@ -21,6 +22,7 @@ public void undeployHooks() {
public Command deployHooksCommand() {
return new InstantCommand(this::deployHooks, this);
}
+
/**
* Factory method for undeploying hooks
*/
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
index 37afbc62..41dc8eee 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -5,47 +5,59 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;
import org.team1540.robot2024.util.LoggedTunableNumber;
+
import static org.team1540.robot2024.Constants.Indexer.*;
import static org.team1540.robot2024.Constants.tuningMode;
public class Indexer extends SubsystemBase {
- private final IndexerIO indexerIO;
- private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged();
+ private final IndexerIO io;
+ private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged();
private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP);
private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI);
private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD);
- public Indexer(IndexerIO indexerIO) {
- this.indexerIO = indexerIO;
+ public Indexer(IndexerIO io) {
+ this.io = io;
}
public void periodic() {
- indexerIO.updateInputs(indexerInputs);
- Logger.processInputs("Indexer", indexerInputs);
+ io.updateInputs(inputs);
+ Logger.processInputs("Indexer", inputs);
if (tuningMode) {
if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
- indexerIO.configureFeederPID(kP.get(), kI.get(), kD.get());
+ io.configureFeederPID(kP.get(), kI.get(), kD.get());
}
}
}
public void setIntakePercent(double percent) {
- indexerIO.setIntakeVoltage(percent * 12.0);
+ io.setIntakeVoltage(percent * 12.0);
+ }
+
+ public boolean isNoteStaged() {
+ return inputs.noteInIntake;
}
- public boolean isNotePresent() {
- return indexerInputs.noteInIntake;
+ public void setFeederVelocity(double setpointRPM) {
+ io.setFeederVelocity(setpointRPM);
}
public Command feedToAmp() {
- return Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this);
+ return Commands.runOnce(() -> io.setFeederVelocity(-600), this);
}
public Command feedToShooter() {
- return Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this);
+ return Commands.runOnce(() -> io.setFeederVelocity(1200), this);
}
+ // TODO: Add method to check if feeder is spun up
+ public void stopFeeder() {
+ io.setFeederVoltage(0);
+ }
+ public void stopIntake() {
+ io.setIntakeVoltage(0);
+ }
}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
index ea11766f..59e564b3 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
@@ -4,7 +4,6 @@
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
-import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
import org.team1540.robot2024.Constants;
import static org.team1540.robot2024.Constants.Indexer.*;
@@ -14,8 +13,8 @@ public class IndexerIOSim implements IndexerIO {
private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO, INTAKE_MOI);
private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO, FEEDER_MOI);
-// private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
- private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD);
+ // private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
+ private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI, FEEDER_KD);
private boolean isClosedLoop = true;
private double intakeVoltage = 0.0;
private double feederVoltage = 0.0;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
index 18c9686b..1922ea27 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
@@ -1,6 +1,9 @@
package org.team1540.robot2024.subsystems.indexer;
-import com.revrobotics.*;
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.DigitalInput;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java
index 947a6d92..24f27a87 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOSim.java
@@ -8,7 +8,7 @@
import static org.team1540.robot2024.Constants.Shooter.Flywheels.*;
-public class FlywheelsIOSim implements FlywheelsIO{
+public class FlywheelsIOSim implements FlywheelsIO {
private final FlywheelSim leftSim =
new FlywheelSim(DCMotor.getFalcon500(1), GEAR_RATIO, SIM_MOI);
private final FlywheelSim rightSim =
@@ -30,10 +30,10 @@ public void updateInputs(FlywheelsIOInputs inputs) {
if (isClosedLoop) {
leftVolts =
leftController.calculate(leftSim.getAngularVelocityRPM() / 60, leftSetpointRPS)
- + feedforward.calculate(leftSetpointRPS);
+ + feedforward.calculate(leftSetpointRPS);
rightVolts =
rightController.calculate(rightSim.getAngularVelocityRPM() / 60, rightSetpointRPS)
- + feedforward.calculate(rightSetpointRPS);
+ + feedforward.calculate(rightSetpointRPS);
}
leftSim.setInputVoltage(leftVolts);
diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java
index 5639305c..65296bfa 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java
@@ -2,12 +2,17 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj2.command.*;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;
import org.team1540.robot2024.util.LoggedTunableNumber;
import org.team1540.robot2024.util.math.AverageFilter;
-import static org.team1540.robot2024.Constants.Shooter.*;
+import java.util.function.Supplier;
+
+import static org.team1540.robot2024.Constants.Shooter.Flywheels;
+import static org.team1540.robot2024.Constants.Shooter.Pivot;
public class Shooter extends SubsystemBase {
private final ShooterPivotIO pivotIO;
@@ -147,7 +152,7 @@ public Rotation2d getPivotPosition() {
public boolean areFlywheelsSpunUp() {
return
MathUtil.isNear(leftFlywheelSetpointRPM, leftSpeedFilter.getAverage(), Flywheels.ERROR_TOLERANCE_RPM) &&
- MathUtil.isNear(rightFlywheelSetpointRPM, rightSpeedFilter.getAverage(), Flywheels.ERROR_TOLERANCE_RPM);
+ MathUtil.isNear(rightFlywheelSetpointRPM, rightSpeedFilter.getAverage(), Flywheels.ERROR_TOLERANCE_RPM);
}
/**
@@ -158,9 +163,9 @@ public boolean isPivotAtSetpoint() {
pivotSetpoint.getRotations(), pivotPositionFilter.getAverage(), Pivot.ERROR_TOLERANCE.getRotations());
}
- public Command spinUpCommand(double leftSetpoint, double rightSetpoint) {
+ public Command spinUpCommand(Supplier leftSetpoint, Supplier rightSetpoint) {
return new FunctionalCommand(
- () -> setFlywheelSpeeds(leftSetpoint, rightSetpoint),
+ () -> setFlywheelSpeeds(leftSetpoint.get(), rightSetpoint.get()),
() -> {},
(ignored) -> {},
this::areFlywheelsSpunUp,
@@ -175,4 +180,5 @@ public Command setPivotPositionCommand(Rotation2d setpoint) {
this::isPivotAtSetpoint,
this);
}
+
}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java
index 7c8d0cb4..64a79449 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java
@@ -37,7 +37,7 @@ public void updateInputs(ShooterPivotIOInputs inputs) {
if (isClosedLoop) {
appliedVolts =
controller.calculate(Units.radiansToRotations(sim.getAngleRads()), goalState)
- + feedforward.calculate(
+ + feedforward.calculate(
Units.rotationsToRadians(controller.getSetpoint().position),
controller.getSetpoint().velocity);
}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java
index 683b2503..dde5e1cf 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java
@@ -1,6 +1,5 @@
package org.team1540.robot2024.subsystems.tramp;
-import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;
@@ -16,7 +15,7 @@ public void setPercent(double percentage) {
io.setVoltage(12.0 * percentage);
}
- public boolean isBeamBreakBlocked() {
+ public boolean isNoteStaged() {
return inputs.breamBreakTripped;
}
@@ -26,7 +25,7 @@ public void periodic() {
Logger.processInputs("Tramp", inputs);
}
- public void stopTramp() {
+ public void stop() {
io.setVoltage(0);
}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java
index e0b4fdd6..fd718ccd 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java
@@ -7,10 +7,11 @@
import static org.team1540.robot2024.Constants.Tramp.GEAR_RATIO;
-public class TrampIOSim implements TrampIO{
- private final DCMotorSim sim = new DCMotorSim(DCMotor.getNEO(1), GEAR_RATIO,0.025); //TODO: Fix MOI its probably wrong :D
+public class TrampIOSim implements TrampIO {
+ private final DCMotorSim sim = new DCMotorSim(DCMotor.getNEO(1), GEAR_RATIO, 0.025); //TODO: Fix MOI its probably wrong :D
private double appliedVolts = 0.0;
+
@Override
public void updateInputs(TrampIOInputs inputs) {
sim.update(Constants.LOOP_PERIOD_SECS);
diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
index dfef09ff..7b91cddf 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
@@ -11,12 +11,14 @@ public class TrampIOSparkMax implements TrampIO {
//TODO: Potentially change name :D
private final CANSparkMax motor = new CANSparkMax(Tramp.TRAMP_MOTOR_ID, MotorType.kBrushless);
private final RelativeEncoder motorEncoder = motor.getEncoder();
+
public TrampIOSparkMax() {
motor.setIdleMode(CANSparkMax.IdleMode.kBrake);
//Potentially invert motor
motor.setSmartCurrentLimit(40);
motor.enableVoltageCompensation(12);
}
+
@Override
public void setVoltage(double volts) {
motor.setVoltage(volts);
diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java
index fe748ccb..02068e9d 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java
@@ -12,7 +12,8 @@
import java.util.function.Consumer;
import java.util.function.Supplier;
-import static org.team1540.robot2024.Constants.Vision.*;
+import static org.team1540.robot2024.Constants.Vision.FRONT_CAMERA_POSE;
+import static org.team1540.robot2024.Constants.Vision.REAR_CAMERA_POSE;
public class AprilTagVision extends SubsystemBase {
private final AprilTagVisionIO frontCameraIO;
diff --git a/src/main/java/org/team1540/robot2024/util/LocalADStarAK.java b/src/main/java/org/team1540/robot2024/util/LocalADStarAK.java
index 4a9e30f9..008d6ff1 100644
--- a/src/main/java/org/team1540/robot2024/util/LocalADStarAK.java
+++ b/src/main/java/org/team1540/robot2024/util/LocalADStarAK.java
@@ -8,15 +8,14 @@
import com.pathplanner.lib.pathfinding.Pathfinder;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Translation2d;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
-import org.littletonrobotics.junction.LogTable;
-import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.inputs.LoggableInputs;
-
// NOTE: This file is available at
// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d
diff --git a/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java
index e9fc1aa7..1feeb5fc 100644
--- a/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java
+++ b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java
@@ -1,10 +1,11 @@
package org.team1540.robot2024.util;
-import java.util.HashMap;
-import java.util.Map;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.robot2024.Constants;
+import java.util.HashMap;
+import java.util.Map;
+
// NOTE: This file is available at
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/main/src/main/java/org/littletonrobotics/frc2023/util/LoggedTunableNumber.java
@@ -19,7 +20,7 @@ public class LoggedTunableNumber {
private boolean hasDefault = false;
private double defaultValue;
private LoggedDashboardNumber dashboardNumber;
- private Map lastHasChangedValues = new HashMap<>();
+ private final Map lastHasChangedValues = new HashMap<>();
/**
* Create a new LoggedTunableNumber
@@ -73,9 +74,9 @@ public double get() {
* Checks whether the number has changed since our last check
*
* @param id Unique identifier for the caller to avoid conflicts when shared between multiple
- * objects. Recommended approach is to pass the result of "hashCode()"
+ * objects. Recommended approach is to pass the result of "hashCode()"
* @return True if the number has changed since the last time this method was called, false
- * otherwise.
+ * otherwise.
*/
public boolean hasChanged(int id) {
if (!Constants.tuningMode) return false;
diff --git a/src/main/java/org/team1540/robot2024/util/math/AverageFilter.java b/src/main/java/org/team1540/robot2024/util/math/AverageFilter.java
index fd47b0d0..fe429f1d 100644
--- a/src/main/java/org/team1540/robot2024/util/math/AverageFilter.java
+++ b/src/main/java/org/team1540/robot2024/util/math/AverageFilter.java
@@ -26,7 +26,7 @@ public void add(double value) {
}
public double getAverage() {
- return sum/(double) items.size();
+ return sum / (double) items.size();
}
}
diff --git a/src/main/java/org/team1540/robot2024/util/math/PolynomialRegression.java b/src/main/java/org/team1540/robot2024/util/math/PolynomialRegression.java
index ecf50fd7..8ecd01cc 100644
--- a/src/main/java/org/team1540/robot2024/util/math/PolynomialRegression.java
+++ b/src/main/java/org/team1540/robot2024/util/math/PolynomialRegression.java
@@ -25,8 +25,8 @@
public class PolynomialRegression implements Comparable {
private final String variableName; // name of the predictor variable
private int degree; // degree of the polynomial regression
- private Matrix beta; // the polynomial regression coefficients
- private double sse; // sum of squares due to error
+ private final Matrix beta; // the polynomial regression coefficients
+ private final double sse; // sum of squares due to error
private double sst; // total sum of squares
/**
diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
index 345120c0..30ef01ba 100644
--- a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
+++ b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
@@ -9,8 +9,8 @@
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
-import static org.team1540.robot2024.Constants.SwerveConfig.CAN_BUS;
import static org.team1540.robot2024.Constants.Drivetrain.TURN_GEAR_RATIO;
+import static org.team1540.robot2024.Constants.SwerveConfig.CAN_BUS;
public class SwerveFactory {
@@ -36,6 +36,7 @@ public enum SwerveCorner {
BACK_RIGHT(0.5);
private final double offsetRots;
+
SwerveCorner(double offsetRots) {
this.offsetRots = offsetRots;
}
@@ -76,7 +77,7 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
turnConfig.Feedback.SensorToMechanismRatio = 1.0;
turnConfig.Feedback.RotorToSensorRatio = TURN_GEAR_RATIO;
- canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id-1] + corner.offsetRots;
+ canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id - 1] + corner.offsetRots;
canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
canCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1;
diff --git a/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java
index 9021f037..8100ac41 100644
--- a/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java
+++ b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java
@@ -1,15 +1,16 @@
package org.team1540.robot2024.util.vision;
+import com.fasterxml.jackson.annotation.JsonFormat;
+import com.fasterxml.jackson.annotation.JsonFormat.Shape;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationFeature;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation2d;
import java.io.IOException;
import java.net.HttpURLConnection;
@@ -17,13 +18,6 @@
import java.net.URL;
import java.util.concurrent.CompletableFuture;
-import com.fasterxml.jackson.annotation.JsonFormat;
-import com.fasterxml.jackson.annotation.JsonFormat.Shape;
-import com.fasterxml.jackson.annotation.JsonProperty;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.DeserializationFeature;
-import com.fasterxml.jackson.databind.ObjectMapper;
-
// NOTE: This file is available at
// https://github.com/LimelightVision/limelightlib-wpijava/blob/main/LimelightHelpers.java
@@ -38,7 +32,7 @@ public static class LimelightTarget_Retro {
private double[] robotPose_FieldSpace;
@JsonProperty("t6r_ts")
- private double[] robotPose_TargetSpace;
+ private double[] robotPose_TargetSpace;
@JsonProperty("t6t_cs")
private double[] targetPose_CameraSpace;
@@ -46,45 +40,43 @@ public static class LimelightTarget_Retro {
@JsonProperty("t6t_rs")
private double[] targetPose_RobotSpace;
- public Pose3d getCameraPose_TargetSpace()
- {
+ public Pose3d getCameraPose_TargetSpace() {
return toPose3D(cameraPose_TargetSpace);
}
- public Pose3d getRobotPose_FieldSpace()
- {
+
+ public Pose3d getRobotPose_FieldSpace() {
return toPose3D(robotPose_FieldSpace);
}
- public Pose3d getRobotPose_TargetSpace()
- {
+
+ public Pose3d getRobotPose_TargetSpace() {
return toPose3D(robotPose_TargetSpace);
}
- public Pose3d getTargetPose_CameraSpace()
- {
+
+ public Pose3d getTargetPose_CameraSpace() {
return toPose3D(targetPose_CameraSpace);
}
- public Pose3d getTargetPose_RobotSpace()
- {
+
+ public Pose3d getTargetPose_RobotSpace() {
return toPose3D(targetPose_RobotSpace);
}
- public Pose2d getCameraPose_TargetSpace2D()
- {
+ public Pose2d getCameraPose_TargetSpace2D() {
return toPose2D(cameraPose_TargetSpace);
}
- public Pose2d getRobotPose_FieldSpace2D()
- {
+
+ public Pose2d getRobotPose_FieldSpace2D() {
return toPose2D(robotPose_FieldSpace);
}
- public Pose2d getRobotPose_TargetSpace2D()
- {
+
+ public Pose2d getRobotPose_TargetSpace2D() {
return toPose2D(robotPose_TargetSpace);
}
- public Pose2d getTargetPose_CameraSpace2D()
- {
+
+ public Pose2d getTargetPose_CameraSpace2D() {
return toPose2D(targetPose_CameraSpace);
}
- public Pose2d getTargetPose_RobotSpace2D()
- {
+
+ public Pose2d getTargetPose_RobotSpace2D() {
return toPose2D(targetPose_RobotSpace);
}
@@ -139,45 +131,43 @@ public static class LimelightTarget_Fiducial {
@JsonProperty("t6t_rs")
private double[] targetPose_RobotSpace;
- public Pose3d getCameraPose_TargetSpace()
- {
+ public Pose3d getCameraPose_TargetSpace() {
return toPose3D(cameraPose_TargetSpace);
}
- public Pose3d getRobotPose_FieldSpace()
- {
+
+ public Pose3d getRobotPose_FieldSpace() {
return toPose3D(robotPose_FieldSpace);
}
- public Pose3d getRobotPose_TargetSpace()
- {
+
+ public Pose3d getRobotPose_TargetSpace() {
return toPose3D(robotPose_TargetSpace);
}
- public Pose3d getTargetPose_CameraSpace()
- {
+
+ public Pose3d getTargetPose_CameraSpace() {
return toPose3D(targetPose_CameraSpace);
}
- public Pose3d getTargetPose_RobotSpace()
- {
+
+ public Pose3d getTargetPose_RobotSpace() {
return toPose3D(targetPose_RobotSpace);
}
- public Pose2d getCameraPose_TargetSpace2D()
- {
+ public Pose2d getCameraPose_TargetSpace2D() {
return toPose2D(cameraPose_TargetSpace);
}
- public Pose2d getRobotPose_FieldSpace2D()
- {
+
+ public Pose2d getRobotPose_FieldSpace2D() {
return toPose2D(robotPose_FieldSpace);
}
- public Pose2d getRobotPose_TargetSpace2D()
- {
+
+ public Pose2d getRobotPose_TargetSpace2D() {
return toPose2D(robotPose_TargetSpace);
}
- public Pose2d getTargetPose_CameraSpace2D()
- {
+
+ public Pose2d getTargetPose_CameraSpace2D() {
return toPose2D(targetPose_CameraSpace);
}
- public Pose2d getTargetPose_RobotSpace2D()
- {
+
+ public Pose2d getTargetPose_RobotSpace2D() {
return toPose2D(targetPose_RobotSpace);
}
@@ -238,7 +228,7 @@ public static class LimelightTarget_Classifier {
@JsonProperty("typ")
public double ty_pixels;
- public LimelightTarget_Classifier() {
+ public LimelightTarget_Classifier() {
}
}
@@ -383,9 +373,8 @@ static final String sanitizeName(String name) {
return name;
}
- private static Pose3d toPose3D(double[] inData){
- if(inData.length < 6)
- {
+ private static Pose3d toPose3D(double[] inData) {
+ if (inData.length < 6) {
System.err.println("Bad LL 3D Pose Data!");
return new Pose3d();
}
@@ -395,9 +384,8 @@ private static Pose3d toPose3D(double[] inData){
Units.degreesToRadians(inData[5])));
}
- private static Pose2d toPose2D(double[] inData){
- if(inData.length < 6)
- {
+ private static Pose2d toPose2D(double[] inData) {
+ if (inData.length < 6) {
System.err.println("Bad LL 2D Pose Data!");
return new Pose2d();
}
@@ -667,6 +655,7 @@ public static void setStreamMode_PiPSecondary(String limelightName) {
public static void setCameraMode_Processor(String limelightName) {
setLimelightNTDouble(limelightName, "camMode", 0);
}
+
public static void setCameraMode_Driver(String limelightName) {
setLimelightNTDouble(limelightName, "camMode", 1);
}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 1cb55125..f8927312 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -39,4 +39,4 @@
}
],
"cppDependencies": []
-}
+}
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
index 16a48b0b..cae13633 100644
--- a/vendordeps/PathplannerLib.json
+++ b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
{
"fileName": "PathplannerLib.json",
"name": "PathplannerLib",
- "version": "2024.1.3",
+ "version": "2024.1.6",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2024",
"mavenUrls": [
@@ -12,7 +12,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
- "version": "2024.1.3"
+ "version": "2024.1.6"
}
],
"jniDependencies": [],
@@ -20,7 +20,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
- "version": "2024.1.3",
+ "version": "2024.1.6",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -35,4 +35,4 @@
]
}
]
-}
+}
\ No newline at end of file