Skip to content

Commit 71368d1

Browse files
srimanachantaTalon540-root
srimanachanta
and
Talon540-root
committed
Implement Full Logging and Drivetrain Subsystem (#1)
* Da Code * Add alerts on drive spark maxes * Add Config Changes from Meeting * Other lil bs * Add automatic brake disable on robot disable * add odometry * add SIM module * Add DriveCommands * Update RobotContainer.java * Formatting fixes * Misc Fixes * Add low voltage warning for DT * Add velocity scalars for DT * Update PID Coefficents and fix odometry issues * Update URCL.json --------- Fix CI Co-Authored-By: Talon540-root <[email protected]>
1 parent fb96d9a commit 71368d1

30 files changed

+3063
-482
lines changed

‎.github/workflows/build.yml

+1-2
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,13 @@
11
name: Build
22

33
on:
4-
push:
54
pull_request:
65

76
jobs:
87
build:
98
name: Build
109
runs-on: ubuntu-latest
11-
container: wpilib/roborio-cross-ubuntu
10+
container: wpilib/roborio-cross-ubuntu:2025-24.04
1211
steps:
1312
- name: Checkout repository
1413
uses: actions/checkout@v4

‎.github/workflows/lint-format.yml

+1-40
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
name: Lint and Format
22

33
on:
4-
push:
4+
pull_request:
55

66
concurrency:
77
group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
@@ -15,45 +15,6 @@ jobs:
1515
- uses: actions/checkout@v4
1616
- uses: gradle/actions/wrapper-validation@v4
1717

18-
wpiformat:
19-
name: "wpiformat"
20-
runs-on: ubuntu-22.04
21-
steps:
22-
- uses: actions/checkout@v4
23-
with:
24-
fetch-depth: 0
25-
- name: Fetch all history and metadata
26-
run: |
27-
git checkout -b pr
28-
git branch -f main origin/main
29-
- name: Set up Python 3.12
30-
uses: actions/setup-python@v5
31-
with:
32-
python-version: '3.12'
33-
- name: Install wpiformat
34-
run: |
35-
python -m venv ${{ runner.temp }}/wpiformat
36-
${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.51
37-
- name: Run
38-
run: ${{ runner.temp }}/wpiformat/bin/wpiformat
39-
- name: Check output
40-
run: git --no-pager diff --exit-code HEAD
41-
- name: Generate diff
42-
run: git diff HEAD > wpiformat-fixes.patch
43-
if: ${{ failure() }}
44-
- uses: actions/upload-artifact@v4
45-
with:
46-
name: wpiformat fixes
47-
path: wpiformat-fixes.patch
48-
if: ${{ failure() }}
49-
- name: Write to job summary
50-
run: |
51-
echo '```diff' >> $GITHUB_STEP_SUMMARY
52-
cat wpiformat-fixes.patch >> $GITHUB_STEP_SUMMARY
53-
echo '' >> $GITHUB_STEP_SUMMARY
54-
echo '```' >> $GITHUB_STEP_SUMMARY
55-
if: ${{ failure() }}
56-
5718
javaformat:
5819
name: "Java format"
5920
runs-on: ubuntu-22.04

‎.wpilib/wpilib_preferences.json

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,5 +2,5 @@
22
"enableCppIntellisense": false,
33
"currentLanguage": "java",
44
"projectYear": "2025",
5-
"teamNumber": 6328
5+
"teamNumber": 540
66
}

‎build.gradle

+2-2
Original file line numberDiff line numberDiff line change
@@ -160,7 +160,7 @@ spotless {
160160
exclude "**/build/**", "**/build-*/**"
161161
}
162162
greclipse()
163-
indentWithSpaces(4)
163+
leadingTabsToSpaces(4)
164164
trimTrailingWhitespace()
165165
endWithNewline()
166166
}
@@ -177,7 +177,7 @@ spotless {
177177
exclude "**/build/**", "**/build-*/**"
178178
}
179179
trimTrailingWhitespace()
180-
indentWithSpaces(2)
180+
leadingTabsToSpaces(2)
181181
endWithNewline()
182182
}
183183
}

‎gradlew

100644100755
File mode changed.

‎src/main/deploy/trajectories/.gitkeep

Whitespace-only changes.

‎src/main/java/frc/robot/constants/Constants.java ‎src/main/java/frc/robot/Constants.java

+5-4
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,19 @@
1-
package frc.robot.constants;
1+
package frc.robot;
22

33
import edu.wpi.first.wpilibj.DriverStation;
44
import edu.wpi.first.wpilibj.RobotBase;
55

