diff --git a/src/main/java/frc/auton/guiauto/AbstractGuiAuto.java b/src/main/java/frc/auton/guiauto/AbstractGuiAuto.java index c82d52bc..454bd777 100644 --- a/src/main/java/frc/auton/guiauto/AbstractGuiAuto.java +++ b/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()) { diff --git a/src/main/java/frc/auton/guiauto/serialization/AbstractAutonomousStep.java b/src/main/java/frc/auton/guiauto/serialization/AbstractAutonomousStep.java index eda10e7e..a68fddb0 100644 --- a/src/main/java/frc/auton/guiauto/serialization/AbstractAutonomousStep.java +++ b/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"), diff --git a/src/main/java/frc/auton/guiauto/serialization/ScriptAutonomousStep.java b/src/main/java/frc/auton/guiauto/serialization/ScriptAutonomousStep.java index f69e8f67..968d12bb 100644 --- a/src/main/java/frc/auton/guiauto/serialization/ScriptAutonomousStep.java +++ b/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(); } } diff --git a/src/main/java/frc/auton/guiauto/serialization/TimedRotation.java b/src/main/java/frc/auton/guiauto/serialization/TimedRotation.java index a27a01a1..e5339396 100644 --- a/src/main/java/frc/auton/guiauto/serialization/TimedRotation.java +++ b/src/main/java/frc/auton/guiauto/serialization/TimedRotation.java @@ -1,10 +1,14 @@ 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; @@ -12,29 +16,36 @@ public final class TimedRotation { @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; } diff --git a/src/main/java/frc/auton/guiauto/serialization/TrajectoryAutonomousStep.java b/src/main/java/frc/auton/guiauto/serialization/TrajectoryAutonomousStep.java index e3d63bcc..1b3a86bf 100644 --- a/src/main/java/frc/auton/guiauto/serialization/TrajectoryAutonomousStep.java +++ b/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(); } } diff --git a/src/main/java/frc/auton/guiauto/serialization/command/SendableCommand.java b/src/main/java/frc/auton/guiauto/serialization/command/SendableCommand.java index 1c13f4f7..8985b73c 100644 --- a/src/main/java/frc/auton/guiauto/serialization/command/SendableCommand.java +++ b/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; diff --git a/src/main/java/frc/auton/guiauto/serialization/command/SendableScript.java b/src/main/java/frc/auton/guiauto/serialization/command/SendableScript.java index 999297f4..2604e303 100644 --- a/src/main/java/frc/auton/guiauto/serialization/command/SendableScript.java +++ b/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 commands; + private final List commands; @JsonCreator public SendableScript(@JsonProperty("delayType") DelayType delayType, @JsonProperty("delay") double delay, - @JsonProperty("commands") ArrayList commands) { + @JsonProperty("commands") List 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 getCommands() { + public List getCommands() { return commands; } diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 691f5385..a7386b33 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/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; } } diff --git a/src/main/java/frc/utility/Serializer.java b/src/main/java/frc/utility/Serializer.java index 512c845a..737c7b93 100644 --- a/src/main/java/frc/utility/Serializer.java +++ b/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 {