Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Pre-initialize autonomous Procedure #136

Open
wants to merge 1 commit into
base: mf3-dev
Choose a base branch
from
Open

Pre-initialize autonomous Procedure #136

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
92 changes: 92 additions & 0 deletions src/main/java/com/team766/framework3/AutonomousModeStateMachine.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
package com.team766.framework3;

import com.team766.logging.Category;
import com.team766.logging.Logger;
import com.team766.logging.Severity;
import edu.wpi.first.wpilibj2.command.Command;
import java.util.function.Supplier;

public class AutonomousModeStateMachine {
private enum AutonomousState {
/**
* m_autonomous has not been started yet.
* It can be scheduled the next time autonomousInit is called.
*/
NEW,
/**
* m_autonomous is currently running.
*/
SCHEDULED,
/**
* m_autonomous has finished, has been canceled, or is null.
* A new instance of the autonomous Command needs to be created before
* autonomous mode can be enabled.
*/
INVALID,
}

private final Supplier<AutonomousMode> selector;
private AutonomousMode m_autonMode = null;
private Command m_autonomous = null;
private AutonomousState m_autonState = AutonomousState.INVALID;

public AutonomousModeStateMachine(Supplier<AutonomousMode> selector) {
this.selector = selector;
}

public void stopAutonomousMode(final String reason) {
if (m_autonState == AutonomousState.SCHEDULED) {
m_autonomous.cancel();
m_autonState = AutonomousState.INVALID;
Logger.get(Category.AUTONOMOUS)
.logRaw(Severity.INFO, "Resetting autonomus procedure from " + reason);
}
}

private void refreshAutonomousMode() {
final AutonomousMode autonomousMode = selector.get();
if (m_autonMode != autonomousMode) {
stopAutonomousMode("selection of new autonomous mode " + autonomousMode);
m_autonState = AutonomousState.INVALID;
}
if (m_autonState == AutonomousState.INVALID && autonomousMode != null) {
m_autonomous = autonomousMode.instantiate();
m_autonMode = autonomousMode;
m_autonState = AutonomousState.NEW;
Logger.get(Category.AUTONOMOUS)
.logRaw(
Severity.INFO,
"Initialized new autonomus procedure " + m_autonomous.getName());
}
}

public void startAutonomousMode() {
refreshAutonomousMode();
switch (m_autonState) {
case INVALID -> {
Logger.get(Category.AUTONOMOUS)
.logRaw(Severity.WARNING, "No autonomous mode selected");
}
case SCHEDULED -> {
Logger.get(Category.AUTONOMOUS)
.logRaw(
Severity.INFO,
"Continuing previous autonomus procedure "
+ m_autonomous.getName());
}
case NEW -> {
m_autonomous.schedule();
m_autonState = AutonomousState.SCHEDULED;
Logger.get(Category.AUTONOMOUS)
.logRaw(
Severity.INFO,
"Starting new autonomus procedure " + m_autonomous.getName());
}
}
}

public void reinitializeAutonomousMode(final String reason) {
stopAutonomousMode(reason);
refreshAutonomousMode();
}
}
55 changes: 12 additions & 43 deletions src/main/java/com/team766/hal/GenericRobotMain3.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,21 +1,17 @@
package com.team766.hal;

import com.team766.framework3.AutonomousMode;
import com.team766.framework3.AutonomousModeStateMachine;
import com.team766.framework3.RuleEngine;
import com.team766.framework3.SchedulerMonitor;
import com.team766.framework3.SchedulerUtils;
import com.team766.library.RateLimiter;
import com.team766.logging.Category;
import com.team766.logging.Logger;
import com.team766.logging.Severity;
import com.team766.web.AutonomousSelector;
import com.team766.web.ConfigUI;
import com.team766.web.Dashboard;
import com.team766.web.DriverInterface;
import com.team766.web.LogViewer;
import com.team766.web.ReadLogs;
import com.team766.web.WebServer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

