mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
Merge branch 'robot-logger' into swerve
This commit is contained in:
nathanrsxtn
@@ -71,11 +71,13 @@ public final class Constants {
|
||||
// swerve auto constants
|
||||
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
|
||||
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
|
||||
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
|
||||
15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
|
||||
|
||||
public static final double MAX_VEL = 5.0;
|
||||
public static final double MAX_ACC = 5.0;
|
||||
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController (15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
|
||||
public static final boolean PATH_RECORD_VELOCITY = true;
|
||||
public static final double PATH_MAX_VELOCITY = 5.0;
|
||||
public static final double PATH_MAX_ACCELERATION = 5.0;
|
||||
public static final double MIN_WAYPOINT_ANGLE = 20;
|
||||
public static final double MIN_WAYPOINT_DISTANCE = 0.1;
|
||||
public static final double MIN_WAYPOINT_VELOCITY = 0.1;
|
||||
|
||||
// swerve configuration
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
@@ -127,7 +129,7 @@ public final class Constants {
|
||||
/* PID Constants Shooter */
|
||||
public static final int CLOSED_LOOP_TIME_MS = 1;
|
||||
|
||||
public static final int SHOOTER_TIMEOUT_MS = 32;
|
||||
public static final int SHOOTER_TIMEOUT_MS = 32;
|
||||
public static final int SHOOTER_SLOT_IDX = 0;
|
||||
public static final int SHOOTER_PID_LOOP_IDX = 1;
|
||||
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
|
||||
@@ -173,11 +175,11 @@ public final class Constants {
|
||||
|
||||
}
|
||||
public static final class VisionConstants {
|
||||
public static final double TURN_P_VALUE = 0.8;
|
||||
public static final double X_ANGLE_ERROR = 0.5;
|
||||
public static final double GRAV = 385.83;
|
||||
public static final double TARGET_HEIGHT = 67.5;
|
||||
public static final double FOV = 29.8; //Field of view limelight
|
||||
public static final double LIME_ANGLE = 24.7;
|
||||
}
|
||||
public static final double TURN_P_VALUE = 0.8;
|
||||
public static final double X_ANGLE_ERROR = 0.5;
|
||||
public static final double GRAV = 385.83;
|
||||
public static final double TARGET_HEIGHT = 67.5;
|
||||
public static final double FOV = 29.8; //Field of view limelight
|
||||
public static final double LIME_ANGLE = 24.7;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,6 +5,12 @@
|
||||
package frc4388.robot;
|
||||
|
||||
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import com.diffplug.common.base.DurianPlugins;
|
||||
import com.diffplug.common.base.Errors;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import frc4388.utility.AnsiLogging;
|
||||
|
||||
@@ -24,6 +30,7 @@ public final class Main {
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
AnsiLogging.systemInstall();
|
||||
DurianPlugins.register(Errors.Plugins.Log.class, e -> Logger.getLogger(e.getStackTrace()[0].getClassName().substring(e.getStackTrace()[0].getClassName().lastIndexOf('.') + 1)).log(Level.SEVERE, e, e::getLocalizedMessage));
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4,14 +4,22 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.io.File;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Path;
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
import java.util.stream.IntStream;
|
||||
|
||||
import com.diffplug.common.base.Errors;
|
||||
|
||||
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 edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import frc4388.utility.RobotTime;
|
||||
|
||||
/**
|
||||
@@ -22,7 +30,7 @@ import frc4388.utility.RobotTime;
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
|
||||
private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName());
|
||||
Command m_autonomousCommand;
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
@@ -34,20 +42,51 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
if (org.fusesource.jansi.Ansi.isEnabled()) {
|
||||
LOGGER.log(Level.ALL, "Logging Test 1/8");
|
||||
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
|
||||
LOGGER.log(Level.WARNING, "Logging Test 3/8");
|
||||
LOGGER.log(Level.INFO, "Logging Test 4/8");
|
||||
LOGGER.log(Level.CONFIG, "Logging Test 5/8");
|
||||
LOGGER.log(Level.FINE, "Logging Test 6/8");
|
||||
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
||||
LOGGER.log(Level.FINEST, "Logging Test 8/8");
|
||||
}
|
||||
LOGGER.log(Level.ALL, "Logging Test 1/8");
|
||||
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
|
||||
LOGGER.log(Level.WARNING, "Logging Test 3/8");
|
||||
LOGGER.log(Level.INFO, "Logging Test 4/8");
|
||||
LOGGER.log(Level.CONFIG, "Logging Test 5/8");
|
||||
LOGGER.log(Level.FINE, "Logging Test 6/8");
|
||||
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
||||
LOGGER.log(Level.FINEST, "Logging Test 8/8");
|
||||
Errors.log().run(() -> { throw new Throwable("Exception Test"); });
|
||||
|
||||
// var path = PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move Forward.path").toFile());
|
||||
// LOGGER.finest(path::toString);
|
||||
LOGGER.fine("robotInit()");
|
||||
// LOGGER.fine("Sssssssssh.");
|
||||
// DriverStation.silenceJoystickConnectionWarning(true);
|
||||
// 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";
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -82,6 +121,15 @@ 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());
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -132,6 +180,11 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
|
||||
@@ -4,36 +4,59 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFX;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.io.StringWriter;
|
||||
import java.nio.file.FileSystems;
|
||||
import java.nio.file.Path;
|
||||
import java.nio.file.StandardWatchEventKinds;
|
||||
import java.nio.file.WatchEvent;
|
||||
import java.nio.file.WatchKey;
|
||||
import java.time.Clock;
|
||||
import java.time.ZoneId;
|
||||
import java.time.ZonedDateTime;
|
||||
import java.time.format.DateTimeFormatter;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.Comparator;
|
||||
import java.util.List;
|
||||
import java.util.Objects;
|
||||
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 java.util.stream.Collectors;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFX;
|
||||
import com.diffplug.common.base.Errors;
|
||||
import com.pathplanner.lib.PathPlanner;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
|
||||
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
||||
|
||||
import edu.wpi.first.math.Pair;
|
||||
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.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.NotifierCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.AimToCenter;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
@@ -42,6 +65,9 @@ import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.ListeningSendableChooser;
|
||||
import frc4388.utility.PathPlannerUtil;
|
||||
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
/**
|
||||
@@ -52,17 +78,12 @@ 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.getSimpleName());
|
||||
/* RobotMap */
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public 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);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
||||
@@ -73,6 +94,18 @@ public class RobotContainer {
|
||||
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
/* Autonomous */
|
||||
private PathPlannerTrajectory loadedPathTrajectory = null;
|
||||
private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
|
||||
private final List<Waypoint> pathPoints = new ArrayList<>();
|
||||
private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
|
||||
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
|
||||
|
||||
private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH:mm:ss.SSS'.path'");
|
||||
private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault());
|
||||
private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner");
|
||||
private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
@@ -89,15 +122,17 @@ public class RobotContainer {
|
||||
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
getDriverController().getLeftX(),
|
||||
getDriverController().getLeftY(),
|
||||
getDriverController().getRightX(),
|
||||
getDriverController().getRightY(),
|
||||
true),
|
||||
m_robotSwerveDrive));
|
||||
getDriverController().getLeftX(),
|
||||
getDriverController().getLeftY(),
|
||||
getDriverController().getRightX(),
|
||||
getDriverController().getRightY(),
|
||||
true),
|
||||
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"));
|
||||
autoInit();
|
||||
recordInit();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -110,21 +145,18 @@ public class RobotContainer {
|
||||
/* Driver Buttons */
|
||||
// "XboxController.Button.kBack" was undefined yet, 7 works just fine
|
||||
new JoystickButton(getDriverController(), 7)
|
||||
.whenPressed(() -> m_robotSwerveDrive.resetGyro());
|
||||
.whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
//.whenPressed(this::resetOdometry);
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
@@ -141,81 +173,33 @@ public class RobotContainer {
|
||||
.whenReleased(() -> m_robotHood.runHood(0.d));
|
||||
}
|
||||
|
||||
/**
|
||||
* Generate autonomous
|
||||
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
||||
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
||||
* @param inputs strings (path names) or commands you want to run (in order)
|
||||
* @return array of commands, which can then be processed in a command group
|
||||
*/
|
||||
public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
|
||||
|
||||
// default vel and acc
|
||||
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL);
|
||||
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC);
|
||||
|
||||
ArrayList<Command> commands = new ArrayList<Command>();
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
|
||||
|
||||
// pids controlling the path
|
||||
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 string, process as pathplanner trajectory
|
||||
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));
|
||||
}
|
||||
|
||||
// if command, just add it to the array
|
||||
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
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
|
||||
return new ParallelCommandGroup(
|
||||
buildAuto(
|
||||
null,
|
||||
null,
|
||||
new SequentialCommandGroup(buildAuto(0.5, 0.5, "Move Forward", "Move Down")),
|
||||
new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))
|
||||
)
|
||||
);
|
||||
if (loadedPathTrajectory != null) {
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
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.severe("No auto selected.");
|
||||
return new RunCommand(() -> {}).withName("No Autonomous Path");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public XboxController getDriverController() {
|
||||
return m_driverXbox;
|
||||
}
|
||||
@@ -235,10 +219,144 @@ public class RobotContainer {
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_robotSwerveDrive.resetOdometry(pose);
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
|
||||
public XboxController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
private void autoInit() {
|
||||
try {
|
||||
WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, StandardWatchEventKinds.ENTRY_DELETE);
|
||||
// TODO: Store this and other commands as fields so they can be rescheduled.
|
||||
new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return true;
|
||||
}
|
||||
}.withName("Path Watcher").schedule();
|
||||
} catch (IOException exception) {
|
||||
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
|
||||
}
|
||||
Arrays.stream(PATHPLANNER_DIRECTORY.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);
|
||||
}
|
||||
|
||||
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 updateAutoChooser(WatchKey watchKey) {
|
||||
List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
|
||||
if (!watchEvents.isEmpty()) {
|
||||
List<WatchEvent<?>> pathWatchEvents = watchEvents.stream().filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
|
||||
for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
|
||||
Path watchEventPath = (Path) pathWatchEvent.context();
|
||||
File watchEventFile = watchEventPath.toFile();
|
||||
String watchEventFileName = watchEventFile.getName();
|
||||
if (watchEventFileName.endsWith(".path")) {
|
||||
if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
|
||||
LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", watchEventFileName);
|
||||
autoChooser.addOption(watchEventFile.getName(), watchEventFile);
|
||||
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
|
||||
LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
|
||||
if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
|
||||
LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
|
||||
loadPath(watchEventFileName);
|
||||
}
|
||||
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
|
||||
LOGGER.log(Level.SEVERE, "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", watchEventFileName);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!watchKey.reset())
|
||||
LOGGER.severe("File watch key invalid.");
|
||||
}
|
||||
|
||||
private void loadPath(String pathName) {
|
||||
LOGGER.warning("Loading path " + pathName);
|
||||
loadedPathTrajectory = null;
|
||||
loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
|
||||
LOGGER.info("Done loading");
|
||||
}
|
||||
|
||||
private void saveRecording() {
|
||||
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
|
||||
File outputFile = PATHPLANNER_DIRECTORY.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
|
||||
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
|
||||
if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
|
||||
// TODO: Change to use measured maximum velocity and acceleration.
|
||||
var path = createPath(null, null, false);
|
||||
if (RobotBase.isReal())
|
||||
path.write(outputFile);
|
||||
StringWriter writer = new StringWriter();
|
||||
path.write(writer);
|
||||
recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
|
||||
autoChooser.setDefaultOption(outputFile.getName(), outputFile);
|
||||
LOGGER.log(Level.INFO, "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();
|
||||
// FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity.
|
||||
Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
|
||||
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
|
||||
pathPoints.add(waypoint);
|
||||
}
|
||||
|
||||
public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
|
||||
// Remove points whose angles to neighboring points are less than 10 degrees apart.
|
||||
int j = 0;
|
||||
for (int i = 1; i < pathPoints.size() - 1; i++) {
|
||||
var prev = pathPoints.get(j).anchorPoint.orElseThrow();
|
||||
var current = pathPoints.get(i).anchorPoint.orElseThrow();
|
||||
var next = pathPoints.get(i + 1).anchorPoint.orElseThrow();
|
||||
var fromPrevious = current.minus(prev);
|
||||
var toNext = next.minus(current);
|
||||
var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
|
||||
var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
|
||||
if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
|
||||
pathPoints.set(i, null);
|
||||
else
|
||||
j = i;
|
||||
}
|
||||
pathPoints.removeIf(Objects::isNull);
|
||||
// Make control points
|
||||
pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
|
||||
for (int i = 1; i < pathPoints.size() - 1; i++) {
|
||||
var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
|
||||
pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
|
||||
pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
|
||||
}
|
||||
pathPoints.get(pathPoints.size() - 1).prevControl = Optional.of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
|
||||
// Create the path
|
||||
PathPlannerUtil.Path path = new PathPlannerUtil.Path();
|
||||
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;
|
||||
}
|
||||
|
||||
private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current, Translation2d next) {
|
||||
var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
|
||||
return Pair.of(current.minus(line), current.plus(line));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -10,6 +10,7 @@ import java.util.Comparator;
|
||||
import java.util.Map;
|
||||
import java.util.Optional;
|
||||
import java.util.function.Function;
|
||||
import java.util.logging.Logger;
|
||||
import java.util.regex.Pattern;
|
||||
import java.util.stream.IntStream;
|
||||
|
||||
@@ -24,82 +25,81 @@ import frc4388.utility.CSV;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class BoomBoom extends SubsystemBase {
|
||||
public WPI_TalonFX m_shooterFalconLeft;
|
||||
public WPI_TalonFX m_shooterFalconRight;
|
||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public static BoomBoom m_boomBoom;
|
||||
private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
|
||||
public WPI_TalonFX m_shooterFalconLeft;
|
||||
public WPI_TalonFX m_shooterFalconRight;
|
||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public static BoomBoom m_boomBoom;
|
||||
|
||||
double velP;
|
||||
double input;
|
||||
double velP;
|
||||
double input;
|
||||
|
||||
public boolean m_isDrumReady = false;
|
||||
public double m_fireVel;
|
||||
public boolean m_isDrumReady = false;
|
||||
public double m_fireVel;
|
||||
|
||||
public Hood m_hoodSubsystem;
|
||||
public Turret m_turretSubsystem;
|
||||
public Hood m_hoodSubsystem;
|
||||
public Turret m_turretSubsystem;
|
||||
|
||||
// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
|
||||
// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values
|
||||
// later
|
||||
|
||||
public static class ShooterTableEntry {
|
||||
public Double distance, hoodExt, drumVelocity;
|
||||
}
|
||||
|
||||
private ShooterTableEntry[] m_shooterTable;
|
||||
|
||||
/**
|
||||
* Creates a new BoomBoom subsystem, which has the drum shooter.
|
||||
* @param shooterFalconLeft The left drum motor.
|
||||
* @param shooterFalconRight The right drum motor.
|
||||
*/
|
||||
public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
|
||||
m_shooterFalconLeft = shooterFalconLeft;
|
||||
m_shooterFalconRight = shooterFalconRight;
|
||||
|
||||
try {
|
||||
CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
|
||||
private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
|
||||
|
||||
@Override
|
||||
protected String headerSanitizer(final String header) {
|
||||
return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
|
||||
}
|
||||
|
||||
};
|
||||
m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath());
|
||||
new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
|
||||
} catch (final IOException e) {
|
||||
e.printStackTrace();
|
||||
// throw new RuntimeException(e);
|
||||
public static class ShooterTableEntry {
|
||||
public Double distance, hoodExt, drumVelocity;
|
||||
}
|
||||
}
|
||||
|
||||
public Double getVelocity(final Double distance) {
|
||||
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
|
||||
}
|
||||
private ShooterTableEntry[] m_shooterTable;
|
||||
|
||||
public Double getHood(final Double distance) {
|
||||
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
|
||||
}
|
||||
/*
|
||||
* Creates new BoomBoom subsystem, has drum shooter and angle adjuster
|
||||
*/
|
||||
/** Creates a new BoomBoom. */
|
||||
public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
|
||||
m_shooterFalconLeft = shooterFalconLeft;
|
||||
m_shooterFalconRight = shooterFalconRight;
|
||||
|
||||
// TODO: @nathanrsxtn pls java doc your shooter tables code
|
||||
try {
|
||||
CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
|
||||
private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
|
||||
|
||||
private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
|
||||
final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
|
||||
final E closestRecord = closestEntry.getValue();
|
||||
final int closestRecordIndex = closestEntry.getKey();
|
||||
final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
|
||||
return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
|
||||
}
|
||||
@Override
|
||||
protected String headerSanitizer(final String header) {
|
||||
return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
|
||||
}
|
||||
|
||||
private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
|
||||
final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
|
||||
return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
|
||||
}
|
||||
};
|
||||
m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
|
||||
new Thread(() -> LOGGER.fine(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
|
||||
} catch (final IOException e) {
|
||||
e.printStackTrace();
|
||||
// throw new RuntimeException(e);
|
||||
}
|
||||
}
|
||||
|
||||
private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
|
||||
final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
|
||||
return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
|
||||
}
|
||||
public Double getVelocity(final Double distance) {
|
||||
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
|
||||
}
|
||||
|
||||
public Double getHood(final Double distance) {
|
||||
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
|
||||
}
|
||||
|
||||
private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
|
||||
final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
|
||||
final E closestRecord = closestEntry.getValue();
|
||||
final int closestRecordIndex = closestEntry.getKey();
|
||||
final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
|
||||
return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
|
||||
}
|
||||
|
||||
private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
|
||||
final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
|
||||
return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
|
||||
}
|
||||
|
||||
private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
|
||||
final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
|
||||
return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
@@ -111,33 +111,34 @@ private static Number lerp2(final Number x, final Number x0, final Number y0, fi
|
||||
m_hoodSubsystem = subsystem0;
|
||||
m_turretSubsystem = subsystem1;
|
||||
}
|
||||
/**
|
||||
* Runs the Drum motor at a given speed
|
||||
* @param speed percent output form -1.0 to 1.0
|
||||
*/
|
||||
public void runDrumShooter(double speed) {
|
||||
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
|
||||
|
||||
}
|
||||
|
||||
public void setShooterGains() {
|
||||
m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
|
||||
m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
}
|
||||
/**
|
||||
* Runs the Drum motor at a given speed
|
||||
* @param speed percent output form -1.0 to 1.0
|
||||
*/
|
||||
public void runDrumShooter(double speed) {
|
||||
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
|
||||
|
||||
}
|
||||
|
||||
public void setShooterGains() {
|
||||
m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
|
||||
m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
public void runDrumShooterVelocityPID(double targetVel) {
|
||||
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init
|
||||
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
|
||||
m_shooterFalconRight.follow(m_shooterFalconLeft);
|
||||
// New BoomBoom controller stuff
|
||||
//Controls a motor with the output of the BangBang controller
|
||||
//Controls a motor with the output of the BangBang conroller and a feedforward
|
||||
//Shrinks the feedforward slightly to avoid over speeding the shooter
|
||||
|
||||
|
||||
// m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel));
|
||||
// New BoomBoom controller stuff
|
||||
// Controls a motor with the output of the BangBang controller
|
||||
// Controls a motor with the output of the BangBang conroller and a feedforward
|
||||
// Shrinks the feedforward slightly to avoid over speeding the shooter
|
||||
|
||||
// m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 *
|
||||
// feedforward.calculate(targetVel));
|
||||
// m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
|
||||
}
|
||||
}
|
||||
@@ -27,7 +27,7 @@ public class LED extends SubsystemBase {
|
||||
m_LEDController = LEDController;
|
||||
setPattern(LEDConstants.DEFAULT_PATTERN);
|
||||
updateLED();
|
||||
Logger.getLogger(LED.class.getName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -4,10 +4,8 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
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;
|
||||
@@ -19,11 +17,9 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
@@ -59,6 +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();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
private final Field2d m_field = new Field2d();
|
||||
|
||||
@@ -112,6 +109,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
|
||||
setModuleStates(states);
|
||||
}
|
||||
|
||||
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
|
||||
{
|
||||
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
|
||||
@@ -122,11 +120,12 @@ public class SwerveDrive extends SubsystemBase {
|
||||
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
|
||||
double xSpeedMetersPerSecond = -speed.getX();
|
||||
double ySpeedMetersPerSecond = speed.getY();
|
||||
chassisSpeeds = fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
|
||||
SwerveModuleState[] states =
|
||||
m_kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
|
||||
chassisSpeeds);
|
||||
setModuleStates(states);
|
||||
}
|
||||
|
||||
@@ -143,7 +142,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
}
|
||||
// modules[0].setDesiredState(desiredStates[0], false);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
|
||||
@@ -9,6 +9,8 @@ import java.io.PrintWriter;
|
||||
import java.io.StringWriter;
|
||||
import java.time.ZoneId;
|
||||
import java.time.ZonedDateTime;
|
||||
import java.util.Map;
|
||||
import java.util.Optional;
|
||||
import java.util.logging.ConsoleHandler;
|
||||
import java.util.logging.Formatter;
|
||||
import java.util.logging.Level;
|
||||
@@ -24,68 +26,76 @@ import org.fusesource.jansi.AnsiConsole;
|
||||
public class AnsiLogging extends ConsoleHandler {
|
||||
public static void systemInstall() {
|
||||
try {
|
||||
// Configures java.util.logging.Logger to output additional colored information.
|
||||
// Configure java.util.logging.Logger to output additional colored information.
|
||||
LogManager.getLogManager().updateConfiguration(key -> (o, n) -> {
|
||||
switch (key) {
|
||||
case ".level": return Level.ALL.getName();
|
||||
case "handlers": return AnsiColorConsoleHandler.class.getName();
|
||||
default: return n;
|
||||
case ".level":
|
||||
return Level.ALL.getName();
|
||||
case "handlers":
|
||||
return AnsiColorConsoleHandler.class.getName();
|
||||
default:
|
||||
return n;
|
||||
}
|
||||
});
|
||||
// Replaces standard output streams with org.fusesource.jansi.AnsiPrintStreams.
|
||||
// Replace standard output streams with org.fusesource.jansi.AnsiPrintStreams.
|
||||
AnsiConsole.systemInstall();
|
||||
// Replaces standard output stream with java.util.logging.Logger.
|
||||
// Replace standard output stream with java.util.logging.Logger.
|
||||
System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO));
|
||||
// Replaces standard error output stream with java.util.logging.Logger.
|
||||
// Replace standard error output stream with java.util.logging.Logger.
|
||||
System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE));
|
||||
} catch (IOException exception) {
|
||||
exception.printStackTrace(AnsiConsole.sysErr());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public static class AnsiColorConsoleHandler extends ConsoleHandler {
|
||||
@Override
|
||||
public void publish(LogRecord logRecord) {
|
||||
AnsiConsole.err().print(getFormatter().format(logRecord));
|
||||
AnsiConsole.err().flush();
|
||||
}
|
||||
|
||||
@Override
|
||||
public Formatter getFormatter() {
|
||||
return formatter;
|
||||
}
|
||||
|
||||
private static final Formatter formatter = new Formatter() {
|
||||
private final ZoneId zoneId = ZoneId.systemDefault();
|
||||
// Specify and prepare formats for messages
|
||||
private final Map<Integer, String> levelColors = Map.of(
|
||||
Level.OFF.intValue(), "",
|
||||
Level.SEVERE.intValue(), makeMessageFormatString(ansi().fgBright(Color.RED)),
|
||||
Level.WARNING.intValue(), makeMessageFormatString(ansi().fgBright(Color.YELLOW)),
|
||||
Level.INFO.intValue(), makeMessageFormatString(ansi().fg(Color.GREEN)),
|
||||
Level.CONFIG.intValue(), makeMessageFormatString(ansi().fgBright(Color.BLUE)),
|
||||
Level.FINE.intValue(), makeMessageFormatString(ansi().fg(Color.CYAN)),
|
||||
Level.FINER.intValue(), makeMessageFormatString(ansi().fg(Color.MAGENTA)),
|
||||
Level.FINEST.intValue(), makeMessageFormatString(ansi().fgBright(Color.BLACK)),
|
||||
Level.ALL.intValue(), makeMessageFormatString(ansi().fg(Color.DEFAULT))
|
||||
);
|
||||
|
||||
private String makeMessageFormatString(Ansi base) {
|
||||
return base.bold().a(Attribute.UNDERLINE).a("[%1$tb %1$td %1$tk:%1$tM:%1$tS.%1$tL] %2$s %3$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%4$s%5$s").a(Attribute.INTENSITY_FAINT).a("%6$s").reset().a("%n").toString();
|
||||
}
|
||||
|
||||
private String makeStackTraceString(Throwable throwable) {
|
||||
StringWriter stringWriter = new StringWriter();
|
||||
try (PrintWriter printWriter = new PrintWriter(stringWriter)) {
|
||||
printWriter.println();
|
||||
throwable.printStackTrace(printWriter);
|
||||
}
|
||||
return stringWriter.toString();
|
||||
}
|
||||
|
||||
@Override
|
||||
public String format(LogRecord logRecord) {
|
||||
ZonedDateTime zdt = ZonedDateTime.ofInstant(logRecord.getInstant(), ZoneId.systemDefault());
|
||||
String source;
|
||||
if (logRecord.getSourceClassName() != null && !logRecord.getSourceClassName().startsWith(AnsiLogging.class.getName())) {
|
||||
source = logRecord.getSourceClassName();
|
||||
if (logRecord.getSourceMethodName() != null) {
|
||||
source += " " + logRecord.getSourceMethodName();
|
||||
}
|
||||
} else
|
||||
source = logRecord.getLoggerName();
|
||||
ZonedDateTime time = ZonedDateTime.ofInstant(logRecord.getInstant(), zoneId);
|
||||
String source = Optional.ofNullable(logRecord.getLoggerName()).or(() -> Optional.ofNullable(logRecord.getSourceClassName())).map(s -> s + " ").orElse("") + Optional.ofNullable(logRecord.getSourceMethodName()).orElse("");
|
||||
String message = formatMessage(logRecord);
|
||||
String throwable = "";
|
||||
if (logRecord.getThrown() != null) {
|
||||
StringWriter sw = new StringWriter();
|
||||
try (PrintWriter pw = new PrintWriter(sw)) {
|
||||
pw.println();
|
||||
logRecord.getThrown().printStackTrace(pw);
|
||||
}
|
||||
throwable = sw.toString();
|
||||
}
|
||||
Ansi ansi;
|
||||
if (logRecord.getLevel() == Level.SEVERE) ansi = ansi().fgBright(Color.RED);
|
||||
else if (logRecord.getLevel() == Level.WARNING) ansi = ansi().fgBright(Color.YELLOW);
|
||||
else if (logRecord.getLevel() == Level.INFO) ansi = ansi().fg(Color.GREEN);
|
||||
else if (logRecord.getLevel() == Level.CONFIG) ansi = ansi().fgBright(Color.BLUE);
|
||||
else if (logRecord.getLevel() == Level.FINE) ansi = ansi().fg(Color.CYAN);
|
||||
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();
|
||||
return String.format(format, zdt, source, logRecord.getLoggerName(), logRecord.getLevel().getLocalizedName(), message, throwable);
|
||||
String throwable = Optional.ofNullable(logRecord.getThrown()).map(this::makeStackTraceString).orElse("");
|
||||
String format = levelColors.getOrDefault(logRecord.getLevel().intValue(), levelColors.get(Level.ALL.intValue()));
|
||||
return String.format(format, time, source, logRecord.getLevel().getLocalizedName(), message.lines().count() > 1 ? System.lineSeparator() : " ", message.contains("\033") ? "\033[0m" + message : message, throwable);
|
||||
}
|
||||
};
|
||||
}
|
||||
@@ -95,7 +105,7 @@ public class AnsiLogging extends ConsoleHandler {
|
||||
private final StringBuilder stringBuilder = new StringBuilder();
|
||||
|
||||
@Override
|
||||
public final void write(int i) throws IOException {
|
||||
public void write(int i) throws IOException {
|
||||
if (i == '\n') {
|
||||
logger.log(level, stringBuilder::toString);
|
||||
stringBuilder.setLength(0);
|
||||
|
||||
@@ -1,10 +1,3 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.utility;
|
||||
|
||||
import java.awt.Color;
|
||||
|
||||
@@ -1,12 +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;
|
||||
|
||||
/** Add your docs here. */
|
||||
public class DataLogging {
|
||||
public DataLogging() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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 ListeningSendableChooser} 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
|
||||
* ListeningSendableChooser} 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 ListeningSendableChooser} what the selected
|
||||
* value is.
|
||||
*
|
||||
* @param <V> The type of the values to be stored
|
||||
*/
|
||||
public class ListeningSendableChooser<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 ListeningSendableChooser}. */
|
||||
public ListeningSendableChooser(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();
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.OutputStream;
|
||||
import java.io.Writer;
|
||||
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;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
|
||||
public final class PathPlannerUtil {
|
||||
public static final class Path {
|
||||
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());
|
||||
objectMapper.setSerializationInclusion(Include.ALWAYS);
|
||||
}
|
||||
|
||||
public static Path read(File src) {
|
||||
return Errors.log().getWithDefault(() -> objectMapper.readValue(src, Path.class), null);
|
||||
}
|
||||
|
||||
public void write(File resultFile) {
|
||||
Errors.log().run(() -> objectMapper.writeValue(resultFile, this));
|
||||
}
|
||||
|
||||
public void write(Writer writer) {
|
||||
Errors.log().run(() -> objectMapper.writeValue(writer, this));
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user