make command groups better

RobotLibrary/src/main/java/com/technototes/library/command/Command.java

@@ -74,10 +74,11 @@ public void end(boolean cancel) {
      * @param command The next command
      * @return A {@link SequentialCommandGroup} for this and the added command
      */
+    @Deprecated
     public final Command andThen(Command command) {
         if (command instanceof SequentialCommandGroup) {
             SequentialCommandGroup c2 = new SequentialCommandGroup(this);
-            c2.commands.addAll(((SequentialCommandGroup) command).commands);
+            //c2.commands.addAll(((SequentialCommandGroup) command).commands);
             return c2;
         }
         return new SequentialCommandGroup(this, command);
@@ -89,6 +90,7 @@ public final Command andThen(Command command) {
      * @param command The command
      * @return A {@link SequentialCommandGroup} with the condition as a condition for the command
      */
+    @Deprecated
     public final Command waitUntil(BooleanSupplier condition, Command command) {
         return andThen(new ConditionalCommand(condition, command));
     }

RobotLibrary/src/main/java/com/technototes/library/command/CommandGroup.java

@@ -8,7 +8,7 @@
  * @author Alex Stedman
  */
 public abstract class CommandGroup extends Command {
-    protected List<Command> commands = new LinkedList<>();
+    protected Command[] commands;
 
     /** Basic constructor
      *
@@ -21,7 +21,7 @@ public CommandGroup(){
      * @param command Commands for group
      */
     public CommandGroup(Command... command) {
-        commands.addAll(Arrays.asList(command));
+        commands = command;
     }
 
     /** Add a command to the group
@@ -30,30 +30,43 @@ public CommandGroup(Command... command) {
      * @return this
      */
     public CommandGroup addCommand(Command command) {
-        commands.add(command);
+        Command[] n = new Command[commands.length+1];
+        for(int i = 0; i < commands.length; i++){
+            n[i]=commands[i];
+        }
+        n[n.length-1] = command;
+        commands = n;
         return this;
     }
 
     @Override
     public void run() {
         switch (commandState) {
+            case RESET:
+                commandRuntime.reset();
+                commandState = CommandState.INITIALIZED;
+            case INITIALIZED:
+                runCommands();
+                commandState = isFinished() ? CommandState.EXECUTED : CommandState.INITIALIZED;
+                if(commandState != CommandState.EXECUTED){
+                    return;
+                }
             case EXECUTED:
                 commandState = CommandState.RESET;
                 return;
-            default:
-                runCommands();
-                commandState = isFinished() ? CommandState.EXECUTED : CommandState.INITIALIZED;
         }
     }
 
+
     /** Run the commands
      *
      */
     public abstract void runCommands();
 
     @Override
-    /** Return if tis is finished
+    /** Return if its is finished
      *
      */
     public abstract boolean isFinished();
+
 }

RobotLibrary/src/main/java/com/technototes/library/command/CommandScheduler.java

@@ -88,7 +88,7 @@ public void run(Command command, BooleanSupplier supplier){
     }
     public void cancel(Command command){
         if(command != null) {
-            command.end(!command.isFinished());
+            command.end(true);
             command.commandState = Command.CommandState.RESET;
         }
     }

RobotLibrary/src/main/java/com/technototes/library/command/ParallelCommandGroup.java

@@ -24,7 +24,10 @@ public ParallelCommandGroup(Command... commands) {
      */
     @Override
     public void runCommands() {
-        commands.forEach(command -> run());
+        for(Command c : commands){
+            c.run();
+        }
+
     }
 
     /** Is this finished
@@ -34,7 +37,7 @@ public void runCommands() {
     @Override
     public boolean isFinished() {
         for (Command c : commands) {
-            if (!c.isFinished()) {
+            if (c.commandState != CommandState.RESET) {
                 return false;
             }
         }

RobotLibrary/src/main/java/com/technototes/library/command/ParallelRaceGroup.java

@@ -23,17 +23,19 @@ public ParallelRaceGroup(Command... commands) {
      */
     @Override
     public void runCommands() {
-        commands.forEach(command -> run());
-    }
+        for(Command c : commands){
+            c.run();
+        }
 
+    }
     /** Is this finished
      *
      * @return If one of the commands is finished
      */
     @Override
     public boolean isFinished() {
         for (Command c : commands) {
-            if (c.isFinished()) {
+            if (c.commandState == CommandState.RESET) {
                 return true;
             }
         }

RobotLibrary/src/main/java/com/technototes/library/command/SequentialCommandGroup.java

@@ -26,8 +26,7 @@ public SequentialCommandGroup(Command... commands) {
      */
     @Override
     public void runCommands() {
-        getCurrentCommand().run();
-
+        commands[currentCommandIndex].run();
     }
 
     /** Returns if all the commands are finished
@@ -36,18 +35,10 @@ public void runCommands() {
      */
     @Override
     public boolean isFinished() {
-        if (getCurrentCommand().isFinished()) {
+        if (commands[currentCommandIndex].commandState == CommandState.RESET) {
             currentCommandIndex++;
         }
-        //if the command goes one above index, the 0 index will equal the 1 index
-        return currentCommandIndex == commands.size();
+        return currentCommandIndex > commands.length-1;
     }
 
-    /** Get the command being currently run
-     *
-     * @return The current command
-     */
-    public Command getCurrentCommand() {
-        return commands.get(currentCommandIndex);
-    }
 }

RobotLibrary/src/main/java/com/technototes/library/command/WaitCommand.java

@@ -16,6 +16,6 @@ public WaitCommand(double sec){
 
     @Override
     public boolean isFinished() {
-        return seconds > commandRuntime.seconds();
+        return seconds <= commandRuntime.seconds();
     }
 }

RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/GyroSensor.java

@@ -6,9 +6,13 @@ public GyroSensor(com.qualcomm.robotcore.hardware.GyroSensor device) {
         super(device);
         device.calibrate();
     }
+    public GyroSensor(String deviceName){
+        super(deviceName);
+        getDevice().calibrate();
+    }
 
     @Override
     public double getSensorValue() {
-        return getDevice().getRotationFraction()*360;
+        return getDevice().getHeading();
     }
 }