// Team 766 - Robot Interface Base class
Expand All @@ -26,9 +22,7 @@ public final class GenericRobotMain3 implements GenericRobotMainBase {
private RuleEngine m_lights;

private WebServer m_webServer;
private AutonomousSelector<AutonomousMode> m_autonSelector;
private AutonomousMode m_autonMode = null;
private Command m_autonomous = null;
private AutonomousModeStateMachine autonomous;

// Reset the autonomous routine if the robot is disabled for more than this
// number of seconds.
Expand All @@ -46,14 +40,15 @@ public GenericRobotMain3(RobotConfigurator3 configurator) {
SchedulerMonitor.start();

this.configurator = configurator;
m_autonSelector = new AutonomousSelector<>(configurator.getAutonomousModes());
var autonSelector = new AutonomousSelector<>(configurator.getAutonomousModes());
autonomous = new AutonomousModeStateMachine(autonSelector::getSelectedAutonMode);
m_webServer = new WebServer();
m_webServer.addHandler(new Dashboard());
m_webServer.addHandler(new DriverInterface(m_autonSelector));
m_webServer.addHandler(new DriverInterface(autonSelector));
m_webServer.addHandler(new ConfigUI());
m_webServer.addHandler(new LogViewer());
m_webServer.addHandler(new ReadLogs());
m_webServer.addHandler(m_autonSelector);
m_webServer.addHandler(autonSelector);
m_webServer.start();
}

Expand Down Expand Up @@ -89,14 +84,14 @@ public void disabledPeriodic() {
// when the communications are restored, we want to continue executing
// the routine that was interrupted, since it has knowledge of where the
// robot is on the field, the state of the robot's mechanisms, etc.
// Thus, we set a threshold on the amount of time spent in autonomous of
// Thus, we set a threshold on the amount of time spent in disabled of
// 10 seconds. It is almost certain that it will take longer than 10
// seconds to reset the field if a match is to be replayed, but it is
// also almost certain that a communication drop will be much shorter
// than 10 seconds.
double timeInState = RobotProvider.instance.getClock().getTime() - m_disabledModeStartTime;
if (timeInState > RESET_IN_DISABLED_PERIOD) {
resetAutonomousMode("time in disabled mode");
autonomous.reinitializeAutonomousMode("time in disabled mode");
}
CommandScheduler.getInstance().run();
if (m_lights != null && m_lightUpdateLimiter.next()) {
Expand All @@ -105,42 +100,20 @@ public void disabledPeriodic() {
}

public void resetAutonomousMode(final String reason) {
if (m_autonomous != null) {
m_autonomous.cancel();
m_autonomous = null;
m_autonMode = null;
Logger.get(Category.AUTONOMOUS)
.logRaw(Severity.INFO, "Resetting autonomus procedure from " + reason);
}
autonomous.reinitializeAutonomousMode(reason);
}

public void autonomousInit() {
faultInAutoInit = true;

if (m_autonomous != null) {
Logger.get(Category.AUTONOMOUS)
.logRaw(
Severity.INFO,
"Continuing previous autonomus procedure " + m_autonomous.getName());
} else if (m_autonSelector.getSelectedAutonMode() == null) {
Logger.get(Category.AUTONOMOUS).logRaw(Severity.WARNING, "No autonomous mode selected");
}
autonomous.startAutonomousMode();

faultInAutoInit = false;
}

public void autonomousPeriodic() {
if (faultInRobotInit || faultInAutoInit) return;

final AutonomousMode autonomousMode = m_autonSelector.getSelectedAutonMode();
if (autonomousMode != null && m_autonMode != autonomousMode) {
m_autonomous = autonomousMode.instantiate();
m_autonomous.schedule();
m_autonMode = autonomousMode;
Logger.get(Category.AUTONOMOUS)
.logRaw(
Severity.INFO,
"Starting new autonomus procedure " + m_autonomous.getName());
}
CommandScheduler.getInstance().run();
if (m_lights != null && m_lightUpdateLimiter.next()) {
m_lights.run();
Expand All @@ -150,11 +123,7 @@ public void autonomousPeriodic() {
public void teleopInit() {
faultInTeleopInit = true;

if (m_autonomous != null) {
m_autonomous.cancel();
m_autonomous = null;
m_autonMode = null;
}
autonomous.stopAutonomousMode("entering teleop");

faultInTeleopInit = false;
}
Expand Down
2 changes: 1 addition & 1 deletion src/test/java/com/team766/TestCase3.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
public abstract class TestCase3 {

@BeforeEach
public void setUp() {
public final void setUpFramework() {
assert HAL.initialize(500, 0);

ConfigFileTestUtils.resetStatics();
Expand Down
Loading
Loading