Skip to content

Commit 6cac836

Browse files
Brainiac11Brainiac11
committed
GOT A LOT DONE
1 parent 3bc1a2c commit 6cac836

34 files changed

+2261
-733
lines changed

‎.vscode/launch.json‎

Lines changed: 23 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,38 @@
44
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
55
"version": "0.2.0",
66
"configurations": [
7-
7+
{
8+
"type": "java",
9+
"name": "AdjustedParabolic",
10+
"request": "launch",
11+
"mainClass": "team5427.lib.kinematics.shooter.projectiles.parabolic.AdjustedParabolic",
12+
"projectName": "SteelTalonsLibrary"
13+
},
14+
{
15+
"type": "java",
16+
"name": "AdjustedParabolic",
17+
"request": "launch",
18+
"mainClass": "team5427.lib.kinematics.shooter.projectiles.AdjustedParabolic",
19+
"projectName": "SteelTalonsLibrary"
20+
},
21+
{
22+
"type": "java",
23+
"name": "MagnusProjectileSimulation",
24+
"request": "launch",
25+
"mainClass": "team5427.lib.kinematics.shooter.projectiles.MagnusProjectileSimulation",
26+
"projectName": "SteelTalonsLibrary"
27+
},
828
{
929
"type": "wpilib",
1030
"name": "WPILib Desktop Debug",
1131
"request": "launch",
12-
"desktop": true,
32+
"desktop": true
1333
},
1434
{
1535
"type": "wpilib",
1636
"name": "WPILib roboRIO Debug",
1737
"request": "launch",
18-
"desktop": false,
38+
"desktop": false
1939
}
2040
]
2141
}

‎.vscode/settings.json‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,5 +56,6 @@
5656
"edu.wpi.first.math.proto.*",
5757
"edu.wpi.first.math.**.proto.*",
5858
"edu.wpi.first.math.**.struct.*",
59-
]
59+
],
60+
"java.debug.settings.onBuildFailureProceed": true
6061
}

