Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add WIP command schedule

Browse Source 
Preload PathPlanner autos
Remove old PathPlanner recorder (again)
Temporarily disable PhotonCamera version check
This commit is contained in:
nathanrsxtn
2022-04-05 17:54:01 -06:00
parent e90d709217
commit c224aa61a6
4 changed files with 142 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+6 -14
View File
@@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.utility.Commander;
import frc4388.utility.RobotTime;
import frc4388.utility.Vector2D;
@@ -57,7 +58,7 @@ public class Robot extends TimedRobot {
LOGGER.log(Level.FINE, "Logging Test 6/8");
LOGGER.log(Level.FINER, "Logging Test 7/8");
LOGGER.log(Level.FINEST, "Logging Test 8/8");
Commander.initialize();
// var path =
// PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move
// Forward.path").toFile());
@@ -93,7 +94,7 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
Commander.periodic();
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00);
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition);
@@ -137,17 +138,6 @@ public class Robot extends TimedRobot {
public void disabledInit() {
LOGGER.fine("disabledInit()");
m_robotTime.endMatchTime();
if (isTest()) {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner")
.resolve("recording." + System.currentTimeMillis() + ".path").toFile();
if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
// m_robotContainer.createPath(null, null, false).write(outputFile);
LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
} else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
}
@@ -179,7 +169,9 @@ public class Robot extends TimedRobot {
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {}
public void autonomousPeriodic() {
final int a = 1;
}
@Override
public void teleopInit() {
src/main/java/frc4388/robot/RobotContainer.java
+5 -2
View File
@@ -122,6 +122,9 @@ public class RobotContainer {
private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>();
private final SequentialCommandGroup autoJMove1 = buildAuto(3.0, 3.0, "JMove1");
private final SequentialCommandGroup autoJMove2 = buildAuto(3.0, 3.0, "JMove2");
/**
* SmartDash
* - Limelight cam X
@@ -625,11 +628,11 @@ public class RobotContainer {
// ! PathPlanner Testing
ParallelDeadlineGroup intakeWithPath1 = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 3.0, true),
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
buildAuto(3.0, 3.0, "JMove"));
autoJMove1);
ParallelDeadlineGroup intakeWithPath2 = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 5.0, true),
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
buildAuto(3.0, 3.0, "JMove2"));
autoJMove2);
ParallelCommandGroup extendWhileTurretIsAiming = new ParallelCommandGroup(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
ParallelCommandGroup intakeWithPathAndTrackTarget = new ParallelCommandGroup(intakeWithPath1, weirdAutoShootingGroup2);
src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+1
View File
@@ -51,6 +51,7 @@ public class VisionOdometry extends SubsystemBase {
setLEDs(false);
setDriverMode(false);
PhotonCamera.setVersionCheckEnabled(false);
}
/** Gets the vision points from the limelight
src/main/java/frc4388/utility/Commander.java
+130
View File
@@ -0,0 +1,130 @@
package frc4388.utility;
import java.lang.invoke.MethodHandle;
import java.lang.invoke.MethodHandles;
import java.lang.reflect.AccessibleObject;
import java.lang.reflect.Field;
import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Method;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
import java.util.Objects;
import java.util.Set;
import java.util.Map.Entry;
import java.util.function.BooleanSupplier;
import java.util.function.Function;
import java.util.function.Predicate;
import java.util.function.Supplier;
import java.util.stream.Collectors;
import com.diffplug.common.base.Errors;
import edu.wpi.first.math.Pair;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.LayoutType;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
public final class Commander {
private static MethodHandle m_scheduledCommandsHandle;
private static MethodHandle m_sequentialCommandGroupCommandsHandle;
private static MethodHandle m_sequentialCommandGroupCurrentCommandIndexHandle;
private static MethodHandle m_parallelCommandGroupCommandsHandle;
private static MethodHandle m_parallelDeadlineGroupCommandsHandle;
private static MethodHandle m_parallelRaceGroupCommandsHandle;
private static ShuffleboardTab tab;
public static void initialize() {
try {
Field m_scheduledCommandsField = CommandScheduler.class.getDeclaredField("m_scheduledCommands");
m_scheduledCommandsField.trySetAccessible();
m_scheduledCommandsHandle = MethodHandles.lookup().unreflectGetter(m_scheduledCommandsField);
Field m_sequentialCommandGroupCommandsField = SequentialCommandGroup.class.getDeclaredField("m_commands");
m_sequentialCommandGroupCommandsField.trySetAccessible();
m_sequentialCommandGroupCommandsHandle = MethodHandles.lookup().unreflectGetter(m_sequentialCommandGroupCommandsField);
Field m_sequentialCommandGroupCurrentCommandIndexField = SequentialCommandGroup.class.getDeclaredField("m_currentCommandIndex");
m_sequentialCommandGroupCurrentCommandIndexField.trySetAccessible();
m_sequentialCommandGroupCurrentCommandIndexHandle = MethodHandles.lookup().unreflectGetter(m_sequentialCommandGroupCurrentCommandIndexField);
Field m_parallelCommandGroupCommandsField = ParallelCommandGroup.class.getDeclaredField("m_commands");
m_parallelCommandGroupCommandsField.trySetAccessible();
m_parallelCommandGroupCommandsHandle = MethodHandles.lookup().unreflectGetter(m_parallelCommandGroupCommandsField);
Field m_parallelDeadlineGroupCommandsField = ParallelDeadlineGroup.class.getDeclaredField("m_commands");
m_parallelDeadlineGroupCommandsField.trySetAccessible();
m_parallelDeadlineGroupCommandsHandle = MethodHandles.lookup().unreflectGetter(m_parallelDeadlineGroupCommandsField);
Field m_parallelRaceGroupCommandsField = ParallelRaceGroup.class.getDeclaredField("m_commands");
m_parallelRaceGroupCommandsField.trySetAccessible();
m_parallelRaceGroupCommandsHandle = MethodHandles.lookup().unreflectGetter(m_parallelRaceGroupCommandsField);
} catch (IllegalArgumentException | NoSuchFieldException | SecurityException | IllegalAccessException e) {
e.printStackTrace();
}
tab = Shuffleboard.getTab("Commander");
try {
scheduledCommands = ((LinkedHashMap<Command, Object>) m_scheduledCommandsHandle.invoke(CommandScheduler.getInstance()));
} catch (Throwable e) {
e.printStackTrace();
}
}
private static LinkedHashMap<Command, Object> scheduledCommands;
public static void periodic() {
int count = scheduledCommands.size();
for (Command command : scheduledCommands.keySet()) {
putCommand(command, null, count, command::isScheduled);
}
}
private static int count = 0;
private static void putCommand(Command command, ShuffleboardContainer layout, int siblings, BooleanSupplier running) {
String name = (count++) + ":" + command.getClass().getSimpleName()/* + "@" + Integer.toHexString(command.hashCode()) */;
if (Objects.requireNonNullElse(layout, tab).getComponents().stream().map(ShuffleboardComponent::getTitle).anyMatch(name::equals)) return;
if (command instanceof CommandGroupBase) {
Collection<Command> commands = List.of();
String glyph = "CUBE";
Function<Command, BooleanSupplier> nestedRunningMaker = c -> () -> !c.isFinished();
if (command instanceof SequentialCommandGroup) {
ArrayList<Command> commandsList = Errors.log().getWithDefault(() -> (ArrayList<Command>) m_sequentialCommandGroupCommandsHandle.invoke(command), new ArrayList<>());
commands = commandsList;
nestedRunningMaker = c -> () -> Errors.log().getWithDefault(() -> (int) m_sequentialCommandGroupCurrentCommandIndexHandle.invoke(command) == commandsList.indexOf(c), false);
glyph = "LIST_OL";
} else if (command instanceof ParallelCommandGroup) {
HashMap<Command, Boolean> commandsMap = Errors.log().getWithDefault(() -> (HashMap<Command, Boolean>) m_parallelCommandGroupCommandsHandle.invoke(command), new HashMap<Command, Boolean>());
commands = commandsMap.keySet();
nestedRunningMaker = c -> () -> commandsMap.get(c);
glyph = "LIST_UL";
} else if (command instanceof ParallelDeadlineGroup) {
HashMap<Command, Boolean> commandsMap = Errors.log().getWithDefault(() -> (HashMap<Command, Boolean>) m_parallelDeadlineGroupCommandsHandle.invoke(command), new HashMap<Command, Boolean>());
commands = commandsMap.keySet();
nestedRunningMaker = c -> () -> commandsMap.get(c);
glyph = "CROSSHAIRS";
} else if (command instanceof ParallelRaceGroup) {
commands = Errors.log().getWithDefault(() -> (HashSet<Command>) m_parallelRaceGroupCommandsHandle.invoke(command), new HashSet<>());
nestedRunningMaker = c -> () -> !command.isFinished();
glyph = "RANDOM";
}
ShuffleboardLayout nestedLayout = Objects.requireNonNullElse(layout, tab).getLayout(name, layout == null ? BuiltInLayouts.kList : BuiltInLayouts.kGrid).withSize(11 / siblings, layout == null ? 6 : 1).withProperties(Map.of("Number of columns", 11 / siblings, "Number of rows", 1, "Label position", "BOTTOM", "Show Glyph", true, "Glyph", glyph));
int count = commands.size();
for (Command nestedCommand : commands) {
putCommand(nestedCommand, nestedLayout, count, nestedRunningMaker.apply(nestedCommand));
}
} else if (command instanceof CommandBase) {
Objects.requireNonNullElse(layout, tab).addBoolean(name, running);
}
}
}