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 auto path preloading

Browse Source 
- Refactor recording to use commands
This commit is contained in:
nathanrsxtn
2022-02-16 22:31:00 -07:00
parent 3cb4a13bb1
commit 9ccf4bcea2
17 changed files with 1870 additions and 1463 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+43 -32
View File
@@ -5,22 +5,23 @@
package frc4388.robot;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.stream.StreamSupport;
import com.diffplug.common.base.Errors;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc4388.utility.PathPlannerUtil;
import frc4388.utility.RobotLogger;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.utility.RobotTime;
import frc4388.utility.PathPlannerUtil.Path;
/**
* The VM is configured to automatically run this class, and to call the
@@ -61,6 +62,33 @@ public class Robot extends TimedRobot {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
// addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
SmartDashboard.putData(CommandScheduler.getInstance());
SmartDashboard.putData("JVM Memory", new RunCommand(() -> {}) {
@Override public boolean runsWhenDisabled() { return true; }
@Override public String getName() {
if (isScheduled()) {
Runtime runtime = Runtime.getRuntime();
long totalMemory = runtime.totalMemory() / 1_000_000;
long freeMemory = runtime.freeMemory() / 1_000_000;
long maxMemory = runtime.maxMemory() / 1_000_000;
return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB";
}
return "Not Running";
}
});
SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {}) {
@Override public boolean runsWhenDisabled() { return true; }
@Override public String getName() {
if (isScheduled()) {
File deploy = Filesystem.getDeployDirectory();
long usedSpace = Errors.suppress().getWithDefault(() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), 0l) / 1_000_000;
long usableSpace = deploy.getUsableSpace() / 1_000_000;
return usedSpace + " MB / " + usableSpace + " MB";
}
return "Not Running";
}
});
}
/**
@@ -94,26 +122,13 @@ public class Robot extends TimedRobot {
LOGGER.fine("disabledInit()");
m_robotTime.endMatchTime();
if (isTest()) {
try {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
Path path = RobotLogger.getInstance().createPath(0.1, 0.1, false);
String pathPath = "recording." + System.currentTimeMillis() + ".path";
File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve(pathPath).toFile();
outputFile.createNewFile();
path.write(outputFile);
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "Recorded path to {0} in the deploy directory on the RoboRIO", pathPath);
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
// LOGGER.finest(path::toString);
} catch (IOException e) {
e.printStackTrace();
}
// 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());
}
}
@@ -130,12 +145,7 @@ public class Robot extends TimedRobot {
@Override
public void autonomousInit() {
LOGGER.fine("autonomousInit()");
try {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
@@ -176,6 +186,7 @@ public class Robot extends TimedRobot {
@Override
public void testInit() {
CommandScheduler.getInstance().cancelAll();
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+185 -56
View File
@@ -6,14 +6,27 @@ package frc4388.robot;
import java.io.File;
import java.io.IOException;
import java.nio.file.FileSystem;
import java.nio.file.FileSystems;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.StandardWatchEventKinds;
import java.nio.file.WatchEvent;
import java.nio.file.WatchKey;
import java.nio.file.WatchService;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import java.util.Optional;
import java.util.function.Function;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.regex.Matcher;
import java.util.regex.Pattern;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.diffplug.common.base.Errors;
import com.diffplug.common.base.Errors.Plugins.Log;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
@@ -23,12 +36,23 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.command.PrintCommand;
import edu.wpi.first.wpilibj.shuffleboard.EventImportance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
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.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.NotifierCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -38,6 +62,9 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.LEDPatterns;
import frc4388.utility.PathPlannerUtil;
import frc4388.utility.SendableRunChooser;
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
import frc4388.utility.controller.DeadbandedXboxController;
/**
@@ -48,6 +75,7 @@ import frc4388.utility.controller.DeadbandedXboxController;
* commands, and button mappings) should be declared here.
*/
public class RobotContainer {
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getName());
/* RobotMap */
private final RobotMap m_robotMap = new RobotMap();
@@ -55,17 +83,28 @@ public class RobotContainer {
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
private final TalonFX m_testMotor = new TalonFX(23);
private final LED m_robotLED = new LED(m_robotMap.LEDController);
/* Controllers */
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private String loadedPathName = "";
private PathPlannerTrajectory loadedPathTrajectory = null;
private static final Function<CharSequence, String> pathExtensionRemover = ((Function<CharSequence, Matcher>) Pattern
.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
private final SendableRunChooser<File> autoChooser = new SendableRunChooser<>(selectedAuto -> {
if (selectedAuto != null && !selectedAuto.equals(loadedPathName)) {
loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto), 5.5, 50);
loadedPathName = selectedAuto;
}
});
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
@SuppressWarnings("unchecked")
public RobotContainer() {
configureButtonBindings();
/* Default Commands */
@@ -77,14 +116,67 @@ public class RobotContainer {
getDriverController().getRightX(),
getDriverController().getRightY(),
true),
m_robotSwerveDrive));
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
try (WatchService watcher = FileSystems.getDefault().newWatchService()) {
WatchKey watchKey = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").register(
FileSystems.getDefault().newWatchService(),
StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
StandardWatchEventKinds.ENTRY_DELETE);
new NotifierCommand(() -> {}, TimedRobot.kDefaultPeriod) {
@Override
public void execute() {
var selectedAuto = autoChooser.getSelected();
if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) {
setName("Path Watcher: Loading Path");
// loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, 50);
loadedPathName = selectedAuto.getName();
}
if (!watchKey.pollEvents().isEmpty()) {
updateAutoChooser();
LOGGER.info("Updated autonomous chooser.");
}
if (!watchKey.reset())
LOGGER.severe("File watch key invalid.");
setName("Path Watcher: Waiting");
}
m_testMotor.set(TalonFXControlMode.PercentOutput, 0);
@Override
public boolean runsWhenDisabled() {
return true;
}
}.schedule();
pathLoaderCommand.schedule();
} catch (IOException exception) {
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
}
updateAutoChooser();
recordInit();
}
private Command pathLoaderCommand = new CommandBase() {
@Override
public void execute() {
var selectedAuto = autoChooser.getSelected();
if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) {
setName("LOADING PATH");
Thread thread = new Thread(() -> {
loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, 50);
setName("Path loader waiting");
}, "Path Loader Thread");
thread.start();
}
}
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Path loader");
private Thread pathLoaderThread = new Thread();
/**
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
@@ -108,7 +200,7 @@ public class RobotContainer {
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
/* Operator Buttons */
// activates "Lit Mode"
@@ -118,59 +210,39 @@ public class RobotContainer {
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
}
public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) {
ArrayList<Command> commands = new ArrayList<>();
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
thetaController.enableContinuousInput(-Math.PI, Math.PI);
// parse input
for (int i = 0; i < inputs.length; i++) {
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
commands.add(new PPSwerveControllerCommand(traj, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive));
} else if (inputs[i] instanceof Command)
commands.add((Command) inputs[i]);
}
commands.add(new InstantCommand(m_robotSwerveDrive::stopModules));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
* @throws IOException
*/
public Command getAutonomousCommand() throws IOException {
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
public Command getAutonomousCommand() {
var selectedAuto = autoChooser.getSelected();
if (loadedPathTrajectory != null) {
// if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName))
// loadedPathTrajectory =
// PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5,
// 50);
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
thetaController.enableContinuousInput(-Math.PI, Math.PI);
// Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down");
// SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands);
// return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)))));
Optional<File> recordedAuto = Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles()).filter(f -> f.getName().startsWith("recording")).max(Comparator.comparingLong(File::lastModified));
if (recordedAuto.isPresent()) {
String name = recordedAuto.orElseThrow().getName().replace(".path", "");
Logger.getLogger(this.getClass().getName()).fine(name);
return new SequentialCommandGroup(buildAuto(1.0, 1.0, name));
PathPlannerState initialState = loadedPathTrajectory.getInitialState();
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
return new SequentialCommandGroup(
new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
} else {
Logger.getLogger(this.getClass().getName()).severe("No auto selected.");
return new RunCommand(() -> {
}).withName("No Autonomous Path");
}
Logger.getLogger(this.getClass().getName()).severe("No recorded autos found.");
return new InstantCommand();
// return new PrintCommand("No recorded autos found.");
}
/**
* Add your docs here.
*/
public XboxController getDriverController() {
return m_driverXbox;
}
@@ -183,10 +255,67 @@ public class RobotContainer {
m_robotSwerveDrive.resetOdometry(pose);
}
/**
* Add your docs here.
*/
public XboxController getOperatorController() {
return m_operatorXbox;
}
private void updateAutoChooser() {
Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles())
.filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
.forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
SmartDashboard.putData("Auto Chooser", autoChooser);
}
private final List<Waypoint> pathPoints = new ArrayList<>();
public void recordInit() {
SmartDashboard.putData("Recording",
new RunCommand(this::recordPeriodic) {
@Override
public void end(boolean interupted) {
new InstantCommand(RobotContainer.this::saveRecording) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Save Recording").schedule();
}
}.withName("Record Path (Cancel to Save)"));
}
private void saveRecording() {
// 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))) {
createPath(null, null, false).write(outputFile);
autoChooser.setDefaultOption(outputFile.getName(), outputFile);
LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
} else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
public void recordPeriodic() {
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond,
m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, velocity.getNorm(),
false);
pathPoints.add(waypoint);
}
public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
PathPlannerUtil.Path path = new PathPlannerUtil.Path();
for (int i = 0; i < pathPoints.size() - 2; i++)
pathPoints.get(i).nextControl = pathPoints.get(i + 1).anchorPoint;
for (int i = 1; i < pathPoints.size() - 1; i++)
pathPoints.get(i).prevControl = pathPoints.get(i - 1).anchorPoint;
path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
path.maxVelocity = Optional.ofNullable(maxVelocity);
path.maxAcceleration = Optional.ofNullable(maxAcceleration);
path.isReversed = Optional.ofNullable(isReversed);
pathPoints.clear();
return path;
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+3 -22
View File
@@ -4,8 +4,8 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -23,8 +23,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import frc4388.utility.RobotLogger;
import frc4388.utility.PathPlannerUtil.Path.Waypoint.Point;
public class SwerveDrive extends SubsystemBase {
@@ -57,7 +55,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
public Rotation2d rotTarget = new Rotation2d();;
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
private final Field2d m_field = new Field2d();
@@ -160,23 +158,7 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
// System.err.println(m_gyro.getFusedHeading() +" aaa");
updateOdometry();
// m_gyro.setFusedHeadingToCompass();
// m_gyro.setYawToCompass();
Pose2d pos = m_poseEstimator.getEstimatedPosition();
RobotLogger.getInstance().put(
/* anchorPointX */ pos.getX(),
/* anchorPointY */ pos.getY(),
/* prevControlX */ pos.getX(),
/* prevControlY */ pos.getY(),
/* nextControlX */ pos.getX(),
/* nextControlY */ pos.getY(),
/* holonomicAngle */ m_gyro.getRotation2d().getDegrees(),
/* isReversal */ false,
/* velOverride */ null,
/* isLocked */ false
);
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
@@ -186,8 +168,7 @@ public class SwerveDrive extends SubsystemBase {
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
super.periodic();
}
src/main/java/frc4388/utility/AnsiLogging.java
+8 -2
View File
@@ -19,6 +19,10 @@ import java.util.logging.Logger;
import org.fusesource.jansi.Ansi;
import org.fusesource.jansi.Ansi.Attribute;
import org.fusesource.jansi.Ansi.Color;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.fusesource.jansi.AnsiConsole;
public class AnsiLogging extends ConsoleHandler {
@@ -35,7 +39,7 @@ public class AnsiLogging extends ConsoleHandler {
// Replaces standard output streams with org.fusesource.jansi.AnsiPrintStreams.
AnsiConsole.systemInstall();
// Replaces standard output stream with java.util.logging.Logger.
System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO));
System.setOut(printStreamLogger(Logger.getGlobal(), Level.ALL));
// Replaces standard error output stream with java.util.logging.Logger.
System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE));
} catch (IOException exception) {
@@ -66,6 +70,7 @@ public class AnsiLogging extends ConsoleHandler {
} else
source = logRecord.getLoggerName();
String message = formatMessage(logRecord);
boolean multiline = message.contains("\n");
String throwable = "";
if (logRecord.getThrown() != null) {
StringWriter sw = new StringWriter();
@@ -74,6 +79,7 @@ public class AnsiLogging extends ConsoleHandler {
logRecord.getThrown().printStackTrace(pw);
}
throwable = sw.toString();
multiline = true;
}
Ansi ansi;
if (logRecord.getLevel() == Level.SEVERE) ansi = ansi().fgBright(Color.RED);
@@ -84,7 +90,7 @@ public class AnsiLogging extends ConsoleHandler {
else if (logRecord.getLevel() == Level.FINER) ansi = ansi().fg(Color.MAGENTA);
else if (logRecord.getLevel() == Level.FINEST) ansi = ansi().fgBright(Color.BLACK);
else ansi = ansi().fg(Color.DEFAULT);
String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%n%5$s%6$s").reset().a("%n").toString();
String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a(multiline ? "%n%5$s%6$s" : " %5$s%6$s").reset().a("%n").toString();
return String.format(format, zdt, source, logRecord.getLoggerName(), logRecord.getLevel().getLocalizedName(), message, throwable);
}
};
src/main/java/frc4388/utility/PathPlannerUtil.java
+31 -40
View File
@@ -2,53 +2,26 @@ package frc4388.utility;
import java.io.File;
import java.util.Optional;
import com.diffplug.common.base.Errors;
import com.fasterxml.jackson.annotation.JsonInclude.Include;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.datatype.jdk8.Jdk8Module;
public class PathPlannerUtil {
import edu.wpi.first.math.geometry.Translation2d;
public final class PathPlannerUtil {
public static final class Path {
public Optional<Waypoint>[] waypoints;
public static final class Waypoint {
public Waypoint() {}
public Waypoint(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
this.anchorPoint = anchorPointX == null && anchorPointY == null ? Optional.empty() : Optional.of(Point.of(anchorPointX, anchorPointY));
this.prevControl = prevControlX == null && prevControlY == null ? Optional.empty() : Optional.of(Point.of(prevControlX, prevControlY));
this.nextControl = nextControlX == null && nextControlY == null ? Optional.empty() : Optional.of(Point.of(nextControlX, nextControlY));
this.holonomicAngle = Optional.ofNullable(holonomicAngle);
this.isReversal = Optional.ofNullable(isReversal);
this.velOverride = Optional.ofNullable(velOverride);
this.isLocked = Optional.ofNullable(isLocked);
}
public Optional<Point> anchorPoint;
public Optional<Point> prevControl;
public Optional<Point> nextControl;
public static final class Point {
public Point() {}
public static Point of(Double x, Double y) {
Point point = new Point();
point.x = Optional.ofNullable(x);
point.y = Optional.ofNullable(y);
return point;
}
public Optional<Double> x;
public Optional<Double> y;
}
public Optional<Double> holonomicAngle;
public Optional<Boolean> isReversal;
public Optional<Double> velOverride;
public Optional<Boolean> isLocked;
}
public Optional<Waypoint[]> waypoints;
public Optional<Double> maxVelocity;
public Optional<Double> maxAcceleration;
public Optional<Boolean> isReversed;
private static final ObjectMapper objectMapper = new ObjectMapper();
static { objectMapper.registerModule(new Jdk8Module()); }
static {
objectMapper.registerModule(new Jdk8Module());
objectMapper.setSerializationInclusion(Include.NON_EMPTY);
}
public static Path read(File src) {
return Errors.log().getWithDefault(() -> objectMapper.readValue(src, Path.class), null);
@@ -58,9 +31,27 @@ public class PathPlannerUtil {
Errors.log().run(() -> objectMapper.writeValue(resultFile, this));
}
@Override
public String toString() {
return Errors.log().getWithDefault(() -> objectMapper.writeValueAsString(this), super.toString());
public static final class Waypoint {
public Optional<Translation2d> anchorPoint;
public Optional<Translation2d> prevControl;
public Optional<Translation2d> nextControl;
public Optional<Double> holonomicAngle;
public Optional<Boolean> isReversal;
public Optional<Double> velOverride;
public Optional<Boolean> isLocked;
public Waypoint() {
}
public Waypoint(Translation2d anchorPoint, Translation2d prevControl, Translation2d nextControl, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
this.anchorPoint = Optional.ofNullable(anchorPoint);
this.prevControl = Optional.ofNullable(prevControl);
this.nextControl = Optional.ofNullable(nextControl);
this.holonomicAngle = Optional.ofNullable(holonomicAngle);
this.isReversal = Optional.ofNullable(isReversal);
this.velOverride = Optional.ofNullable(velOverride);
this.isLocked = Optional.ofNullable(isLocked);
}
}
}
}
src/main/java/frc4388/utility/RobotGyro.java.old
-245
View File
@@ -1,245 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.utility;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.interfaces.Gyro;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
*/
public class RobotGyro implements Gyro, PIDSource, Sendable {
private RobotTime m_robotTime = RobotTime.getInstance();
private PigeonIMU m_pigeon = null;
private AHRS m_navX = null;
public boolean m_isGyroAPigeon; // true if pigeon, false if navX
private double m_lastPigeonAngle;
private double m_deltaPigeonAngle;
/**
* Creates a Gyro based on a pigeon
*
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(PigeonIMU gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
/**
* Creates a Gyro based on a navX
*
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(AHRS gyro) {
m_navX = gyro;
m_isGyroAPigeon = false;
}
/**
* Run in periodic if you are using a pigeon. Updates a delta angle so that it
* can calculate getRate(). Note
* that the getRate() method for a navX will likely be much more accurate than
* for a pigeon.
*/
public void updatePigeonDeltas() {
double currentPigeonAngle = getAngle();
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
m_lastPigeonAngle = currentPigeonAngle;
}
/**
* <p>
* NavX:
* <p>
* Calibrate the gyro by running for a number of samples and computing the
* center value. Then use
* the center value as the Accumulator center value for subsequent measurements.
* It's important to
* make sure that the robot is not moving while the centering calculations are
* in progress, this
* is typically done when the robot is first turned on while it's sitting at
* rest before the
* competition starts.
*
* <p>
* Pigeon:
* <p>
* Calibrate the gyro by collecting data at a range of tempuratures. Allow
* pigeon to cool, then boot
* into calibration mode. For faster calibration, use a heat lamp to heat up the
* pigeon. Once the pigeon
* has seen a reasonable range of tempuratures, it will exit calibration mode.
* It's important to
* make sure that the robot is not moving while the tempurature calculations are
* in progress, this
* is typically done when the robot is first turned on while it's sitting at
* rest before the
* competition starts.
*/
@Override
public void calibrate() {
if (m_isGyroAPigeon)
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
else
m_navX.calibrate();
}
@Override
public void reset() {
if (m_isGyroAPigeon)
m_pigeon.setYaw(0);
else
m_navX.reset();
}
/**
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] angles = new double[3];
m_pigeon.getYawPitchRoll(angles);
return angles;
}
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
} else {
return m_navX.getAngle();
}
}
/**
* Gets an absolute heading of the robot
*
* @return heading from -180 to 180 degrees
*/
public double getHeading() {
return getHeading(getAngle());
}
/**
* Gets an absolute heading of the robot
*
* @return heading from -180 to 180 degrees
*/
public double getHeading(double angle) {
return Math.IEEEremainder(angle, 360);
}
/**
* Returns the current pitch value (in degrees, from -90 to 90)
* reported by the sensor. Pitch is a measure of rotation around
* the Y Axis.
*
* @return The current pitch value in degrees (-90 to 90).
*/
public double getPitch() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
} else {
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
}
}
/**
* Returns the current roll value (in degrees, from -90 to 90)
* reported by the sensor. Roll is a measure of rotation around
* the X Axis.
*
* @return The current roll value in degrees (-90 to 90).
*/
public double getRoll() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
} else {
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
}
}
@Override
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
} else {
return m_navX.getRate();
}
}
public PigeonIMU getPigeon() {
return m_pigeon;
}
public AHRS getNavX() {
return m_navX;
}
@Override
public void close() throws Exception {
}
// Begin old GyroBase class
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Set which parameter of the gyro you are using as a process control variable.
* The Gyro class
* supports the rate and displacement parameters
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the output of the gyro for use with PIDControllers. May be the angle or
* rate depending on
* the set PIDSourceType
*
* @return the output according to the gyro
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kRate:
return getRate();
case kDisplacement:
return getAngle();
default:
return 0.0;
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}
src/main/java/frc4388/utility/RobotLogger.java
-51
View File
@@ -1,51 +0,0 @@
package frc4388.utility;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
public final class RobotLogger {
private static RobotLogger instance = null;
public static RobotLogger getInstance() {
return Objects.requireNonNullElseGet(instance, () -> instance = new RobotLogger());
}
public static double getTime() {
return DriverStation.getMatchTime();
}
private RobotLogger() {
sendableChooser.setDefaultOption("Disable", false);
sendableChooser.addOption("Enable", true);
SmartDashboard.putData("Recording", sendableChooser);
}
private final List<Waypoint> data = new ArrayList<>();
private final SendableChooser<Boolean> sendableChooser = new SendableChooser<>();
double lastVelocityMetersPerSecond = 0;
public void put(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
if (Boolean.TRUE.equals(sendableChooser.getSelected())) {
Waypoint waypoint = new Waypoint(anchorPointX, anchorPointY, prevControlX, prevControlY, nextControlX, nextControlY, holonomicAngle, isReversal, velOverride, isLocked);
data.add(waypoint);
}
}
@SuppressWarnings("unchecked")
public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
PathPlannerUtil.Path path = new PathPlannerUtil.Path();
path.waypoints = data.stream().map(Optional::ofNullable).toArray(Optional[]::new);
path.maxVelocity = Optional.ofNullable(maxVelocity);
path.maxAcceleration = Optional.ofNullable(maxAcceleration);
path.isReversed = Optional.ofNullable(isReversed);
data.clear();
return path;
}
}
src/main/java/frc4388/utility/SendableRunChooser.java
+182
View File
@@ -0,0 +1,182 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
// package edu.wpi.first.wpilibj.smartdashboard;
package frc4388.utility;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.networktables.NTSendable;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.util.ArrayList;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.Consumer;
/**
* The {@link SendableRunChooser} class is a useful tool for presenting a selection of options to the
* {@link SmartDashboard}.
*
* <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
* this by putting every possible Command you want to run as an autonomous into a {@link
* SendableRunChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
* on the laptop. Once autonomous starts, simply ask the {@link SendableRunChooser} what the selected
* value is.
*
* @param <V> The type of the values to be stored
*/
public class SendableRunChooser<V> implements NTSendable, AutoCloseable {
/** The key for the default value. */
private static final String DEFAULT = "default";
/** The key for the selected option. */
private static final String SELECTED = "selected";
/** The key for the active option. */
private static final String ACTIVE = "active";
/** The key for the option array. */
private static final String OPTIONS = "options";
/** The key for the instance number. */
private static final String INSTANCE = ".instance";
/** A map linking strings to the objects the represent. */
private final Map<String, V> m_map = new LinkedHashMap<>();
private String m_defaultChoice = "";
private final int m_instance;
private static final AtomicInteger s_instances = new AtomicInteger();
private Consumer<String> m_consumer;
/** Instantiates a {@link SendableRunChooser}. */
public SendableRunChooser(Consumer<String> consumer) {
m_consumer = consumer;
m_instance = s_instances.getAndIncrement();
SendableRegistry.add(this, "SendableChooser", m_instance);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
* object will appear as the given name.
*
* @param name the name of the option
* @param object the option
*/
public void addOption(String name, V object) {
m_map.put(name, object);
}
/**
* Adds the given object to the list of options.
*
* @param name the name of the option
* @param object the option
* @deprecated Use {@link #addOption(String, Object)} instead.
*/
@Deprecated
public void addObject(String name, V object) {
addOption(name, object);
}
/**
* Adds the given object to the list of options and marks it as the default. Functionally, this is
* very close to {@link #addOption(String, Object)} except that it will use this as the default
* option if none other is explicitly selected.
*
* @param name the name of the option
* @param object the option
*/
public void setDefaultOption(String name, V object) {
requireNonNullParam(name, "name", "setDefaultOption");
m_defaultChoice = name;
addOption(name, object);
}
/**
* Adds the given object to the list of options and marks it as the default.
*
* @param name the name of the option
* @param object the option
* @deprecated Use {@link #setDefaultOption(String, Object)} instead.
*/
@Deprecated
public void addDefault(String name, V object) {
setDefaultOption(name, object);
}
/**
* Returns the selected option. If there is none selected, it will return the default. If there is
* none selected and no default, then it will return {@code null}.
*
* @return the option selected
*/
public V getSelected() {
m_mutex.lock();
try {
if (m_selected != null) {
return m_map.get(m_selected);
} else {
return m_map.get(m_defaultChoice);
}
} finally {
m_mutex.unlock();
}
}
private String m_selected;
private final List<NetworkTableEntry> m_activeEntries = new ArrayList<>();
private final ReentrantLock m_mutex = new ReentrantLock();
@Override
public void initSendable(NTSendableBuilder builder) {
builder.setSmartDashboardType("String Chooser");
builder.getEntry(INSTANCE).setDouble(m_instance);
builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
builder.addStringProperty(
ACTIVE,
() -> {
m_mutex.lock();
try {
if (m_selected != null) {
return m_selected;
} else {
return m_defaultChoice;
}
} finally {
m_mutex.unlock();
}
},
null);
m_mutex.lock();
try {
m_activeEntries.add(builder.getEntry(ACTIVE));
} finally {
m_mutex.unlock();
}
builder.addStringProperty(
SELECTED,
null,
val -> {
m_mutex.lock();
try {
m_selected = val;
m_consumer.accept(val);
for (NetworkTableEntry entry : m_activeEntries) {
entry.setString(val);
}
} finally {
m_mutex.unlock();
}
});
}
}