Skip to content

Commit 0310fca

Browse files
srimanachantasrimanachanta
committed
WIP (squash later)
1 parent fb96d9a commit 0310fca

18 files changed

+1271
-460
lines changed

‎.github/workflows/build.yml

+1-1
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ jobs:
88
build:
99
name: Build
1010
runs-on: ubuntu-latest
11-
container: wpilib/roborio-cross-ubuntu
11+
container: wpilib/roborio-cross-ubuntu:2025-24.04
1212
steps:
1313
- name: Checkout repository
1414
uses: actions/checkout@v4

‎.github/workflows/lint-format.yml

-39
Original file line numberDiff line numberDiff line change
@@ -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

‎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
}

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,8 @@
1616
import edu.wpi.first.wpilibj.DriverStation;
1717
import edu.wpi.first.wpilibj.RobotController;
1818
import edu.wpi.first.wpilibj.Threads;
19-
import edu.wpi.first.wpilibj2.command.CommandScheduler;
2019
import edu.wpi.first.wpilibj2.command.Command;
20+
import edu.wpi.first.wpilibj2.command.CommandScheduler;
2121
import frc.robot.constants.Constants;
2222
import frc.robot.util.LoggerUtil;
2323
import org.littletonrobotics.junction.LogFileUtil;

‎src/main/java/frc/robot/commands/DriveCommands.java

+41
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
package frc.robot.commands;
2+
3+
import edu.wpi.first.wpilibj.Timer;
4+
import edu.wpi.first.wpilibj2.command.Command;
5+
import edu.wpi.first.wpilibj2.command.Commands;
6+
import frc.robot.subsystems.drive.DriveBase;
7+
import java.util.LinkedList;
8+
import java.util.List;
9+
import java.util.function.DoubleSupplier;
10+
11+
public class DriveCommands {
12+
/**
13+
* Field relative drive command using two joysticks (controlling linear and angular velocities).
14+
*/
15+
public static Command joystickDrive(
16+
DriveBase driveBase,
17+
DoubleSupplier xSupplier,
18+
DoubleSupplier ySupplier,
19+
DoubleSupplier omegaSupplier) {
20+
return Commands.run(() -> {}, driveBase);
21+
}
22+
23+
// /**
24+
// * Field relative drive command using joystick for linear control and PID for angular control.
25+
// * Possible use cases include snapping to an angle, aiming at a vision target, or controlling
26+
// * absolute rotation with a joystick.
27+
// */
28+
// public static Command joystickDriveAtAngle() {}
29+
30+
/** Measures the velocity feedforward constants for the drive motors. */
31+
public static Command feedforwardCharacterization() {
32+
List<Double> velocitySamples = new LinkedList<>();
33+
List<Double> voltageSamples = new LinkedList<>();
34+
Timer timer = new Timer();
35+
36+
return Commands.sequence();
37+
}
38+
39+
// /** Measures the robot's wheel radius by spinning in a circle. */
40+
// public static Command wheelRadiusCharacterization() {}
41+
}

‎src/main/java/frc/robot/subsystems/drive/DriveBase.java

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
package frc.robot.subsystems.drive;
2+
3+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
4+
5+
public class DriveBase extends SubsystemBase {
6+
// private final GyroIO gyroIO;
7+
// private final Module[] modules = new Module[4]; // FL, FR, BL, BR
8+
// private final Alert gyroDisconnectedAlert =
9+
// new Alert("Disconnected gyro, using kinematic approximation as fallback.",
10+
// Alert.AlertType.kError);
11+
//
12+
//
13+
// public static Translation2d[] getModuleTranslations(double trackWidthX, double trackWidthY) {
14+
// return new Translation2d[] {
15+
// new Translation2d(trackWidthX / 2.0, trackWidthY / 2.0),
16+
// new Translation2d(trackWidthX / 2.0, -trackWidthY / 2.0),
17+
// new Translation2d(-trackWidthX / 2.0, trackWidthY / 2.0),
18+
// new Translation2d(-trackWidthX / 2.0, -trackWidthY / 2.0)
19+
// };
20+
// }
21+
22+
public void test() {}
23+
}