66
public class Constants {
7-
private static RobotType kRobotType = RobotType.ROBOT_SIMBOT;
7+
private static RobotType kRobotType = RobotType.ROBOT_2025_COMP;
88
// Allows tunable values to be changed when enabled. Also adds tunable selectors to AutoSelector
9-
public static final boolean TUNING_MODE = false;
9+
public static final boolean TUNING_MODE = true;
1010
// Disable the AdvantageKit logger from running
1111
public static final boolean ENABLE_LOGGING = true;
1212

1313
public static final double kLoopPeriodSecs = 0.02;
1414

15+
public static final double LOW_VOLTAGE_WARNING_THRESHOLD = 10.0;
16+
1517
public enum RobotMode {
1618
REAL,
1719
SIM,
@@ -20,7 +22,6 @@ public enum RobotMode {
2022

2123
public enum RobotType {
2224
ROBOT_2025_COMP,
23-
ROBOT_2024_OFFSEASON,
2425
ROBOT_SIMBOT
2526
}
2627

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

+203
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,203 @@
1+
package frc.robot;
2+
3+
import edu.wpi.first.math.geometry.*;
4+
import edu.wpi.first.math.util.Units;
5+
import java.util.ArrayList;
6+
import java.util.HashMap;
7+
import java.util.List;
8+
import java.util.Map;
9+
10+
/**
11+
* Contains various field dimensions and useful reference points. All units are in meters and poses
12+
* have a blue alliance origin.
13+
*/
14+
public class FieldConstants {
15+
public static final double fieldLength = Units.inchesToMeters(690.876);
16+
public static final double fieldWidth = Units.inchesToMeters(317);
17+
public static final double startingLineX =
18+
Units.inchesToMeters(299.438); // Measured from the inside of starting line
19+
20+
public static class Processor {
21+
public static final Pose2d centerFace =
22+
new Pose2d(Units.inchesToMeters(235.726), 0, Rotation2d.fromDegrees(90));
23+
}
24+
25+
public static class Barge {
26+
public static final Translation2d farCage =
27+
new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(286.779));
28+
public static final Translation2d middleCage =
29+
new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(242.855));
30+
public static final Translation2d closeCage =
31+
new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947));
32+
33+
// Measured from floor to bottom of cage
34+
public static final double deepHeight = Units.inchesToMeters(3.125);
35+
public static final double shallowHeight = Units.inchesToMeters(30.125);
36+
}
37+
38+
public static class CoralStation {
39+
public static final Pose2d leftCenterFace =
40+
new Pose2d(
41+
Units.inchesToMeters(33.526),
42+
Units.inchesToMeters(291.176),
43+
Rotation2d.fromDegrees(90 - 144.011));
44+
public static final Pose2d rightCenterFace =
45+
new Pose2d(
46+
Units.inchesToMeters(33.526),
47+
Units.inchesToMeters(25.824),
48+
Rotation2d.fromDegrees(144.011 - 90));
49+
}
50+
51+
public static class Reef {
52+
public static final Translation2d center =
53+
new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501));
54+
public static final double faceToZoneLine =
55+
Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line
56+
57+
public static final Pose2d[] centerFaces =
58+
new Pose2d[6]; // Starting facing the driver station in clockwise order
59+
public static final List<Map<ReefHeight, Pose3d>> branchPositions =
60+
new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise
61+
62+
static {
63+
// Initialize faces
64+
centerFaces[0] =
65+
new Pose2d(
66+
Units.inchesToMeters(144.003),
67+
Units.inchesToMeters(158.500),
68+
Rotation2d.fromDegrees(180));
69+
centerFaces[1] =
70+
new Pose2d(
71+
Units.inchesToMeters(160.373),
72+
Units.inchesToMeters(186.857),
73+
Rotation2d.fromDegrees(120));
74+
centerFaces[2] =
75+
new Pose2d(
76+
Units.inchesToMeters(193.116),
77+
Units.inchesToMeters(186.858),
78+
Rotation2d.fromDegrees(60));
79+
centerFaces[3] =
80+
new Pose2d(
81+
Units.inchesToMeters(209.489),
82+
Units.inchesToMeters(158.502),
83+
Rotation2d.fromDegrees(0));
84+
centerFaces[4] =
85+
new Pose2d(
86+
Units.inchesToMeters(193.118),
87+
Units.inchesToMeters(130.145),
88+
Rotation2d.fromDegrees(-60));
89+
centerFaces[5] =
90+
new Pose2d(
91+
Units.inchesToMeters(160.375),
92+
Units.inchesToMeters(130.144),
93+
Rotation2d.fromDegrees(-120));
94+
95+
// Initialize branch positions
96+
for (int face = 0; face < 6; face++) {
97+
Map<ReefHeight, Pose3d> fillRight = new HashMap<>();
98+
Map<ReefHeight, Pose3d> fillLeft = new HashMap<>();
99+
for (var level : ReefHeight.values()) {
100+
Pose2d poseDirection = new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face)));
101+
double adjustX = Units.inchesToMeters(30.738);
102+
double adjustY = Units.inchesToMeters(6.469);
103+
104+
fillRight.put(
105+
level,
106+
new Pose3d(
107+
new Translation3d(
108+
poseDirection
109+
.transformBy(new Transform2d(adjustX, adjustY, new Rotation2d()))
110+
.getX(),
111+
poseDirection
112+
.transformBy(new Transform2d(adjustX, adjustY, new Rotation2d()))
113+
.getY(),
114+
level.height),
115+
new Rotation3d(
116+
0,
117+
Units.degreesToRadians(level.pitch),
118+
poseDirection.getRotation().getRadians())));
119+
fillLeft.put(
120+
level,
121+
new Pose3d(
122+
new Translation3d(
123+
poseDirection
124+
.transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d()))
125+
.getX(),
126+
poseDirection
127+
.transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d()))
128+
.getY(),
129+
level.height),
130+
new Rotation3d(
131+
0,
132+
Units.degreesToRadians(level.pitch),
133+
poseDirection.getRotation().getRadians())));
134+
}
135+
branchPositions.add((face * 2) + 1, fillRight);
136+
branchPositions.add((face * 2) + 2, fillLeft);
137+
}
138+
}
139+
}
140+
141+
public static class StagingPositions {
142+
// Measured from the center of the ice cream
143+
public static final Pose2d leftIceCream =
144+
new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), new Rotation2d());
145+
public static final Pose2d middleIceCream =
146+
new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d());
147+
public static final Pose2d rightIceCream =
148+
new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d());
149+
}
150+
151+
public enum ReefHeight {
152+
L4(Units.inchesToMeters(72), -90),
153+
L3(Units.inchesToMeters(47.625), -35),
154+
L2(Units.inchesToMeters(31.875), -35),
155+
L1(Units.inchesToMeters(18), 0);
156+
157+
ReefHeight(double height, double pitch) {
158+
this.height = height;
159+
this.pitch = pitch; // in degrees
160+
}
161+
162+
public final double height;
163+
public final double pitch;
164+
}
165+
166+
// TODO
167+
// public static final double aprilTagWidth = Units.inchesToMeters(6.50);
168+
// public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL;
169+
// public static final int aprilTagCount = 22;
170+
//
171+
// @Getter
172+
// public enum AprilTagLayoutType {
173+
// OFFICIAL("2025-official");
174+
//
175+
// AprilTagLayoutType(String name) {
176+
// if (Constants.disableHAL) {
177+
// layout = null;
178+
// } else {
179+
// try {
180+
// layout =
181+
// new AprilTagFieldLayout(
182+
// Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name +
183+
// ".json"));
184+
// } catch (IOException e) {
185+
// throw new RuntimeException(e);
186+
// }
187+
// }
188+
// if (layout == null) {
189+
// layoutString = "";
190+
// } else {
191+
// try {
192+
// layoutString = new ObjectMapper().writeValueAsString(layout);
193+
// } catch (JsonProcessingException e) {
194+
// throw new RuntimeException(
195+
// "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar");
196+
// }
197+
// }
198+
// }
199+
//
200+
// private final AprilTagFieldLayout layout;
201+
// private final String layoutString;
202+
// }
203+
}

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

