fix issues with serialization of the auto

src/main/java/frc/auton/guiauto/AbstractGuiAuto.java

@@ -66,7 +66,9 @@ private void init() {
     public void run() {
         System.out.println("Started Running: " + Timer.getFPGATimestamp());
         //Set our initial pose in our robot tracker
-        RobotTracker.getInstance().resetPosition(initialPose);
+        if (initialPose != null) {
+            RobotTracker.getInstance().resetPosition(initialPose);
+        }
 
         //Loop though all the steps and execute them
         for (AbstractAutonomousStep autonomousStep : autonomous.getAutonomousSteps()) {

src/main/java/frc/auton/guiauto/serialization/AbstractAutonomousStep.java

@@ -8,6 +8,7 @@
 import frc.auton.TemplateAuto;
 
 @JsonTypeInfo(use = JsonTypeInfo.Id.NAME,
+        include = JsonTypeInfo.As.PROPERTY,
         property = "type")
 @JsonSubTypes({
         @Type(value = TrajectoryAutonomousStep.class, name = "trajectory"),

src/main/java/frc/auton/guiauto/serialization/ScriptAutonomousStep.java

@@ -10,29 +10,29 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 public class ScriptAutonomousStep extends AbstractAutonomousStep {
 
-    private final SendableScript script;
+    private final SendableScript sendableScript;
 
     @JsonCreator
-    public ScriptAutonomousStep(@JsonProperty(required = true, value = "sendableScript") SendableScript script) {
-        this.script = script;
+    public ScriptAutonomousStep(@JsonProperty(required = true, value = "sendableScript") SendableScript sendableScript) {
+        this.sendableScript = sendableScript;
     }
 
     @Override
     public String toString() {
-        return "ScriptAutonomousStep{" + "script='" + script + '\'' + '}';
+        return "ScriptAutonomousStep{" + "sendableScript='" + sendableScript + '\'' + '}';
     }
 
     @JsonProperty
-    public SendableScript getScript() {
-        return script;
+    public SendableScript getSendableScript() {
+        return sendableScript;
     }
 
     @Override
     //If you've removed this method argument elsewhere you will need to remove it here too
     public void execute(TemplateAuto templateAuto) {
         if (!templateAuto.isDead()) { //Check that our auto is still running
-            if (!script.execute()) {
-                //The script failed to execute; kill the auto
+            if (!sendableScript.execute()) {
+                //The sendableScript failed to execute; kill the auto
                 templateAuto.killSwitch();
             }
         }

src/main/java/frc/auton/guiauto/serialization/TimedRotation.java

@@ -1,40 +1,51 @@
 package frc.auton.guiauto.serialization;
 
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnore;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
 
+@JsonIgnoreProperties(ignoreUnknown = true)
 public final class TimedRotation {
     @JsonProperty("time")
     public double time;
 
     @JsonProperty("rotation")
     public Rotation2d rotation;
 
+    @JsonIgnore
     public TimedRotation() {
         this(0, new Rotation2d());
     }
 
+    @JsonIgnore
     public TimedRotation(Rotation2d rotation) {
         this(0, rotation);
     }
 
-    public TimedRotation(double time, Rotation2d rotation) {
+    @JsonCreator
+    public TimedRotation(@JsonProperty("time") double time,
+                         @JsonProperty("rotation") Rotation2d rotation) {
         this.time = time;
         this.rotation = rotation;
     }
 
+    @JsonIgnore
     public TimedRotation(@NotNull TimedRotation other) {
         this(other.time, other.rotation);
     }
 
+    @JsonIgnore
     @Contract(value = " -> new", pure = true)
     @NotNull
     public TimedRotation copy() {
         return new TimedRotation(this);
     }
 
+    @JsonIgnore
     public Rotation2d getRotation() {
         return rotation;
     }

src/main/java/frc/auton/guiauto/serialization/TrajectoryAutonomousStep.java

@@ -7,7 +7,6 @@
 import edu.wpi.first.wpilibj.trajectory.Trajectory.State;
 import frc.auton.TemplateAuto;
 import frc.subsystem.Drive;
-import org.jetbrains.annotations.Contract;
 
 import java.util.List;
 
@@ -40,8 +39,8 @@ public void execute(TemplateAuto templateAuto) {
         //If this is not how your autonomous code work you can change the implementation to fit your needs.
         //You just need to ensure that this thread will be blocked until the path is finished being driven.
         if (!templateAuto.isDead()) { //Check that the auto is not dead
-            Drive.getInstance().setAutoPath(getTrajectory(),
-                    rotations.get(0).rotation); //Send the auto to our drive class to be executed
+            Drive.getInstance().setAutoPath(getTrajectory()); //Send the auto to our drive class to be executed
+            Drive.getInstance().setAutoRotation(rotations.get(0).rotation);
             int rotationIndex = 1; //Start at the second rotation (the first is the starting rotation)
             while (!Drive.getInstance().isFinished()) {
                 if (templateAuto.isDead()) {
@@ -55,6 +54,7 @@ public void execute(TemplateAuto templateAuto) {
                     Thread.yield(); //Wait for the auto to finish
                 }
             }
+            Drive.getInstance().stopMovement();
         }
     }
 

src/main/java/frc/auton/guiauto/serialization/command/SendableCommand.java

@@ -110,8 +110,13 @@ public SendableCommand(@JsonProperty("methodName") String methodName,
         this.instance = instance;
     }
 
+    @JsonIgnoreProperties
     private final Object instance;
+
+    @JsonIgnoreProperties
     private final Method methodToCall;
+
+    @JsonIgnoreProperties
     private final Object[] objArgs;
 
 

src/main/java/frc/auton/guiauto/serialization/command/SendableScript.java

@@ -1,10 +1,13 @@
 package frc.auton.guiauto.serialization.command;
 
 import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 
-import java.util.ArrayList;
+import java.util.List;
 
+
+@JsonIgnoreProperties(ignoreUnknown = true)
 public class SendableScript {
 
     /**
@@ -28,22 +31,18 @@ public enum DelayType {
     private DelayType delayType;
     private double delay;
 
-    private final ArrayList<SendableCommand> commands;
+    private final List<SendableCommand> commands;
 
 
     @JsonCreator
     public SendableScript(@JsonProperty("delayType") DelayType delayType,
                           @JsonProperty("delay") double delay,
-                          @JsonProperty("commands") ArrayList<SendableCommand> commands) {
+                          @JsonProperty("commands") List<SendableCommand> commands) {
         this.delayType = delayType;
         this.delay = delay;
         this.commands = commands;
     }
 
-    public SendableScript() {
-        this(DelayType.NONE, 0, new ArrayList<>());
-    }
-
     @JsonProperty("delayType")
     public DelayType getDelayType() {
         return delayType;
@@ -55,7 +54,7 @@ public double getDelay() {
     }
 
     @JsonProperty("commands")
-    public ArrayList<SendableCommand> getCommands() {
+    public List<SendableCommand> getCommands() {
         return commands;
     }
 

src/main/java/frc/subsystem/Drive.java

@@ -368,20 +368,19 @@ public double getSpeedSquared() {
     HolonomicDriveController controller = new HolonomicDriveController(
             new PIDController(1.5, 0, 0),
             new PIDController(1.5, 0, 0),
-            new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(0.2, 0.2)));
+            new ProfiledPIDController(5, 0, 0, new TrapezoidProfile.Constraints(6, 5)));
 
     {
-        controller.setTolerance(new Pose2d(0.5, 0.5, Rotation2d.fromDegrees(360))); //TODO: Tune
+        controller.setTolerance(new Pose2d(0.5, 0.5, Rotation2d.fromDegrees(10))); //TODO: Tune
     }
 
 
-    public synchronized void setAutoPath(Trajectory trajectory, Rotation2d autoTargetHeading) {
+    public synchronized void setAutoPath(Trajectory trajectory) {
         driveState = DriveState.RAMSETE;
         this.currentAutoTrajectory = trajectory;
         autoStartTime = Timer.getFPGATimestamp();
         configAuto();
         configCoast();
-        this.autoTargetHeading = autoTargetHeading;
     }
 
     Trajectory currentAutoTrajectory;
@@ -405,6 +404,7 @@ private void updateRamsete() {
 
     public synchronized void setAutoRotation(Rotation2d rotation) {
         autoTargetHeading = rotation;
+        System.out.println("new rotation" + rotation.getDegrees());
     }
 
     public double getAutoElapsedTime() {
@@ -547,6 +547,7 @@ public void logData() {
 
     public double getAbsolutePosition(int moduleNumber) {
         double angle = ((1 - swerveEncodersDIO[moduleNumber].getOutput()) * 360) - 90;
+        if (moduleNumber == 3) angle -= 72;
         return angle < 0 ? angle + 360 : angle;
     }
 }

src/main/java/frc/utility/Serializer.java

@@ -7,7 +7,7 @@
 import java.io.File;
 import java.io.IOException;
 
-public class Serializer {
+public final class Serializer {
     static ObjectMapper objectMapper = new ObjectMapper();
 
     public static String serializeToString(Object obj) throws IOException {