‎src/main/java/frc/robot/subsystems/drive/GyroIO.java

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
package frc.robot.subsystems.drive;
2+
3+
public interface GyroIO {}

‎src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
package frc.robot.subsystems.drive;
2+
3+
public class GyroIOPigeon2 {}

‎src/main/java/frc/robot/subsystems/drive/Module.java

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
package frc.robot.subsystems.drive;
2+
3+
public class Module {}

‎src/main/java/frc/robot/subsystems/drive/ModuleIO.java

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
package frc.robot.subsystems.drive;
2+
3+
public interface ModuleIO {}

‎src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
// package frc.robot.subsystems.drive;
2+
//
3+
// public class ModuleIOSim {}

‎src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
package frc.robot.subsystems.drive;
2+
3+
public class ModuleIOSpark {}

‎src/main/java/frc/robot/subsystems/drive/OdometryManager.java

+56
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
package frc.robot.subsystems.drive;
2+
3+
import edu.wpi.first.math.geometry.Rotation2d;
4+
import edu.wpi.first.math.kinematics.SwerveModulePosition;
5+
import java.util.ArrayList;
6+
import java.util.List;
7+
import java.util.Optional;
8+
import java.util.Queue;
9+
import java.util.concurrent.ArrayBlockingQueue;
10+
import java.util.concurrent.locks.Lock;
11+
import java.util.concurrent.locks.ReentrantLock;
12+
import java.util.function.Supplier;
13+
import org.littletonrobotics.junction.AutoLog;
14+
15+
public class OdometryManager {
16+
public static Lock odometryLock = new ReentrantLock();
17+
public static double ODOMETRY_FREQUENCY_HZ = 200.0;
18+
19+
private static OdometryManager instance = null;
20+
21+
public static OdometryManager getInstance() {
22+
if (instance == null) {
23+
instance = new OdometryManager();
24+
}
25+
return instance;
26+
}
27+
28+
private final Queue<Double> timestampQueue = new ArrayBlockingQueue<>(20);
29+
private final List<ModuleSource> m_moduleSources = new ArrayList<>(4);
30+
private GyroSource m_gyroSource = null;
31+
32+
public void registerModuleSource(
33+
Supplier<Optional<Double>> drivePositionSupplier,
34+
Supplier<Optional<Rotation2d>> turnAngleSupplier) {}
35+
36+
public void registerGyroSource(Supplier<Optional<Rotation2d>> robotYawSupplier) {}
37+
38+
private static void run() {}
39+
40+
private record ModuleSource(
41+
Queue<Double> drivePositionQueue,
42+
Supplier<Optional<Double>> drivePositionSupplier,
43+
Queue<Rotation2d> turnAngleQueue,
44+
Supplier<Optional<Rotation2d>> turnAngleSupplier) {}
45+
46+
private record GyroSource(
47+
Queue<Rotation2d> robotYawQueue, Supplier<Optional<Rotation2d>> robotYawSupplier) {}
48+
49+
@AutoLog
50+
public class TimestampInputs {
51+
public double[] timestamps;
52+
53+
public Rotation2d[] gyroYaws;
54+
public SwerveModulePosition[][] modulePositions;
55+
}
56+
}

‎src/main/java/frc/robot/util/EqualsUtil.java

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
package frc.robot.util;
2+
3+
import edu.wpi.first.math.geometry.Twist2d;
4+
5+
public class EqualsUtil {
6+
public static boolean epsilonEquals(double a, double b, double epsilon) {
7+
return (a - epsilon <= b) && (a + epsilon >= b);
8+
}
9+
10+
public static boolean epsilonEquals(double a, double b) {
11+
return epsilonEquals(a, b, 1e-9);
12+
}
13+
14+
/** Extension methods for wpi geometry objects */
15+
public static class GeomExtensions {
16+
public static boolean epsilonEquals(Twist2d twist, Twist2d other) {
17+
return EqualsUtil.epsilonEquals(twist.dx, other.dx)
18+
&& EqualsUtil.epsilonEquals(twist.dy, other.dy)
19+
&& EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta);
20+
}
21+
}
22+
}

0 commit comments

Comments
 (0)