Allow configuring a command to be run on entering state

Author: Tony Brar

core/src/main/java/org/teamtators/rotator/config/TriggerBinder.java

@@ -3,22 +3,26 @@
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.databind.JsonNode;
 import com.fasterxml.jackson.databind.ObjectMapper;
+import com.google.common.collect.HashMultimap;
+import com.google.common.collect.Multimap;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.teamtators.rotator.operatorInterface.AbstractOperatorInterface;
 import org.teamtators.rotator.operatorInterface.LogitechF310;
 import org.teamtators.rotator.scheduler.*;
 
 import javax.inject.Inject;
+import java.util.Collection;
 import java.util.Map;
 import java.util.Set;
 
-public class TriggerBinder {
+public class TriggerBinder implements StateListener {
     private static final Logger logger = LoggerFactory.getLogger(TriggerBinder.class);
     private Scheduler scheduler;
     private CommandStore commandStore;
     private ObjectMapper objectMapper;
     private AbstractOperatorInterface operatorInterface;
+    private Multimap<RobotState, Command> stateCommands = HashMultimap.create();
 
     @Inject
     public TriggerBinder() {
@@ -53,6 +57,14 @@ public void bindTriggers(TriggersConfig triggersConfig) {
         bindButtonsToLogitechF310(triggersConfig.driver, operatorInterface.driverJoystick());
         bindButtonsToLogitechF310(triggersConfig.gunner, operatorInterface.gunnerJoystick());
         registerDefaults(triggersConfig.defaults);
+        registerStateCommands(triggersConfig.stateCommands);
+    }
+
+    private void registerStateCommands(Map<RobotState, String> newStateCommands) {
+        for (Map.Entry<RobotState, String> commandEntry : newStateCommands.entrySet()) {
+            Command stateCommand = getCommandForBinding(commandEntry.getValue());
+            stateCommands.put(commandEntry.getKey(), stateCommand);
+        }
     }
 
     private void registerDefaults(Set<String> defaults) {
@@ -154,4 +166,12 @@ private Command getCommandForBinding(String commandName) {
         }
         return command;
     }
+
+    public void onEnterState(RobotState state) {
+        Collection<Command> thisStateCommands = stateCommands.get(state);
+        if (thisStateCommands == null) return;
+        for (Command stateCommand : thisStateCommands) {
+            scheduler.startCommand(stateCommand);
+        }
+    }
 }

core/src/main/java/org/teamtators/rotator/config/TriggersConfig.java

@@ -3,6 +3,7 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import com.fasterxml.jackson.databind.JsonNode;
 import org.teamtators.rotator.operatorInterface.LogitechF310;
+import org.teamtators.rotator.scheduler.RobotState;
 
 import java.util.Map;
 import java.util.Set;
@@ -13,4 +14,5 @@ public class TriggersConfig {
     @JsonProperty("Gunner")
     public Map<LogitechF310.Button, JsonNode> gunner;
     public Set<String> defaults;
+    public Map<RobotState, String> stateCommands;
 }

rio/src/main/java/org/teamtators/rotator/Robot.java

@@ -13,7 +13,10 @@
 import org.teamtators.rotator.control.Stepper;
 import org.teamtators.rotator.datalogging.DataCollector;
 import org.teamtators.rotator.operatorInterface.AbstractOperatorInterface;
-import org.teamtators.rotator.scheduler.*;
+import org.teamtators.rotator.scheduler.RobotState;
+import org.teamtators.rotator.scheduler.Scheduler;
+import org.teamtators.rotator.scheduler.StateListener;
+import org.teamtators.rotator.scheduler.Subsystem;
 import org.teamtators.rotator.tester.ITestable;
 import org.teamtators.rotator.tester.ManualTester;
 
@@ -25,7 +28,6 @@
 public class Robot extends IterativeRobot {
     private static final Logger logger = LoggerFactory.getLogger(Robot.class);
     private Scheduler scheduler;
-    private Command autoCommand;
 
     @Override
     public void startCompetition() {
@@ -94,8 +96,6 @@ private void initialize() {
         logger.debug("Starting stepper");
         stepper.start();
 
-        autoCommand = commandStore.getCommand("AutoInit");
-
         logger.info("Robot initialized");
     }
 
@@ -107,7 +107,6 @@ public void disabledInit() {
     @Override
     public void autonomousInit() {
         scheduler.enterState(RobotState.AUTONOMOUS);
-        scheduler.startCommand(autoCommand);
     }
 
     @Override