fix issues with running scripts/trajectories

varun7654 · Jan 7, 2024 · 07bca72 · 07bca72
1 parent fd1c604
commit 07bca72
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 2 deletions.
diff --git a/...rc/main/java/com/dacubeking/AutoBuilder/robot/serialization/TrajectoryAutonomousStep.java b/...rc/main/java/com/dacubeking/AutoBuilder/robot/serialization/TrajectoryAutonomousStep.java
@@ -19,6 +19,7 @@
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
 
+import java.util.ArrayList;
 import java.util.List;
 import java.util.concurrent.ExecutionException;
 
@@ -52,6 +53,9 @@ public TrajectoryAutonomousStep(@JsonProperty(required = true, value = "states")
         super(closed);
         this.reversed = reversed;
         this.color = color;
+        if (controlVectors == null) {
+            controlVectors = new ArrayList<>();
+        }
         this.controlVectors = controlVectors;
         this.rotations = rotations;
 
@@ -124,6 +128,7 @@ public boolean execute(@NotNull List<SendableScript> scriptsToExecuteByTime,
                     .apply(new TrajectoryBuilderInfo(trajectory, this::getTargetRotation));
             pathDriveCommand.schedule();
             timer.restart();
+            isFirstRun = false;
         }
 
         final double elapsedTime = timer.get();

diff --git a/.../src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableScript.java b/.../src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableScript.java
@@ -26,12 +26,15 @@ public boolean execute() throws CommandExecutionFailedException, ExecutionExcept
             return true;
         }
 
-        SendableCommand command = executionQueue.peek();
+        SendableCommand command = executionQueue.pollFirst();
 
 
         // Keep executing commands until we reach one we need to wait at.
         while (command.execute()) {
-            executionQueue.remove();
+            if (executionQueue.isEmpty()) {
+                return true;
+            }
+            command = executionQueue.pollFirst();
         }
 
         return executionQueue.isEmpty();