+16-3
Original file line numberDiff line numberDiff line change
@@ -13,12 +13,13 @@
1313

1414
package frc.robot;
1515

16+
import edu.wpi.first.math.filter.Debouncer;
17+
import edu.wpi.first.wpilibj.Alert;
1618
import edu.wpi.first.wpilibj.DriverStation;
1719
import edu.wpi.first.wpilibj.RobotController;
1820
import edu.wpi.first.wpilibj.Threads;
19-
import edu.wpi.first.wpilibj2.command.CommandScheduler;
2021
import edu.wpi.first.wpilibj2.command.Command;
21-
import frc.robot.constants.Constants;
22+
import edu.wpi.first.wpilibj2.command.CommandScheduler;
2223
import frc.robot.util.LoggerUtil;
2324
import org.littletonrobotics.junction.LogFileUtil;
2425
import org.littletonrobotics.junction.LoggedRobot;
@@ -36,7 +37,11 @@
3637
*/
3738
public class Robot extends LoggedRobot {
3839
private Command autonomousCommand;
39-
private RobotContainer robotContainer;
40+
private final RobotContainer robotContainer;
41+
42+
private final Alert lowBatteryVoltageAlert =
43+
new Alert("Battery voltage is too low, change the battery", Alert.AlertType.kWarning);
44+
private final Debouncer batteryVoltageDebouncer = new Debouncer(0.5);
4045

4146
public Robot() {
4247
super(Constants.kLoopPeriodSecs);
@@ -74,6 +79,9 @@ public Robot() {
7479

7580
// Configure brownout voltage
7681
RobotController.setBrownoutVoltage(6.0);
82+
83+
// Create RobotConatiner
84+
robotContainer = new RobotContainer();
7785
}
7886

7987
@Override
@@ -84,6 +92,11 @@ public void robotPeriodic() {
8492
// Run command scheduler
8593
CommandScheduler.getInstance().run();
8694

95+
// Update Battery Voltage Alert
96+
lowBatteryVoltageAlert.set(
97+
batteryVoltageDebouncer.calculate(
98+
RobotController.getBatteryVoltage() <= Constants.LOW_VOLTAGE_WARNING_THRESHOLD));
99+
87100
// Return to normal thread priority
88101
Threads.setCurrentThreadPriority(false, 10);
89102
}

0 commit comments

Comments
 (0)