Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add auto path preloading

Browse Source 
- Refactor recording to use commands
This commit is contained in:
nathanrsxtn
2022-02-16 22:31:00 -07:00
parent 3cb4a13bb1
commit 9ccf4bcea2
17 changed files with 1870 additions and 1463 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/utility/PathPlannerUtil.java
+31 -40
View File
@@ -2,53 +2,26 @@ package frc4388.utility;
import java.io.File;
import java.util.Optional;
import com.diffplug.common.base.Errors;
import com.fasterxml.jackson.annotation.JsonInclude.Include;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.datatype.jdk8.Jdk8Module;
public class PathPlannerUtil {
import edu.wpi.first.math.geometry.Translation2d;
public final class PathPlannerUtil {
public static final class Path {
public Optional<Waypoint>[] waypoints;
public static final class Waypoint {
public Waypoint() {}
public Waypoint(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
this.anchorPoint = anchorPointX == null && anchorPointY == null ? Optional.empty() : Optional.of(Point.of(anchorPointX, anchorPointY));
this.prevControl = prevControlX == null && prevControlY == null ? Optional.empty() : Optional.of(Point.of(prevControlX, prevControlY));
this.nextControl = nextControlX == null && nextControlY == null ? Optional.empty() : Optional.of(Point.of(nextControlX, nextControlY));
this.holonomicAngle = Optional.ofNullable(holonomicAngle);
this.isReversal = Optional.ofNullable(isReversal);
this.velOverride = Optional.ofNullable(velOverride);
this.isLocked = Optional.ofNullable(isLocked);
}
public Optional<Point> anchorPoint;
public Optional<Point> prevControl;
public Optional<Point> nextControl;
public static final class Point {
public Point() {}
public static Point of(Double x, Double y) {
Point point = new Point();
point.x = Optional.ofNullable(x);
point.y = Optional.ofNullable(y);
return point;
}
public Optional<Double> x;
public Optional<Double> y;
}
public Optional<Double> holonomicAngle;
public Optional<Boolean> isReversal;
public Optional<Double> velOverride;
public Optional<Boolean> isLocked;
}
public Optional<Waypoint[]> waypoints;
public Optional<Double> maxVelocity;
public Optional<Double> maxAcceleration;
public Optional<Boolean> isReversed;
private static final ObjectMapper objectMapper = new ObjectMapper();
static { objectMapper.registerModule(new Jdk8Module()); }
static {
objectMapper.registerModule(new Jdk8Module());
objectMapper.setSerializationInclusion(Include.NON_EMPTY);
}
public static Path read(File src) {
return Errors.log().getWithDefault(() -> objectMapper.readValue(src, Path.class), null);
@@ -58,9 +31,27 @@ public class PathPlannerUtil {
Errors.log().run(() -> objectMapper.writeValue(resultFile, this));
}
@Override
public String toString() {
return Errors.log().getWithDefault(() -> objectMapper.writeValueAsString(this), super.toString());
public static final class Waypoint {
public Optional<Translation2d> anchorPoint;
public Optional<Translation2d> prevControl;
public Optional<Translation2d> nextControl;
public Optional<Double> holonomicAngle;
public Optional<Boolean> isReversal;
public Optional<Double> velOverride;
public Optional<Boolean> isLocked;
public Waypoint() {
}
public Waypoint(Translation2d anchorPoint, Translation2d prevControl, Translation2d nextControl, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
this.anchorPoint = Optional.ofNullable(anchorPoint);
this.prevControl = Optional.ofNullable(prevControl);
this.nextControl = Optional.ofNullable(nextControl);
this.holonomicAngle = Optional.ofNullable(holonomicAngle);
this.isReversal = Optional.ofNullable(isReversal);
this.velOverride = Optional.ofNullable(velOverride);
this.isLocked = Optional.ofNullable(isLocked);
}
}
}
}