‎build.gradle‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -123,6 +123,7 @@ dependencies {
123123
implementation wpi.java.deps.wpilib()
124124
implementation wpi.java.vendor.java()
125125

126+
126127
roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
127128
roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
128129

@@ -147,6 +148,7 @@ dependencies {
147148
annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
148149

149150
implementation 'org.team4206:battleaid:0.4.0'
151+
implementation 'org.apache.commons:commons-math3:3.6.1' // Or the latest version
150152
}
151153

152154
test {

‎src/main/java/frc/robot/BuildConstants.java‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,12 @@ public final class BuildConstants {
77
public static final String MAVEN_GROUP = "";
88
public static final String MAVEN_NAME = "SteelTalonsLibrary";
99
public static final String VERSION = "unspecified";
10-
public static final int GIT_REVISION = 6;
11-
public static final String GIT_SHA = "5be2e4cf20fcbbd683812a871aa6ac7d26a62317";
12-
public static final String GIT_DATE = "2025-04-12 16:18:18 CDT";
13-
public static final String GIT_BRANCH = "main";
14-
public static final String BUILD_DATE = "2025-04-24 16:26:26 CDT";
15-
public static final long BUILD_UNIX_TIME = 1745529986093L;
10+
public static final int GIT_REVISION = 8;
11+
public static final String GIT_SHA = "3bc1a2c13d6f2883e96febe185076f30b2cd96dd";
12+
public static final String GIT_DATE = "2025-04-24 19:49:20 CDT";
13+
public static final String GIT_BRANCH = "HEAD";
14+
public static final String BUILD_DATE = "2025-05-20 14:20:43 CDT";
15+
public static final long BUILD_UNIX_TIME = 1747768843547L;
1616
public static final int DIRTY = 1;
1717

1818
private BuildConstants(){}

‎src/main/java/team5427/frc/robot/Robot.java‎

Lines changed: 31 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,35 @@
11
package team5427.frc.robot;
22

33
import com.pathplanner.lib.commands.FollowPathCommand;
4+
5+
import edu.wpi.first.math.MathUtil;
6+
import edu.wpi.first.math.geometry.Pose3d;
7+
import edu.wpi.first.math.geometry.Rotation2d;
8+
import edu.wpi.first.math.geometry.Translation2d;
9+
import edu.wpi.first.math.geometry.Translation3d;
10+
import edu.wpi.first.units.measure.LinearVelocity;
411
import edu.wpi.first.wpilibj.RobotBase;
512
import edu.wpi.first.wpilibj2.command.Command;
613
import edu.wpi.first.wpilibj2.command.CommandScheduler;
714
import frc.robot.BuildConstants;
15+
16+
import static edu.wpi.first.units.Units.MetersPerSecond;
17+
18+
import org.apache.commons.math3.util.MathUtils;
819
import org.littletonrobotics.junction.AutoLogOutputManager;
920
import org.littletonrobotics.junction.LogFileUtil;
1021
import org.littletonrobotics.junction.LoggedRobot;
1122
import org.littletonrobotics.junction.Logger;
1223
import org.littletonrobotics.junction.networktables.NT4Publisher;
1324
import org.littletonrobotics.junction.wpilog.WPILOGReader;
1425
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
26+
import team5427.frc.robot.subsystems.vision.QuestNav;
27+
import team5427.lib.detection.tuples.Tuple3Plus;
1528
// import team5427.frc.robot.subsystems.ProngEffector.ProngSubsystem;
1629
import team5427.lib.drivers.SteelTalonsLogger;
1730
import team5427.lib.drivers.VirtualSubsystem;
31+
import team5427.lib.kinematics.shooter.projectiles.parabolic.AdjustedParabolic;
32+
import team5427.lib.kinematics.shooter.projectiles.parabolic.AdjustedParabolicThread;
1833

1934
// import frc.robot.BuildConstants;
2035

@@ -85,6 +100,9 @@ public Robot() {
85100
m_robotContainer = new RobotContainer();
86101

87102
FollowPathCommand.warmupCommand().schedule();
103+
AdjustedParabolicThread.getInstance().setShouldCompute(true);
104+
AdjustedParabolicThread.getInstance().start();
105+
88106
}
89107

90108
/**
@@ -108,7 +126,16 @@ public void robotPeriodic() {
108126
CommandScheduler.getInstance().run();
109127
VirtualSubsystem.periodicAll();
110128
RobotState.getInstance().log();
111-
129+
QuestNav.getInstance().processHeartbeat();
130+
QuestNav.getInstance().cleanupResponses();
131+
Translation3d target = new Pose3d(RobotState.getInstance().getEstimatedPose()).getTranslation();
132+
133+
AdjustedParabolicThread.getInstance().setTarget(target);
134+
135+
Logger.recordOutput("Rotation Output: " , MathUtil.inputModulus( AdjustedParabolicThread.getInstance().getOutputState().r.getDegrees(), 0, 360));
136+
Logger.recordOutput("Velocity Output: ", new Translation2d(AdjustedParabolicThread.getInstance().getOutputState().t.in(MetersPerSecond), AdjustedParabolicThread.getInstance().getOutputState().a.in(MetersPerSecond)));
137+
Logger.recordOutput("Target", target);
138+
Logger.recordOutput("Thread Interupted", AdjustedParabolicThread.interrupted());
112139
// Return to normal thread priority
113140
// Threads.setCurrentThreadPriority(false, 10);
114141
}
@@ -169,7 +196,9 @@ public void testPeriodic() {}
169196

170197
/** This function is called once when the robot is first started up. */
171198
@Override
172-
public void simulationInit() {}
199+
public void simulationInit() {
200+
m_robotContainer.resetSimulationField();
201+
}
173202

174203
/** This function is called periodically whilst in simulation. */
175204
@Override

‎src/main/java/team5427/frc/robot/RobotContainer.java‎

Lines changed: 27 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,15 @@
44

55
package team5427.frc.robot;
66

7+
import com.pathplanner.lib.config.RobotConfig;
78
import edu.wpi.first.math.geometry.Pose2d;
89
import edu.wpi.first.math.geometry.Rotation2d;
910
import edu.wpi.first.wpilibj2.command.Command;
1011
import org.ironmaple.simulation.SimulatedArena;
11-
import org.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation;
12-
import org.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation.GamePieceInfo;
1312
import org.littletonrobotics.junction.Logger;
14-
15-
import com.pathplanner.lib.config.RobotConfig;
16-
1713
import team5427.frc.robot.io.PilotingControls;
1814
import team5427.frc.robot.subsystems.Swerve.SwerveSubsystem;
15+
import team5427.frc.robot.subsystems.vision.QuestNav;
1916

2017
/**
2118
* This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -28,9 +25,9 @@ public class RobotContainer {
2825
/** The container for the robot. Contains subsystems, OI devices, and commands. */
2926
public RobotContainer() {
3027

31-
try{
32-
Constants.config = RobotConfig.fromGUISettings();
33-
} catch(Exception e){
28+
try {
29+
Constants.config = RobotConfig.fromGUISettings();
30+
} catch (Exception e) {
3431
System.out.println("Robot Config not loading from GUI Settings");
3532
e.printStackTrace();
3633
return;
@@ -48,11 +45,12 @@ public RobotContainer() {
4845
SimulatedArena.getInstance()
4946
.addDriveTrainSimulation(SwerveSubsystem.getInstance().getKDriveSimulation());
5047
SimulatedArena.getInstance().clearGamePieces();
51-
48+
5249
break;
5350
default:
5451
break;
5552
}
53+
QuestNav.getInstance().setPose(new Pose2d(10 * Math.random(), 4, Rotation2d.kZero));
5654
buttonBindings();
5755
}
5856

@@ -70,21 +68,27 @@ public Command getAutonomousCommand() {
7068
return null;
7169
}
7270

73-
public void resetSimulationField() {
74-
if (Constants.currentMode != Constants.Mode.SIM) return;
71+
public void resetSimulationField() {
72+
if (Constants.currentMode != Constants.Mode.SIM) return;
73+
Pose2d pose = new Pose2d(3, 3, new Rotation2d());
7574

76-
SwerveSubsystem.getInstance().getKDriveSimulation().setSimulationWorldPose(new Pose2d(3, 3, new Rotation2d()));
77-
SimulatedArena.getInstance().resetFieldForAuto();
78-
}
75+
SwerveSubsystem.getInstance()
76+
.getKDriveSimulation()
77+
.setSimulationWorldPose(pose);
78+
RobotState.getInstance().resetAllPose(pose);
79+
SimulatedArena.getInstance().resetFieldForAuto();
80+
}
7981

80-
public void updateSimulation() {
81-
if (Constants.currentMode != Constants.Mode.SIM) return;
82+
public void updateSimulation() {
83+
if (Constants.currentMode != Constants.Mode.SIM) return;
8284

83-
SimulatedArena.getInstance().simulationPeriodic();
84-
Logger.recordOutput("FieldSimulation/RobotPosition", SwerveSubsystem.getInstance().getKDriveSimulation().getSimulatedDriveTrainPose());
85-
Logger.recordOutput(
86-
"FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral"));
87-
Logger.recordOutput(
88-
"FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae"));
89-
}
85+
SimulatedArena.getInstance().simulationPeriodic();
86+
Logger.recordOutput(
87+
"FieldSimulation/RobotPosition",
88+
SwerveSubsystem.getInstance().getKDriveSimulation().getSimulatedDriveTrainPose());
89+
Logger.recordOutput(
90+
"FieldSimulation/Coral", SimulatedArena.getInstance().getGamePiecesArrayByType("Coral"));
91+
Logger.recordOutput(
92+
"FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae"));
93+
}
9094
}

‎src/main/java/team5427/frc/robot/RobotState.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import org.littletonrobotics.junction.Logger;
1515
import team5427.frc.robot.subsystems.Swerve.SwerveConstants;
1616
import team5427.frc.robot.subsystems.Swerve.SwerveSubsystem;
17+
import team5427.frc.robot.subsystems.vision.QuestNav;
1718
import team5427.lib.detection.tuples.Tuple2Plus;
1819

1920
public class RobotState {
@@ -150,6 +151,6 @@ public void log() {
150151
Logger.recordOutput(
151152
"Localization/Estimation/Robot", RobotState.getInstance().getEstimatedPose());
152153
Logger.recordOutput("Localization/Odometry/Robot", RobotState.getInstance().getOdometryPose());
153-
Logger.recordOutput("Quest Pose", this.questPose);
154+
Logger.recordOutput("Quest Pose", QuestNav.getInstance().getPose());
154155
}
155156
}

‎src/main/java/team5427/frc/robot/io/PilotingControls.java‎

Lines changed: 64 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -4,16 +4,19 @@
44
import static edu.wpi.first.units.Units.Meters;
55
import static edu.wpi.first.units.Units.MetersPerSecond;
66

7-
import org.ironmaple.simulation.SimulatedArena;
8-
import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly;
9-
7+
import edu.wpi.first.math.geometry.Pose2d;
8+
import edu.wpi.first.math.geometry.Rotation2d;
109
import edu.wpi.first.math.geometry.Translation2d;
1110
import edu.wpi.first.wpilibj2.command.Commands;
11+
import edu.wpi.first.wpilibj2.command.InstantCommand;
1212
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
13+
import org.ironmaple.simulation.SimulatedArena;
14+
import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly;
1315
import team5427.frc.robot.Constants;
1416
import team5427.frc.robot.Constants.DriverConstants;
1517
import team5427.frc.robot.commands.chassis.ChassisMovement;
1618
import team5427.frc.robot.subsystems.Swerve.SwerveSubsystem;
19+
import team5427.frc.robot.subsystems.vision.QuestNav;
1720

1821
public class PilotingControls {
1922
private CommandXboxController joy;
@@ -31,29 +34,64 @@ public PilotingControls(CommandXboxController joy) {
3134
/** Made private to prevent multiple calls to this method */
3235
private void initalizeTriggers() {
3336
SwerveSubsystem.getInstance().setDefaultCommand(new ChassisMovement(joy));
37+
joy.a()
38+
.onTrue(
39+
new InstantCommand(
40+
() ->
41+
QuestNav.getInstance()
42+
.setPose(new Pose2d(10 * Math.random(), 4, Rotation2d.kZero)))
43+
.ignoringDisable(true));
44+
joy.x().onTrue(new InstantCommand(() -> {}));
45+
3446
// Example Coral Placement Code
35-
// TODO: delete these code for your own project
36-
if (Constants.currentMode == Constants.Mode.SIM) {
37-
// L4 placement
38-
joy.y().onTrue(Commands.runOnce(() -> SimulatedArena.getInstance()
39-
.addGamePieceProjectile(new ReefscapeCoralOnFly(
40-
SwerveSubsystem.getInstance().getKDriveSimulation().getSimulatedDriveTrainPose().getTranslation(),
41-
new Translation2d(0.4, 0),
42-
SwerveSubsystem.getInstance().getKDriveSimulation().getDriveTrainSimulatedChassisSpeedsFieldRelative(),
43-
SwerveSubsystem.getInstance().getKDriveSimulation().getSimulatedDriveTrainPose().getRotation(),
44-
Meters.of(2),
45-
MetersPerSecond.of(1.5),
46-
Degrees.of(-80)))));
47-
// L3 placement
48-
joy.b().onTrue(Commands.runOnce(() -> SimulatedArena.getInstance()
49-
.addGamePieceProjectile(new ReefscapeCoralOnFly(
50-
SwerveSubsystem.getInstance().getKDriveSimulation().getSimulatedDriveTrainPose().getTranslation(),
51-
new Translation2d(0.4, 0),
52-
SwerveSubsystem.getInstance().getKDriveSimulation().getDriveTrainSimulatedChassisSpeedsFieldRelative(),
53-
SwerveSubsystem.getInstance().getKDriveSimulation().getSimulatedDriveTrainPose().getRotation(),
54-
Meters.of(1.35),
55-
MetersPerSecond.of(1.5),
56-
Degrees.of(-60)))));
57-
}
47+
// TODO: delete these code for your own project
48+
if (Constants.currentMode == Constants.Mode.SIM) {
49+
// L4 placement
50+
joy.y()
51+
.onTrue(
52+
Commands.runOnce(
53+
() ->
54+
SimulatedArena.getInstance()
55+
.addGamePieceProjectile(
56+
new ReefscapeCoralOnFly(
57+
SwerveSubsystem.getInstance()
58+
.getKDriveSimulation()
59+
.getSimulatedDriveTrainPose()
60+
.getTranslation(),
61+
new Translation2d(0.4, 0),
62+
SwerveSubsystem.getInstance()
63+
.getKDriveSimulation()
64+
.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
65+
SwerveSubsystem.getInstance()
66+
.getKDriveSimulation()
67+
.getSimulatedDriveTrainPose()
68+
.getRotation(),
69+
Meters.of(2),
70+
MetersPerSecond.of(1.5),
71+
Degrees.of(-80)))));
72+
// L3 placement
73+
joy.b()
74+
.onTrue(
75+
Commands.runOnce(
76+
() ->
77+
SimulatedArena.getInstance()
78+
.addGamePieceProjectile(
79+
new ReefscapeCoralOnFly(
80+
SwerveSubsystem.getInstance()
81+
.getKDriveSimulation()
82+
.getSimulatedDriveTrainPose()
83+
.getTranslation(),
84+
new Translation2d(0.4, 0),
85+
SwerveSubsystem.getInstance()
86+
.getKDriveSimulation()
87+
.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
88+
SwerveSubsystem.getInstance()
89+
.getKDriveSimulation()
90+
.getSimulatedDriveTrainPose()
91+
.getRotation(),
92+
Meters.of(1.35),
93+
MetersPerSecond.of(1.5),
94+
Degrees.of(-60)))));
95+
}
5896
}
5997
}

‎src/main/java/team5427/frc/robot/subsystems/Swerve/SwerveModule.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,8 @@
1414
import team5427.frc.robot.Constants;
1515
import team5427.frc.robot.subsystems.Swerve.io.ModuleIO;
1616
import team5427.frc.robot.subsystems.Swerve.io.ModuleIOInputsAutoLogged;
17-
import team5427.frc.robot.subsystems.Swerve.io.ModuleIOTalonFXReal;
18-
import team5427.frc.robot.subsystems.Swerve.io.ModuleIOTalonFXSim;
17+
import team5427.frc.robot.subsystems.Swerve.io.talon.ModuleIOTalonFXReal;
18+
import team5427.frc.robot.subsystems.Swerve.io.talon.ModuleIOTalonFXSim;
1919

2020
public class SwerveModule {
2121
private ModuleIO io;

0 commit comments

Comments
 (0)