State machine

controls/build.gradle

@@ -12,6 +12,11 @@ plugins {
 repositories {
 	// Use Maven Central for resolving dependencies.
 	mavenCentral()
+	maven {
+		// AdvantageKit maven repository
+		url "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+	}
+	gradlePluginPortal()
 }
 
 spotless {
@@ -42,16 +47,18 @@ compileJava.dependsOn 'spotlessApply'
 
 
 dependencies {
+	def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+	annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 	// Use JUnit Jupiter for testing.
 	testImplementation libs.junit.jupiter
-
 	testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
-
 	// This dependency is exported to consumers, that is to say found on their compile classpath.
 	api libs.commons.math3
 
 	// This dependency is used internally, and not exposed to consumers on their own compile classpath.
 	implementation libs.guava
+	implementation project(":parameter_tools")
+
 }
 
 // Apply a specific Java toolchain to ease working on different environments.
@@ -66,6 +73,12 @@ tasks.named('test') {
 	useJUnitPlatform()
 }
 
+
+
+test {
+	useJUnitPlatform()
+}
+
 compileJava.dependsOn 'spotlessApply'
 
 

controls/src/main/java/coppercore/controls/state_machine/StateMachine.java

@@ -0,0 +1,132 @@
+package coppercore.controls.state_machine;
+
+import coppercore.controls.state_machine.state.PeriodicStateInterface;
+import coppercore.controls.state_machine.state.StateConfiguration;
+import coppercore.controls.state_machine.state.StateContainer;
+import coppercore.controls.state_machine.state.StateInterface;
+import coppercore.controls.state_machine.transition.Transition;
+import coppercore.controls.state_machine.transition.TransitionInfo;
+import java.util.Optional;
+
+/** Generic State Machine */
+public class StateMachine<State, Trigger> {
+    private final StateMachineConfiguration<State, Trigger> configuration;
+    private TransitionInfo<State, Trigger> transitionInfo;
+    private State currentState;
+
+    /**
+     * Creates a StateMachine in the given state with the given configuration
+     *
+     * @param config The state machine configuration
+     * @param initialState default state
+     */
+    public StateMachine(StateMachineConfiguration<State, Trigger> config, State initialState) {
+        configuration = config;
+        currentState = initialState;
+    }
+
+    /**
+     * Method to transition States based on given trigger
+     *
+     * @param trigger Trigger event to run
+     */
+    public void fire(Trigger trigger) {
+        transitionInfo = new TransitionInfo<>(currentState, trigger);
+        Optional<Transition<State, Trigger>> transitionOptional =
+                configuration.getTransition(currentState, trigger);
+        if (transitionOptional.isEmpty()) {
+            transitionInfo.fail();
+            return;
+        }
+        Transition<State, Trigger> transition = transitionOptional.get();
+        if (!transition.canTransition()) {
+            transitionInfo.fail();
+            return;
+        }
+        transitionInfo.setTransition(transition);
+        if (!transition.isInternal()) {
+            Optional<StateConfiguration<State, Trigger>> currentStateConfigurationOptional =
+                    configuration.getStateConfiguration(currentState);
+            Optional<StateConfiguration<State, Trigger>> nextStateConfigurationOptional =
+                    configuration.getStateConfiguration(transition.getDestination());
+            if (currentStateConfigurationOptional.isPresent()) {
+                StateConfiguration<State, Trigger> config = currentStateConfigurationOptional.get();
+                if (config.doRunDefaultExitAction()) {
+                    configuration.runOnExit(transition);
+                } else {
+                    config.runOnExit(transition);
+                }
+            } else {
+                configuration.runOnExit(transition);
+            }
+            transition.runAction();
+            if (nextStateConfigurationOptional.isPresent()) {
+                StateConfiguration<State, Trigger> config = nextStateConfigurationOptional.get();
+                if (config.doRunDefaultExitAction()) {
+                    configuration.runOnEntry(transition);
+                } else {
+                    config.runOnEntry(transition);
+                }
+            } else {
+                configuration.runOnEntry(transition);
+            }
+        }
+        currentState = transition.getDestination();
+    }
+
+    /**
+     * Returns current state
+     *
+     * @return current state
+     */
+    public State getCurrentState() {
+        return currentState;
+    }
+
+    /** Runs states Period if is periodic */
+    public void periodic() {
+        if (currentState instanceof PeriodicStateInterface) {
+            ((PeriodicStateInterface) currentState).periodic();
+        } else {
+            periodicContainer();
+        }
+    }
+
+    /** Runs states Period if is periodic (This method is for if state is in Container) */
+    public void periodicContainer() {
+        if (currentState instanceof StateContainer) {
+            StateInterface state = ((StateContainer) currentState).getState();
+            if (state instanceof PeriodicStateInterface) {
+                ((PeriodicStateInterface) state).periodic();
+            }
+        }
+    }
+
+    /**
+     * Returns if last transition was successful
+     *
+     * @return success
+     */
+    public boolean successfulTransition() {
+        return !transitionInfo.wasFail();
+    }
+
+    /**
+     * Returns infomation about last transtion
+     *
+     * @return information of last transiton
+     */
+    public TransitionInfo<State, Trigger> getTransitionInfo() {
+        return transitionInfo;
+    }
+
+    /**
+     * Tests if in state
+     *
+     * @param state target state
+     * @return if in state
+     */
+    public boolean inState(State state) {
+        return currentState.equals(state);
+    }
+}

controls/src/main/java/coppercore/controls/state_machine/StateMachineConfiguration.java

@@ -0,0 +1,124 @@
+package coppercore.controls.state_machine;
+
+import coppercore.controls.state_machine.state.StateConfiguration;
+import coppercore.controls.state_machine.transition.Transition;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Optional;
+import java.util.function.Consumer;
+
+/** Object to configure State Machine */
+public class StateMachineConfiguration<State, Trigger> {
+    private final Map<State, StateConfiguration<State, Trigger>> stateConfigurations;
+    private Consumer<Transition<State, Trigger>> onEntryAction;
+    private Consumer<Transition<State, Trigger>> onExitAction;
+
+    /** Creates StateMachineConfiuration Object */
+    public StateMachineConfiguration() {
+        // temp solution
+        stateConfigurations = new HashMap<>();
+    }
+
+    /**
+     * Starts configuration of a state returning a StateConfiguration and registers it for the
+     * state.
+     *
+     * @param source Source State
+     * @return configuration
+     */
+    public StateConfiguration<State, Trigger> configure(State source) {
+        StateConfiguration<State, Trigger> configuration = stateConfigurations.get(source);
+        if (configuration == null) {
+            configuration = new StateConfiguration<>(source);
+            stateConfigurations.put(source, configuration);
+        }
+        return configuration;
+    }
+
+    /**
+     * Gets a StateConfiguration specified by State
+     *
+     * @param source Source State
+     * @return Optional configuration
+     */
+    public Optional<StateConfiguration<State, Trigger>> getStateConfiguration(State source) {
+        Optional<StateConfiguration<State, Trigger>> configurationOptional = Optional.empty();
+
+        if (stateConfigurations.containsKey(source)) {
+            StateConfiguration<State, Trigger> configuration = stateConfigurations.get(source);
+            if (configuration != null) {
+                configurationOptional = Optional.of(configuration);
+            }
+        }
+
+        return configurationOptional;
+    }
+
+    /**
+     * Gets a Transition defined by State and Trigger
+     *
+     * @param state Start state
+     * @param trigger Trigger event
+     * @return Optional of Transition
+     */
+    public Optional<Transition<State, Trigger>> getTransition(State state, Trigger trigger) {
+        Optional<Transition<State, Trigger>> transition = Optional.empty();
+        Optional<StateConfiguration<State, Trigger>> configurationOptional =
+                getStateConfiguration(state);
+
+        if (configurationOptional.isPresent()) {
+            StateConfiguration<State, Trigger> configuration = configurationOptional.get();
+            Optional<Transition<State, Trigger>> transitionOptional =
+                    configuration.getFilteredTransition(trigger);
+            transition = transition.or(() -> transitionOptional);
+        }
+
+        return transition;
+    }
+
+    /**
+     * Set the default onEntry function.
+     *
+     * @param action action to run onEntry
+     * @return configuration
+     */
+    public StateMachineConfiguration<State, Trigger> configureDefaultOnEntryAction(
+            Consumer<Transition<State, Trigger>> action) {
+        this.onEntryAction = action;
+        return this;
+    }
+
+    /**
+     * Set the default onExit function.
+     *
+     * @param action action to run onExit
+     * @return configuration
+     */
+    public StateMachineConfiguration<State, Trigger> configureDefaultOnExitAction(
+            Consumer<Transition<State, Trigger>> action) {
+        this.onExitAction = action;
+        return this;
+    }
+
+    /**
+     * Method used by statemachine to handle processing on entry of a state.
+     *
+     * @param transition Transition used
+     */
+    public void runOnEntry(Transition<State, Trigger> transition) {
+        if (onEntryAction != null) {
+            onEntryAction.accept(transition);
+        }
+    }
+
+    /**
+     * Method used by statemachine to handle processing on exiting of a state.
+     *
+     * @param transition Transition used
+     */
+    public void runOnExit(Transition<State, Trigger> transition) {
+        if (onExitAction != null) {
+            onExitAction.accept(transition);
+        }
+    }
+}

controls/src/main/java/coppercore/controls/state_machine/state/PeriodicStateInterface.java

@@ -0,0 +1,8 @@
+package coppercore.controls.state_machine.state;
+
+/** Periodic State Base */
+public interface PeriodicStateInterface extends StateInterface {
+
+    /** Method run on subsystem periodics (Does not get run automaticly) */
+    public default void periodic() {}
+}