Skip to content

Commit f83e7cb

Browse files
PatribotsProgrammingPatribotsProgramming
committed
you have to break a few eggs to make an omelette
1 parent 9e12cff commit f83e7cb

File tree

10 files changed

+213
-59
lines changed

10 files changed

+213
-59
lines changed

‎build.gradle‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,22 @@ wpi.java.debugJni = false
4949
// Set this to true to enable desktop support.
5050
def includeDesktopSupport = true
5151

52+
repositories {
53+
maven {
54+
url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
55+
credentials {
56+
username = "Mechanical-Advantage-Bot"
57+
password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
58+
}
59+
}
60+
mavenLocal()
61+
}
62+
63+
task(replayWatch, type: JavaExec) {
64+
mainClass = "org.littletonrobotics.junction.ReplayWatch"
65+
classpath = sourceSets.main.runtimeClasspath
66+
}
67+
5268
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
5369
// Also defines JUnit 5.
5470
dependencies {
@@ -74,6 +90,9 @@ dependencies {
7490

7591
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
7692
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
93+
94+
def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
95+
annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
7796
}
7897

7998
test {

‎src/main/deploy/pathplanner/navgrid.json‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}

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

Lines changed: 19 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -4,9 +4,9 @@
44

55
import org.littletonrobotics.junction.AutoLogOutput;
66

7-
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
8-
import com.pathplanner.lib.util.PIDConstants;
9-
import com.pathplanner.lib.util.ReplanningConfig;
7+
import com.pathplanner.lib.config.PIDConstants;
8+
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
9+
1010
import edu.wpi.first.math.geometry.Pose2d;
1111
import edu.wpi.first.math.geometry.Pose3d;
1212
import edu.wpi.first.math.geometry.Rotation2d;
@@ -29,7 +29,6 @@
2929
import frc.robot.subsystems.drive.Swerve;
3030
import frc.robot.subsystems.test.KrakenTest;
3131
import frc.robot.util.Constants.AutoConstants;
32-
import frc.robot.util.Constants.DriveConstants;
3332
import frc.robot.util.Constants.OIConstants;
3433
import frc.robot.util.auto.PathPlannerStorage;
3534
import frc.robot.util.custom.PatriBoxController;
@@ -183,12 +182,12 @@ private void configureOperatorBindings(PatriBoxController controller) {
183182
}
184183

185184
public void updateNTGains() {
186-
double HPFCP = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/0-P").getDouble(-1);
187-
double HPFCI = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/1-I").getDouble(-1);
188-
double HPFCD = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/2-D").getDouble(-1);
189-
double HPFCP2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/0-P").getDouble(-1);
190-
double HPFCI2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/1-I").getDouble(-1);
191-
double HPFCD2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/2-D").getDouble(-1);
185+
double PPHDCP = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/0-P").getDouble(-1);
186+
double PPHDCI = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/1-I").getDouble(-1);
187+
double PPHDCD = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/2-D").getDouble(-1);
188+
double PPHDCP2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/0-P").getDouble(-1);
189+
double PPHDCI2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/1-I").getDouble(-1);
190+
double PPHDCD2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/2-D").getDouble(-1);
192191

193192
double HDCP = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Translation/0-P").getDouble(-1);
194193
double HDCI = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Translation/1-I").getDouble(-1);
@@ -197,7 +196,7 @@ public void updateNTGains() {
197196
double HDCI2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/1-I").getDouble(-1);
198197
double HDCD2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/2-D").getDouble(-1);
199198

200-
if (HPFCP == -1 || HPFCI == -1 || HPFCD == -1 || HPFCP2 == -1 || HPFCI2 == -1 || HPFCD2 == -1 ||
199+
if (PPHDCP == -1 || PPHDCI == -1 || PPHDCD == -1 || PPHDCP2 == -1 || PPHDCI2 == -1 || PPHDCD2 == -1 ||
201200
HDCP == -1 || HDCI == -1 || HDCD == -1 || HDCP2 == -1 || HDCI2 == -1 || HDCD2 == -1) {
202201
NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/0-P").setDouble(AutoConstants.XY_CORRECTION_P);
203202
NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/1-I").setDouble(AutoConstants.XY_CORRECTION_I);
@@ -214,18 +213,15 @@ public void updateNTGains() {
214213
NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/2-D").setDouble(AutoConstants.ROTATION_CORRECTION_D);
215214
return;
216215
} else {
217-
AutoConstants.HPFC = new HolonomicPathFollowerConfig(
218-
new PIDConstants(
219-
HPFCP,
220-
HPFCI,
221-
HPFCD),
222-
new PIDConstants(
223-
HPFCP2,
224-
HPFCI2,
225-
HPFCD2),
226-
DriveConstants.MAX_SPEED_METERS_PER_SECOND,
227-
Math.hypot(DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH) / 2.0,
228-
new ReplanningConfig());
216+
AutoConstants.PPHDC = new PPHolonomicDriveController(
217+
new PIDConstants(
218+
PPHDCP,
219+
PPHDCI,
220+
PPHDCD),
221+
new PIDConstants(
222+
PPHDCP2,
223+
PPHDCI2,
224+
PPHDCD2));
229225

230226
AutoConstants.XY_PID.setP(HDCP);
231227
AutoConstants.XY_PID.setI(HDCI);

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

Lines changed: 14 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import org.littletonrobotics.junction.Logger;
1212

1313
import com.pathplanner.lib.auto.AutoBuilder;
14+
import com.pathplanner.lib.config.RobotConfig;
1415

1516
import java.util.Arrays;
1617
import edu.wpi.first.math.MathUtil;
@@ -117,14 +118,19 @@ public Swerve() {
117118
resetEncoders();
118119
setBrakeMode();
119120

120-
AutoBuilder.configureHolonomic(
121-
this::getPose,
122-
this::resetOdometryAuto,
123-
this::getRobotRelativeVelocity,
124-
this::drive,
125-
AutoConstants.HPFC,
126-
Robot::isRedAlliance,
127-
this);
121+
try {
122+
AutoBuilder.configure(
123+
this::getPose,
124+
this::resetOdometryAuto,
125+
this::getRobotRelativeVelocity,
126+
(speeds, feedforwards) -> drive(speeds),
127+
AutoConstants.PPHDC,
128+
RobotConfig.fromGUISettings(),
129+
Robot::isRedAlliance,
130+
this);
131+
} catch (Exception e) {
132+
e.printStackTrace();
133+
}
128134

129135
poseEstimator = new SwerveDrivePoseEstimator(
130136
DriveConstants.DRIVE_KINEMATICS,
@@ -497,17 +503,6 @@ public Command resetHDCCommand() {
497503
return Commands.runOnce(() -> resetHDC());
498504
}
499505

500-
public void reconfigureAutoBuilder() {
501-
AutoBuilder.configureHolonomic(
502-
this::getPose,
503-
this::resetOdometryAuto,
504-
this::getRobotRelativeVelocity,
505-
this::drive,
506-
AutoConstants.HPFC,
507-
Robot::isRedAlliance,
508-
this);
509-
}
510-
511506
public boolean atPose(Pose2d position) {
512507
// More lenient on x axis, less lenient on y axis and rotation
513508
Pose2d currentPose = getPose();

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

Lines changed: 7 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,10 @@
99
import java.util.List;
1010
import java.util.Map;
1111

12-
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
13-
import com.pathplanner.lib.util.PIDConstants;
14-
import com.pathplanner.lib.util.ReplanningConfig;
15-
import com.revrobotics.CANSparkBase;
12+
import com.pathplanner.lib.config.PIDConstants;
13+
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
14+
import com.revrobotics.spark.SparkBase;
15+
1616
import edu.wpi.first.math.controller.HolonomicDriveController;
1717
import edu.wpi.first.math.controller.PIDController;
1818
import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -207,7 +207,7 @@ public static final class AutoConstants {
207207
THETA_PID
208208
);
209209

210-
public static HolonomicPathFollowerConfig HPFC = new HolonomicPathFollowerConfig(
210+
public static PPHolonomicDriveController PPHDC = new PPHolonomicDriveController(
211211
new PIDConstants(
212212
AutoConstants.XY_GAINS.getP(),
213213
AutoConstants.XY_GAINS.getI(),
@@ -216,10 +216,7 @@ public static final class AutoConstants {
216216
AutoConstants.THETA_GAINS.getP(),
217217
AutoConstants.THETA_GAINS.getI(),
218218
AutoConstants.THETA_GAINS.getD(),
219-
Units.degreesToRadians(45)),
220-
MAX_SPEED_METERS_PER_SECOND,
221-
Math.hypot(DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH)/2.0,
222-
new ReplanningConfig());
219+
Units.degreesToRadians(45)));
223220

224221
public static final String[] AUTO_NAMES = new String[] {};
225222

@@ -447,7 +444,7 @@ public static final class OIConstants {
447444
public static final class NeoMotorConstants {
448445
public static final double VORTEX_FREE_SPEED_RPM = 6784;
449446

450-
public static ArrayList<CANSparkBase> motors = new ArrayList<CANSparkBase>();
447+
public static ArrayList<SparkBase> motors = new ArrayList<SparkBase>();
451448

452449
public static final boolean SAFE_SPARK_MODE = false;
453450
public static final double NEO_FREE_SPEED_RPM = 5676;

‎src/main/java/frc/robot/util/auto/PathPlannerStorage.java‎

Lines changed: 9 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -69,14 +69,17 @@ public void configureAutoChooser() {
6969
// Load the auto and add it to the auto chooser
7070
Command auto = AutoBuilder.buildAuto(autoName);
7171
autoChooser.addOption(autoName, auto);
72-
// Load the auto and add it to the list of starting positions
73-
// for LPI
74-
Pose2d startingPosition = PathPlannerAuto.getStaringPoseFromAutoFile(autoName);
75-
PathPlannerStorage.AUTO_STARTING_POSITIONS.add(startingPosition);
7672
// Load the auto and add it to the list of paths
7773
// for trajectory visualization
78-
List<PathPlannerPath> paths = PathPlannerAuto.getPathGroupFromAutoFile(autoName);
79-
PathPlannerStorage.AUTO_PATHS.put(autoName, paths);
74+
List<PathPlannerPath> paths;
75+
try {
76+
paths = PathPlannerAuto.getPathGroupFromAutoFile(autoName);
77+
Pose2d startingPosition = paths.get(0).getStartingHolonomicPose().get();
78+
PathPlannerStorage.AUTO_STARTING_POSITIONS.add(startingPosition);
79+
PathPlannerStorage.AUTO_PATHS.put(autoName, paths);
80+
} catch (Exception e) {
81+
e.printStackTrace();
82+
}
8083
}
8184
System.out.println("Configured auto chooser");
8285
bindListener(getUpdatePathViewerCommand());

0 commit comments

Comments
 (0)