* call.
*/
public final class Main {
- private Main() {
- }
+ private Main() {}
- /**
- * Main initialization function. Do not perform any initialization here.
- *
-
* <p>If you change your main robot class, change the parameter type.
- */
- public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
- }
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ * <p>If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
}
* project.
*/
public class Robot extends LoggedRobot {
- private Command autoCommand;
- private RobotContainer robotContainer;
-
- public Robot(){
- CanBridge.runTCP();
- PortForwarder.add(5800, Constants.VISION_CAMERA_HOST, 5800);
- PortForwarder.add(1182, Constants.VISION_CAMERA_HOST, 1182);
-
- // Set up data receivers & replay source
- switch (Constants.CURRENT_MODE) {
- case REAL:
- // Running on a real robot, log to a USB stick ("/U/logs")
- Logger.addDataReceiver(new WPILOGWriter());
- Logger.addDataReceiver(new NT4Publisher());
- break;
-
- case SIM:
- // Running a physics simulator, log to NT
- Logger.addDataReceiver(new NT4Publisher());
- break;
-
- case REPLAY:
- // Replaying a log, set up replay source
- setUseTiming(false); // Run as fast as possible
- String logPath = LogFileUtil.findReplayLog();
- Logger.setReplaySource(new WPILOGReader(logPath));
- Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
- break;
- }
- Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
+ private Command autoCommand;
+ private RobotContainer robotContainer;
+
+ public Robot() {
+ CanBridge.runTCP();
+ PortForwarder.add(5800, Constants.VISION_CAMERA_HOST, 5800);
+ PortForwarder.add(1182, Constants.VISION_CAMERA_HOST, 1182);
+
+ // Set up data receivers & replay source
+ switch (Constants.CURRENT_MODE) {
+ case REAL:
+ // Running on a real robot, log to a USB stick ("/U/logs")
+ Logger.addDataReceiver(new WPILOGWriter());
+ Logger.addDataReceiver(new NT4Publisher());
+ break;
+
+ case SIM:
+ // Running a physics simulator, log to NT
+ Logger.addDataReceiver(new NT4Publisher());
+ break;
+
+ case REPLAY:
+ // Replaying a log, set up replay source
+ setUseTiming(false); // Run as fast as possible
+ String logPath = LogFileUtil.findReplayLog();
+ Logger.setReplaySource(new WPILOGReader(logPath));
+ Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+ break;
}
-
- /**
- * This function is run when the robot is first started up and should be used for any
- * initialization code.
- */
- @Override
- public void robotInit() {
- // To Set the Robot Identity
- // SimGUI: Persistent Values, Preferences, RobotId, then restart Simulation
- // changes networktables.json, networktables.json.bck (both Untracked)
- // Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
- // RobotId.setRobotId(RobotId.SwerveCompetition);
-
- RobotController.setBrownoutVoltage(4.6); //TODO might break on systemcores https://www.chiefdelphi.com/t/frc-1678-citrus-circuits-systemcore-alpha-testing-thread/506842
- // obtain this robot's identity
- RobotId robotId = RobotId.getRobotId();
-
- DriveConstants.update(robotId);
-
- // Record metadata
- Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME);
- Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE);
- Logger.recordMetadata("GitSHA", BuildData.GIT_SHA);
- Logger.recordMetadata("GitDate", BuildData.GIT_DATE);
- Logger.recordMetadata("GitBranch", BuildData.GIT_BRANCH);
- switch (BuildData.DIRTY) {
- case 0:
- Logger.recordMetadata("GitDirty", "All changes committed");
- break;
- case 1:
- Logger.recordMetadata("GitDirty", "Uncomitted changes");
- break;
- default:
- Logger.recordMetadata("GitDirty", "Unknown");
- break;
- }
-
- robotContainer = new RobotContainer(robotId);
-
+ Logger
+ .start(); // Start logging! No more data receivers, replay sources, or metadata values may
+ // be added.
+ }
+
+ /**
+ * This function is run when the robot is first started up and should be used for any
+ * initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // To Set the Robot Identity
+ // SimGUI: Persistent Values, Preferences, RobotId, then restart Simulation
+ // changes networktables.json, networktables.json.bck (both Untracked)
+ // Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
+ // RobotId.setRobotId(RobotId.SwerveCompetition);
+
+ RobotController.setBrownoutVoltage(
+ 4.6); // TODO might break on systemcores
+ // https://www.chiefdelphi.com/t/frc-1678-citrus-circuits-systemcore-alpha-testing-thread/506842
+ // obtain this robot's identity
+ RobotId robotId = RobotId.getRobotId();
+
+ DriveConstants.update(robotId);
+
+ // Record metadata
+ Logger.recordMetadata("ProjectName", BuildData.MAVEN_NAME);
+ Logger.recordMetadata("BuildDate", BuildData.BUILD_DATE);
+ Logger.recordMetadata("GitSHA", BuildData.GIT_SHA);
+ Logger.recordMetadata("GitDate", BuildData.GIT_DATE);
+ Logger.recordMetadata("GitBranch", BuildData.GIT_BRANCH);
+ switch (BuildData.DIRTY) {
+ case 0:
+ Logger.recordMetadata("GitDirty", "All changes committed");
+ break;
+ case 1:
+ Logger.recordMetadata("GitDirty", "Uncomitted changes");
+ break;
+ default:
+ Logger.recordMetadata("GitDirty", "Unknown");
+ break;
}
- /**
- * This function is called every robot packet, no matter the mode. Use this for items like
- * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
- *
- * <p>This runs after the mode-specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
- */
- @Override
- public void robotPeriodic() {
- // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
- // commands, running already-scheduled commands, removing finished or interrupted commands,
- // and running subsystem periodic() methods. This must be called from the robot's periodic
- // block in order for anything in the Command-based framework to work.
-
- CommandScheduler.getInstance().run();
-
- robotContainer.logComponents();
- robotContainer.periodic();
+ robotContainer = new RobotContainer(robotId);
+ }
+
+ /**
+ * This function is called every robot packet, no matter the mode. Use this for items like
+ * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+ *
+ * <p>This runs after the mode-specific periodic functions, but before LiveWindow and
+ * SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
+ // commands, running already-scheduled commands, removing finished or interrupted commands,
+ // and running subsystem periodic() methods. This must be called from the robot's periodic
+ // block in order for anything in the Command-based framework to work.
+
+ CommandScheduler.getInstance().run();
+
+ robotContainer.logComponents();
+ robotContainer.periodic();
+ }
+
+ /** This function is called once each time the robot enters Disabled mode. */
+ @Override
+ public void disabledInit() {
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically when the robot is disabled */
+ @Override
+ public void disabledPeriodic() {}
+
+ /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+ @Override
+ public void autonomousInit() {
+ // Disable vision if the constant is false.
+ robotContainer.setVisionEnabled(VisionConstants.ENABLED_AUTO);
+
+ // Get the autonomous command.
+ // This access is fast (about 14 microseconds) because the value is already resident in the
+ // Network Tables.
+ // There was a problem last year because the operation also installed about over a dozen items
+ // (taking more than 20 ms).
+ autoCommand = robotContainer.getAutoCommand();
+
+ // If there is an autonomous command, then schedule it
+ if (autoCommand != null) {
+ CommandScheduler.getInstance().schedule(autoCommand);
}
-
- /**
- * This function is called once each time the robot enters Disabled mode.
- */
- @Override
- public void disabledInit() {
- CommandScheduler.getInstance().cancelAll();
- }
-
- /**
- * This function is called periodically when the robot is disabled
- */
- @Override
- public void disabledPeriodic() {
- }
-
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
- @Override
- public void autonomousInit() {
- // Disable vision if the constant is false.
- robotContainer.setVisionEnabled(VisionConstants.ENABLED_AUTO);
-
- // Get the autonomous command.
- // This access is fast (about 14 microseconds) because the value is already resident in the Network Tables.
- // There was a problem last year because the operation also installed about over a dozen items (taking more than 20 ms).
- autoCommand = robotContainer.getAutoCommand();
-
- // If there is an autonomous command, then schedule it
- if (autoCommand != null) {
- CommandScheduler.getInstance().schedule(autoCommand);
- }
- }
-
- /**
- * This function is called periodically during autonomous.
- */
- @Override
- public void autonomousPeriodic() {
- }
-
-
- /**
- * This function is called once each time the robot enters Teleop mode.
- */
- @Override
- public void teleopInit() {
- robotContainer.setVisionEnabled(true);
-
- // This makes sure that the autonomous stops running when
- // teleop starts running. If you want the autonomous to
- // continue until interrupted by another command, remove
- // this line or comment it out.
- if (autoCommand != null) {
- autoCommand.cancel();
- }
+ }
+
+ /** This function is called periodically during autonomous. */
+ @Override
+ public void autonomousPeriodic() {}
+
+ /** This function is called once each time the robot enters Teleop mode. */
+ @Override
+ public void teleopInit() {
+ robotContainer.setVisionEnabled(true);
+
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (autoCommand != null) {
+ autoCommand.cancel();
}
-
- /**
- * This function is called periodically during operator control.
- */
- @Override
- public void teleopPeriodic() {
- }
-
- /**
- * This function is called once each time the robot enters Test mode.
- */
- @Override
- public void testInit() {
- // Cancels all running commands at the start of test mode.
- CommandScheduler.getInstance().cancelAll();
- }
-
- /**
- * This function is called periodically during test mode.
- */
- @Override
- public void testPeriodic() {
- }
-
- @Override
- public void simulationPeriodic() {
- }
-
- /**
- * Gets the set Alliance; defaults to red if not set.
- * This method replaces {@link edu.first.wpilibj.DriverStation.getAlliance}.
- * The .get() is not necessary, so DriverStation.getAlliance().get() becomes Robot.getAlliance()
- */
- public static Alliance getAlliance() {
- Optional<Alliance> dsAlliance = DriverStation.getAlliance();
- if (dsAlliance.isPresent())
- return dsAlliance.get();
- else
- return Alliance.Red; // default to Red alliance
- }
+ }
+
+ /** This function is called periodically during operator control. */
+ @Override
+ public void teleopPeriodic() {}
+
+ /** This function is called once each time the robot enters Test mode. */
+ @Override
+ public void testInit() {
+ // Cancels all running commands at the start of test mode.
+ CommandScheduler.getInstance().cancelAll();
+ }
+
+ /** This function is called periodically during test mode. */
+ @Override
+ public void testPeriodic() {}
+
+ @Override
+ public void simulationPeriodic() {}
+
+ /**
+ * Gets the set Alliance; defaults to red if not set. This method replaces {@link
+ * edu.first.wpilibj.DriverStation.getAlliance}. The .get() is not necessary, so
+ * DriverStation.getAlliance().get() becomes Robot.getAlliance()
+ */
+ public static Alliance getAlliance() {
+ Optional<Alliance> dsAlliance = DriverStation.getAlliance();
+ if (dsAlliance.isPresent()) return dsAlliance.get();
+ else return Alliance.Red; // default to Red alliance
+ }
}
import frc.robot.commands.DoNothing;
import frc.robot.commands.LogCommand;
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
import frc.robot.commands.auto_comm.ChoreoPathCommandBuilder;
import frc.robot.commands.auto_comm.DynamicAutoBuilder;
import frc.robot.commands.drive_comm.SysIDDriveCommand;
import frc.robot.util.Vision.Vision;
/**
- * This class is where the bulk of the robot should be declared. Since
- * Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in
- * the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of
- * the robot (including
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
* subsystems, commands, and trigger mappings) should be declared here.
*/
public class RobotContainer {
private final AutoChooser choreoAutoChooser = new AutoChooser();
// choreo auto factory
- AutoFactory autoFactory ;
+ AutoFactory autoFactory;
+
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
- * <p>
- * Different robots may have different subsystems.
+ *
+ * <p>Different robots may have different subsystems.
*/
public RobotContainer(RobotId robotId) {
// display the current robot id on smartdashboard
SmartDashboard.putNumber("Match Time", 0.0);
}
-
// Filling the SendableChooser on SmartDashboard
// dispatch on the robot
case TwinBot:
-
case PrimeJr: // AKA Valence
case WaffleHouse: // AKA Betabot
case BetaBot: // AKA "Pancake"
vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
- // fall-through
+ // fall-through
case Vivace:
initializeAutoBuilder();
autoChooserInit();
-
if (drive != null && driver != null) {
// drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
SmartDashboard.putData("SysId Characterization", new SysIDDriveCommand(drive));
break;
}
-
- // CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret)); (no more crt for this)
+ // CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret)); (no more
+ // crt for this)
// This is really annoying so it's disabled
DriverStation.silenceJoystickConnectionWarning(true);
}
private void initChoreo() {
- // choreo auto factory init
- autoFactory = new AutoFactory(
+ // choreo auto factory init
+ autoFactory =
+ new AutoFactory(
drive::getPose,
drive::resetOdometry,
sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false),
true,
drive,
(trajectory, startOrFinish) -> {
- Logger.recordOutput(
- "Autos/Trajectory", trajectory.getPoses());
+ Logger.recordOutput("Autos/Trajectory", trajectory.getPoses());
Logger.recordOutput("Autos/StartingOrFinishing", startOrFinish);
- });
+ });
- // warmup command for choreo, prevents lag on auto startup
- CommandScheduler.getInstance().schedule(autoFactory.warmupCmd().ignoringDisable(true));
+ // warmup command for choreo, prevents lag on auto startup
+ CommandScheduler.getInstance().schedule(autoFactory.warmupCmd().ignoringDisable(true));
}
- /**
- * Sets whether the drivetrain uses vision toupdate odometry
- */
+ /** Sets whether the drivetrain uses vision toupdate odometry */
public void setVisionEnabled(boolean enabled) {
- if (drive != null)
- drive.setVisionEnabled(enabled);
+ if (drive != null) drive.setVisionEnabled(enabled);
}
public void initializeAutoBuilder() {
drive);
}
-
- public void registerCommands() {
-
- }
+ public void registerCommands() {}
public void addAuto(String name) {
try {
public void addAuto(String name, Command auto) {
try {
autoChooser.addOption(name, auto);
- } catch (AutoBuilderException e){
+ } catch (AutoBuilderException e) {
e.printStackTrace();
System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND");
}
}
/**
- * Initialize the SendableChooser on the SmartDashboard.
- * Fill the SendableChooser with available Commands.
+ * Initialize the SendableChooser on the SmartDashboard. Fill the SendableChooser with available
+ * Commands.
*/
public void autoChooserInit() {
// add the options to the Chooser
ChoreoPathCommandBuilder choreo = new ChoreoPathCommandBuilder();
- // addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true, autoFactory));
+ // addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true,
+ // autoFactory));
// put the Chooser on the SmartDashboard
SmartDashboard.putData("Auto chooser", autoChooser);
}
public Command getDefaultAuto() {
- return new DoNothing();
+ return new DoNothing();
}
public Command getAutoCommand() {
}
public void logComponents() {
- if (!Constants.LOG_MECHANISMS)
- return;
+ if (!Constants.LOG_MECHANISMS) return;
Logger.recordOutput(
"ComponentPoses",
new Pose3d[] {
- // Subsystem Pose3ds
+ // Subsystem Pose3ds
});
}
- public void periodic() {
-
- }
+ public void periodic() {}
}
/**
* Set of known Robot Names.
- * <p>The name of a robot in the RoboRIO's persistent memory.
- * At deploy time, that name is used to set the corresponding RobotId.
+ *
+ * <p>The name of a robot in the RoboRIO's persistent memory. At deploy time, that name is used to
+ * set the corresponding RobotId.
+ *
* <p>Note that the RobotId is determined at Deploy time.
*/
public enum RobotId {
- Default,
- PrimeJr, WaffleHouse, TwinBot, SwerveCompetition, Vertigo, Vivace, Phil, BetaBot,
- ClassBot1, ClassBot2, ClassBot3, ClassBot4,
- TestBed1, TestBed2;
+ Default,
+ PrimeJr,
+ WaffleHouse,
+ TwinBot,
+ SwerveCompetition,
+ Vertigo,
+ Vivace,
+ Phil,
+ BetaBot,
+ ClassBot1,
+ ClassBot2,
+ ClassBot3,
+ ClassBot4,
+ TestBed1,
+ TestBed2;
- /** The key used to access the RobotId name in the RoboRIO's persistent memory. */
- public static final String ROBOT_ID_KEY = "RobotId";
+ /** The key used to access the RobotId name in the RoboRIO's persistent memory. */
+ public static final String ROBOT_ID_KEY = "RobotId";
- /**
- * Is this robot a classbot?
- * @return true if a classbot
- * @deprecated this method is not needed....
- */
- @Deprecated
- public boolean isClassBot() {
- return this == WaffleHouse || this == ClassBot1 || this == ClassBot2 || this == ClassBot3 || this == ClassBot4;
- }
-
- /**
- * Whether this robot is a swerve bot
- * @return true if a swerve bot
- * @deprecated this method is not needed....
- */
- @Deprecated
- public boolean isSwerveBot() {
- return this == SwerveCompetition || this == Phil || this == Vertigo || this == Vivace || this == BetaBot;
- }
-
- /**
- * Determine the Robot Identity from the RoboRIO's onboard Preferences (flash memory).
- * @returns the RobotId
- */
- public static RobotId getRobotId() {
- // assume a default identity
- RobotId robotId = RobotId.Default;
+ /**
+ * Is this robot a classbot?
+ *
+ * @return true if a classbot
+ * @deprecated this method is not needed....
+ */
+ @Deprecated
+ public boolean isClassBot() {
+ return this == WaffleHouse
+ || this == ClassBot1
+ || this == ClassBot2
+ || this == ClassBot3
+ || this == ClassBot4;
+ }
- // check whether Preferences has an entry for the RobotId
- if (!Preferences.containsKey(ROBOT_ID_KEY)) {
- // There is no such key. Set it to the default identity.
- // This step guarantees persistent memory will have a key.
- setRobotId(RobotId.Default);
- }
+ /**
+ * Whether this robot is a swerve bot
+ *
+ * @return true if a swerve bot
+ * @deprecated this method is not needed....
+ */
+ @Deprecated
+ public boolean isSwerveBot() {
+ return this == SwerveCompetition
+ || this == Phil
+ || this == Vertigo
+ || this == Vivace
+ || this == BetaBot;
+ }
- // Remove the "Default" key if present.
- // This key was the result of a programming error in 2023.
- if (Preferences.containsKey("Default")) {
- Preferences.remove("Default");
- }
+ /**
+ * Determine the Robot Identity from the RoboRIO's onboard Preferences (flash memory).
+ *
+ * @returns the RobotId
+ */
+ public static RobotId getRobotId() {
+ // assume a default identity
+ RobotId robotId = RobotId.Default;
- // get the RobotId string from the RoboRIO's Preferences
- String strId = Preferences.getString(ROBOT_ID_KEY, RobotId.Default.name());
+ // check whether Preferences has an entry for the RobotId
+ if (!Preferences.containsKey(ROBOT_ID_KEY)) {
+ // There is no such key. Set it to the default identity.
+ // This step guarantees persistent memory will have a key.
+ setRobotId(RobotId.Default);
+ }
- // match that string to a RobotId by looking at all possible RobotId enums
- for (RobotId rid : RobotId.values()) {
- // does the preference string match the RobotId enum?
- if (strId.equals(rid.name())) {
- // yes, this instance is the desired RobotId
- robotId = rid;
- break;
- }
- }
+ // Remove the "Default" key if present.
+ // This key was the result of a programming error in 2023.
+ if (Preferences.containsKey("Default")) {
+ Preferences.remove("Default");
+ }
- if (robotId == RobotId.Default) {
- if (Robot.isSimulation()) {
- robotId = RobotId.SwerveCompetition; // Default to competition robot for simulation
- } else {
- throw new RuntimeException("RobotId is set to Default (or was unset)! Please set it to something.");
- }
- }
+ // get the RobotId string from the RoboRIO's Preferences
+ String strId = Preferences.getString(ROBOT_ID_KEY, RobotId.Default.name());
- // return the robot identity
- return robotId;
+ // match that string to a RobotId by looking at all possible RobotId enums
+ for (RobotId rid : RobotId.values()) {
+ // does the preference string match the RobotId enum?
+ if (strId.equals(rid.name())) {
+ // yes, this instance is the desired RobotId
+ robotId = rid;
+ break;
+ }
}
-
- /**
- * Set the RobotId in the RoboRIO's preferences (flash memory).
- * <p>
- * Calling it after the robot has been constructed (robotInit()) does not affect the robot.
- */
- static void setRobotId(RobotId robotId) {
- // Set the robot identity in the RoboRIO Preferences
- Preferences.setString(ROBOT_ID_KEY, robotId.name());
+
+ if (robotId == RobotId.Default) {
+ if (Robot.isSimulation()) {
+ robotId = RobotId.SwerveCompetition; // Default to competition robot for simulation
+ } else {
+ throw new RuntimeException(
+ "RobotId is set to Default (or was unset)! Please set it to something.");
+ }
}
+
+ // return the robot identity
+ return robotId;
+ }
+
+ /**
+ * Set the RobotId in the RoboRIO's preferences (flash memory).
+ *
+ * <p>Calling it after the robot has been constructed (robotInit()) does not affect the robot.
+ */
+ static void setRobotId(RobotId robotId) {
+ // Set the robot identity in the RoboRIO Preferences
+ Preferences.setString(ROBOT_ID_KEY, robotId.name());
+ }
}
import edu.wpi.first.wpilibj2.command.InstantCommand;
-/**
- * Does nothing. Can be used to more clearly mark commands intended not to do anything.
- */
-public class DoNothing extends InstantCommand {
-}
+/** Does nothing. Can be used to more clearly mark commands intended not to do anything. */
+public class DoNothing extends InstantCommand {}
package frc.robot.commands;
-import org.littletonrobotics.junction.Logger;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
-import frc.robot.util.Elastic;
-import frc.robot.util.Elastic.Notification;
-import frc.robot.util.Elastic.NotificationLevel;
/// Command for logging stuff
public class LogCommand extends Command {
- private boolean hubActive = false;
+ private boolean hubActive = false;
- public LogCommand() {
- }
-
- @Override
- public void execute() {
- if (Constants.DISABLE_LOGGING) {
- return;
- }
+ public LogCommand() {}
+ @Override
+ public void execute() {
+ if (Constants.DISABLE_LOGGING) {
+ return;
}
+ }
- @Override
- public boolean runsWhenDisabled() {
- return true;
- }
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
- @Override
- public boolean isFinished() {
- return false;
- }
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
}
import edu.wpi.first.wpilibj2.command.Command;
public class Music extends Command {
- private Orchestra orchestra;
-
- public Music(TalonFX[] motors) {
- orchestra = new Orchestra(Filesystem.getDeployDirectory() + "/chirp/file.chrp");
- for (TalonFX motor : motors) {
- System.out.println(motor.getDescription());
- orchestra.addInstrument(motor);
- }
- }
-
- @Override
- public void initialize() {
- orchestra.play();
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-
- @Override
- public void end(boolean interrupted) {
- orchestra.stop();
- }
+ private Orchestra orchestra;
- @Override
- public boolean runsWhenDisabled() {
- return true;
+ public Music(TalonFX[] motors) {
+ orchestra = new Orchestra(Filesystem.getDeployDirectory() + "/chirp/file.chrp");
+ for (TalonFX motor : motors) {
+ System.out.println(motor.getDescription());
+ orchestra.addInstrument(motor);
}
+ }
+
+ @Override
+ public void initialize() {
+ orchestra.play();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ orchestra.stop();
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
}
import java.util.function.Supplier;
/**
- * Runs the given command when this command is initialized, and ends when it ends.
- * Useful for commands that are not created yet because the constructor parameters are not available until initialization.
- * This is very similar to WPILib's DeferredCommand
+ * Runs the given command when this command is initialized, and ends when it ends. Useful for
+ * commands that are not created yet because the constructor parameters are not available until
+ * initialization. This is very similar to WPILib's DeferredCommand
*/
public class SupplierCommand extends Command {
- private final Supplier<Command> commandSupplier;
- private Command command;
-
- /**
- * Runs the given command when this command is initialized, and ends when it ends.
- * Useful for commands that are not created yet because the constructor parameters are not available until initialization.
- *
- * @param commandSupplier A Supplier to the command to run
- * @param Subsystem all subsystems that may be required to run the command supplied
- */
- public SupplierCommand(Supplier<Command> commandSupplier, Subsystem... Subsystem) {
- addRequirements(Subsystem);
- this.commandSupplier = commandSupplier;
- }
-
- @Override
- public final void initialize() {
- command = commandSupplier.get();
- command.initialize();
- }
-
- @Override
- public final void execute() {
- command.execute();
- }
-
- @Override
- public final void end(boolean interrupted) {
- command.end(interrupted);
- }
-
- @Override
- public final boolean isFinished() {
- return command.isFinished();
- }
-
+ private final Supplier<Command> commandSupplier;
+ private Command command;
+
+ /**
+ * Runs the given command when this command is initialized, and ends when it ends. Useful for
+ * commands that are not created yet because the constructor parameters are not available until
+ * initialization.
+ *
+ * @param commandSupplier A Supplier to the command to run
+ * @param Subsystem all subsystems that may be required to run the command supplied
+ */
+ public SupplierCommand(Supplier<Command> commandSupplier, Subsystem... Subsystem) {
+ addRequirements(Subsystem);
+ this.commandSupplier = commandSupplier;
+ }
+
+ @Override
+ public final void initialize() {
+ command = commandSupplier.get();
+ command.initialize();
+ }
+
+ @Override
+ public final void execute() {
+ command.execute();
+ }
+
+ @Override
+ public final void end(boolean interrupted) {
+ command.end(interrupted);
+ }
+
+ @Override
+ public final boolean isFinished() {
+ return command.isFinished();
+ }
}
package frc.robot.commands.auto_comm;
import choreo.auto.AutoFactory;
-import choreo.auto.AutoRoutine;
-import choreo.auto.AutoTrajectory;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
public class ChoreoPathCommandBuilder {
+ public ChoreoPathCommandBuilder() {}
- public ChoreoPathCommandBuilder() {
-
- }
-
- public static Command basicTrajectoryAuto(String pathName, boolean resetOdemetry, AutoFactory factory) {
+ public static Command basicTrajectoryAuto(
+ String pathName, boolean resetOdemetry, AutoFactory factory) {
Command command = factory.trajectoryCmd(pathName);
return Commands.sequence(
resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
command);
}
-
}
package frc.robot.commands.auto_comm;
import edu.wpi.first.wpilibj2.command.*;
-import frc.robot.commands.DoNothing;
-import com.pathplanner.lib.commands.PathPlannerAuto;
public class DynamicAutoBuilder {
- public DynamicAutoBuilder() {
- }
+ public DynamicAutoBuilder() {}
/*
* Autos have no named commands within them. They must be added here
/** Add your docs here. */
public class FollowPathCommand extends SequentialCommandGroup {
- Drivetrain drive;
- PathPlannerPath path;
-
+ Drivetrain drive;
+ PathPlannerPath path;
- public FollowPathCommand(String name, Drivetrain drive){
- this(name, false, drive);
- }
-
- public FollowPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive){
- this.drive = drive;
- this.path = PathGroupLoader.getPathGroup(pathName);
- addCommands(
- new InstantCommand(()->resetOdemetry(resetOdemetry)),
- new SupplierCommand(()->AutoBuilder.followPath(path), drive) // "problem" (254)
- // or pp's interaction with the drivetrain
- // or pp config
- );
- }
+ public FollowPathCommand(String name, Drivetrain drive) {
+ this(name, false, drive);
+ }
+
+ public FollowPathCommand(String pathName, boolean resetOdemetry, Drivetrain drive) {
+ this.drive = drive;
+ this.path = PathGroupLoader.getPathGroup(pathName);
+ addCommands(
+ new InstantCommand(() -> resetOdemetry(resetOdemetry)),
+ new SupplierCommand(() -> AutoBuilder.followPath(path), drive) // "problem" (254)
+ // or pp's interaction with the drivetrain
+ // or pp config
+ );
+ }
- public void resetOdemetry(boolean resetOdemetry){
- if (resetOdemetry){
- if(RobotContainer.getAllianceColorBooleanSupplier().getAsBoolean()){
- drive.resetOdometry(new Pose2d(path.getAllPathPoints().get(0).flip().position, path.getIdealStartingState().flip().rotation()));
- }else{
- drive.resetOdometry(new Pose2d(path.getAllPathPoints().get(0).position, path.getIdealStartingState().rotation()));
- }
- }
+ public void resetOdemetry(boolean resetOdemetry) {
+ if (resetOdemetry) {
+ if (RobotContainer.getAllianceColorBooleanSupplier().getAsBoolean()) {
+ drive.resetOdometry(
+ new Pose2d(
+ path.getAllPathPoints().get(0).flip().position,
+ path.getIdealStartingState().flip().rotation()));
+ } else {
+ drive.resetOdometry(
+ new Pose2d(
+ path.getAllPathPoints().get(0).position, path.getIdealStartingState().rotation()));
+ }
}
+ }
}
package frc.robot.commands.drive_comm;
-import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.util.Vision.DriverAssist;
-/**
- * Default drive command. Drives robot using driver controls.
- */
+/** Default drive command. Drives robot using driver controls. */
public class DefaultDriveCommand extends Command {
- protected final Drivetrain swerve;
- private final BaseDriverConfig driver;
- private PIDController trenchAssistPid = new PIDController(9, 0.0, 3);
+ protected final Drivetrain swerve;
+ private final BaseDriverConfig driver;
+ private PIDController trenchAssistPid = new PIDController(9, 0.0, 3);
- public DefaultDriveCommand(
- Drivetrain swerve,
- BaseDriverConfig driver) {
- this.swerve = swerve;
- this.driver = driver;
+ public DefaultDriveCommand(Drivetrain swerve, BaseDriverConfig driver) {
+ this.swerve = swerve;
+ this.driver = driver;
- addRequirements(swerve);
- }
+ addRequirements(swerve);
+ }
- @Override
- public void initialize() {
- swerve.setStateDeadband(true);
+ @Override
+ public void initialize() {
+ swerve.setStateDeadband(true);
- trenchAssistPid.setIZone(2);
- trenchAssistPid.setIntegratorRange(-1, 1);
+ trenchAssistPid.setIZone(2);
+ trenchAssistPid.setIntegratorRange(-1, 1);
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putNumber("0 degrees snap location", 0);
- }
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putNumber("0 degrees snap location", 0);
}
+ }
- @Override
- public void execute() {
- double forwardTranslation = driver.getForwardTranslation();
- double sideTranslation = driver.getSideTranslation();
- double rotation = -driver.getRotation();
-
- double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1;
+ @Override
+ public void execute() {
+ double forwardTranslation = driver.getForwardTranslation();
+ double sideTranslation = driver.getSideTranslation();
+ double rotation = -driver.getRotation();
- forwardTranslation *= slowFactor;
- sideTranslation *= slowFactor;
- rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1;
+ double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1;
- int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1;
- forwardTranslation *= allianceReversal;
- sideTranslation *= allianceReversal;
+ forwardTranslation *= slowFactor;
+ sideTranslation *= slowFactor;
+ rotation *= driver.getIsSlowMode() ? DriveConstants.SLOW_ROT_FACTOR : 1;
- ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
- ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true);
+ int allianceReversal = Robot.getAlliance() == Alliance.Red ? 1 : -1;
+ forwardTranslation *= allianceReversal;
+ sideTranslation *= allianceReversal;
- }
+ ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation);
+ ChassisSpeeds corrected =
+ DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true);
+ }
- /**
- * Drives the robot
- *
- * @param speeds The ChassisSpeeds to drive at
- */
- protected void drive(ChassisSpeeds speeds) {
- // If the driver is pressing the align button or a command set the drivetrain to
- // align, then align to speaker
- if (driver.getIsAlign() || swerve.getIsAlign()) {
- swerve.driveHeading(
- speeds.vxMetersPerSecond,
- speeds.vyMetersPerSecond,
- swerve.getAlignAngle(),
- true);
- } else {
- swerve.drive(
- speeds.vxMetersPerSecond,
- speeds.vyMetersPerSecond,
- speeds.omegaRadiansPerSecond,
- true,
- false);
- }
+ /**
+ * Drives the robot
+ *
+ * @param speeds The ChassisSpeeds to drive at
+ */
+ protected void drive(ChassisSpeeds speeds) {
+ // If the driver is pressing the align button or a command set the drivetrain to
+ // align, then align to speaker
+ if (driver.getIsAlign() || swerve.getIsAlign()) {
+ swerve.driveHeading(
+ speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, swerve.getAlignAngle(), true);
+ } else {
+ swerve.drive(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ speeds.omegaRadiansPerSecond,
+ true,
+ false);
}
+ }
}
private final ProfiledPIDController driveController =
new ProfiledPIDController(
- drivekP, 0.0, drivekD, new TrapezoidProfile.Constraints(driveMaxVelocity, driveMaxAcceleration), Constants.LOOP_TIME);
+ drivekP,
+ 0.0,
+ drivekD,
+ new TrapezoidProfile.Constraints(driveMaxVelocity, driveMaxAcceleration),
+ Constants.LOOP_TIME);
private final ProfiledPIDController thetaController =
new ProfiledPIDController(
- thetakP, 0.0, thetakD, new TrapezoidProfile.Constraints(thetaMaxVelocity, thetaMaxAcceleration), Constants.LOOP_TIME);
+ thetakP,
+ 0.0,
+ thetakD,
+ new TrapezoidProfile.Constraints(thetaMaxVelocity, thetaMaxAcceleration),
+ Constants.LOOP_TIME);
private Translation2d lastSetpointTranslation = new Translation2d();
private double driveErrorAbs = 0.0;
targetPose = target.get();
Pose2d currentPose = robot.get();
- ChassisSpeeds fieldVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(drive.getChassisSpeeds(), currentPose.getRotation());
+ ChassisSpeeds fieldVelocity =
+ ChassisSpeeds.fromRobotRelativeSpeeds(drive.getChassisSpeeds(), currentPose.getRotation());
Translation2d linearFieldVelocity =
new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond);
-
+
thetaController.reset(
currentPose.getRotation().getRadians(), fieldVelocity.omegaRadiansPerSecond);
- lastSetpointTranslation = currentPose.getTranslation();
-
- if(targetPose != null){
- driveController.reset(
- currentPose.getTranslation().getDistance(target.get().getTranslation()),
- -linearFieldVelocity
- .rotateBy(
- targetPose
- .getTranslation()
- .minus(currentPose.getTranslation())
- .getAngle()
- .unaryMinus())
- .getX());
- }
+ lastSetpointTranslation = currentPose.getTranslation();
+
+ if (targetPose != null) {
+ driveController.reset(
+ currentPose.getTranslation().getDistance(target.get().getTranslation()),
+ -linearFieldVelocity
+ .rotateBy(
+ targetPose
+ .getTranslation()
+ .minus(currentPose.getTranslation())
+ .getAngle()
+ .unaryMinus())
+ .getX());
+ }
}
@Override
// Get current pose and target pose
Pose2d currentPose = robot.get();
- if(updateTarget){
- targetPose = target.get();
+ if (updateTarget) {
+ targetPose = target.get();
}
- if(targetPose == null){
- return;
+ if (targetPose == null) {
+ return;
}
// Calculate drive speed
double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation());
double ffScaler =
- MathUtil.clamp(
- (currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius),
- 0.0,
- 1.0);
+ MathUtil.clamp((currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius), 0.0, 1.0);
driveErrorAbs = currentDistance;
driveController.reset(
lastSetpointTranslation.getDistance(targetPose.getTranslation()),
public boolean withinTolerance(double driveTolerance, Rotation2d thetaTolerance) {
return running
&& (Math.abs(driveErrorAbs) < driveTolerance
- && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians()
- || targetPose == null);
+ && Math.abs(thetaErrorAbs) < thetaTolerance.getRadians()
+ || targetPose == null);
}
@Override
- public boolean isFinished(){
+ public boolean isFinished() {
return debouncer.calculate(withinTolerance(driveTolerance, new Rotation2d(thetaTolerance)));
}
}
import frc.robot.constants.swerve.DriveConstants;
import frc.robot.subsystems.drivetrain.Drivetrain;
-/**
-* Moves the robot to a pose using PathPlanner
-*/
+/** Moves the robot to a pose using PathPlanner */
public class GoToPose extends SequentialCommandGroup {
private Drivetrain drive;
/**
* Uses PathPlanner to go to a pose
+ *
* @param poseSupplier The supplier for the pose to use
* @param drive The drivetrain
*/
public GoToPose(Supplier<Pose2d> poseSupplier, Drivetrain drive) {
this(poseSupplier, AutoConstants.MAX_AUTO_SPEED, AutoConstants.MAX_AUTO_ACCEL, drive);
}
- public GoToPose(Pose2d pose, Drivetrain drive){
- this(()->pose, drive);
+
+ public GoToPose(Pose2d pose, Drivetrain drive) {
+ this(() -> pose, drive);
}
/**
* Uses PathPlanner to go to a pose
+ *
* @param poseSupplier The supplier for the pose to use
* @param maxSpeed The maximum speed to use
* @param maxAccel The maximum acceleration to use
* @param drive The drivetrain
*/
- public GoToPose(Supplier<Pose2d> poseSupplier, double maxSpeed, double maxAccel, Drivetrain drive) {
+ public GoToPose(
+ Supplier<Pose2d> poseSupplier, double maxSpeed, double maxAccel, Drivetrain drive) {
this.poseSupplier = poseSupplier;
this.maxSpeed = maxSpeed;
this.maxAccel = maxAccel;
this.drive = drive;
addCommands(
- new InstantCommand(()->drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE)),
- new SupplierCommand(() -> createCommand(), drive).handleInterrupt(()->drive.setVisionEnabled(true)),
- new InstantCommand(()->drive.setVisionEnabled(true))
- );
+ new InstantCommand(() -> drive.setVisionEnabled(VisionConstants.ENABLED_GO_TO_POSE)),
+ new SupplierCommand(() -> createCommand(), drive)
+ .handleInterrupt(() -> drive.setVisionEnabled(true)),
+ new InstantCommand(() -> drive.setVisionEnabled(true)));
}
- /**
- * Creates the PathPlanner command and schedules it
- */
+ /** Creates the PathPlanner command and schedules it */
public Command createCommand() {
Pose2d pose = poseSupplier.get();
- if(pose==null){
+ if (pose == null) {
return new DoNothing();
}
- Command command = AutoBuilder.pathfindToPose(
- pose,
- new PathConstraints(maxSpeed, maxAccel, DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL),
- 0
- );
+ Command command =
+ AutoBuilder.pathfindToPose(
+ pose,
+ new PathConstraints(
+ maxSpeed,
+ maxAccel,
+ DriveConstants.MAX_ANGULAR_SPEED,
+ DriveConstants.MAX_ANGULAR_ACCEL),
+ 0);
// get the distance to the pose.
double dist = drive.getPose().minus(pose).getTranslation().getNorm();
- // if greater than 3m or less than 2 cm, don't run it. If the path is too small pathplanner makes weird paths.
+ // if greater than 3m or less than 2 cm, don't run it. If the path is too small pathplanner
+ // makes weird paths.
if (dist > 3) {
command = new DoNothing();
DriverStation.reportWarning("Alignment Path too long, doing nothing, GoToPose.java", false);
import frc.robot.constants.VisionConstants;
import frc.robot.subsystems.drivetrain.Drivetrain;
-/**
- * Runs the chassis PIDs to move the robot to a specific pose.
- */
+/** Runs the chassis PIDs to move the robot to a specific pose. */
public class GoToPosePID extends Command {
- private Drivetrain drive;
-
+ private Drivetrain drive;
+
private Supplier<Pose2d> poseSupplier;
private Pose2d pose;
-
+
/**
- * Runs the chassis PIDs to move the robot to a specific pose.
+ * Runs the chassis PIDs to move the robot to a specific pose.
+ *
* @param pose The pose supplier to go to
* @param drive The drivetrain
*/
addRequirements(drive);
}
- public GoToPosePID(Pose2d pose, Drivetrain drive){
- this(()->pose, drive);
+ public GoToPosePID(Pose2d pose, Drivetrain drive) {
+ this(() -> pose, drive);
}
@Override
@Override
public void execute() {
- if(pose == null) {
+ if (pose == null) {
return;
}
- drive.driveWithPID(pose.getX(), pose.getY(), pose.getRotation().getRadians());
+ drive.driveWithPID(pose.getX(), pose.getY(), pose.getRotation().getRadians());
}
@Override
}
@Override
- public boolean isFinished() {
- return pose == null || drive.getXController().atSetpoint() && drive.getYController().atSetpoint() && drive.getRotationController().atSetpoint();
- }
-}
\ No newline at end of file
+ public boolean isFinished() {
+ return pose == null
+ || drive.getXController().atSetpoint()
+ && drive.getYController().atSetpoint()
+ && drive.getRotationController().atSetpoint();
+ }
+}
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.drivetrain.Drivetrain;
-/**
- * Sets the robot's wheels to an X formation to prevent being pushed around by other bots.
- */
+/** Sets the robot's wheels to an X formation to prevent being pushed around by other bots. */
public class SetFormationX extends SequentialCommandGroup {
- public SetFormationX(Drivetrain drive) {
- addRequirements(drive);
- addCommands(
- new InstantCommand(() -> drive.setStateDeadband(false), drive),
- new RunCommand(() -> drive.setModuleStates(new SwerveModuleState[]{
- new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))),
- new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
- new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
- new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45)))
- }, false), drive)
- );
- }
-}
\ No newline at end of file
+ public SetFormationX(Drivetrain drive) {
+ addRequirements(drive);
+ addCommands(
+ new InstantCommand(() -> drive.setStateDeadband(false), drive),
+ new RunCommand(
+ () ->
+ drive.setModuleStates(
+ new SwerveModuleState[] {
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))),
+ new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45)))
+ },
+ false),
+ drive));
+ }
+}
import frc.robot.subsystems.drivetrain.Drivetrain;
/**
- * Attempts to set all four modules to a constant angle. Determines if the modules are able to reach the angle requested in a certain time.
+ * Attempts to set all four modules to a constant angle. Determines if the modules are able to reach
+ * the angle requested in a certain time.
*/
public class SimplePresetSteerAngles extends InstantCommand {
- /**
- * sets the angle of module steer to 0 to remove initial turn time and drift
- *
- * @param drive drivetrain to be used
- */
- public SimplePresetSteerAngles(Drivetrain drive) {
- this(drive, new Rotation2d());
- }
+ /**
+ * sets the angle of module steer to 0 to remove initial turn time and drift
+ *
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive) {
+ this(drive, new Rotation2d());
+ }
- /**
- * sets the angle of module steer to a angle to remove initial turn time and drift
- *
- * @param angle angle to set module steer to in radians
- * @param drive drivetrain to be used
- */
- public SimplePresetSteerAngles(Drivetrain drive, double angle) {
- this(drive, new Rotation2d(angle));
- }
+ /**
+ * sets the angle of module steer to a angle to remove initial turn time and drift
+ *
+ * @param angle angle to set module steer to in radians
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive, double angle) {
+ this(drive, new Rotation2d(angle));
+ }
- /**
- * sets the angle of module steer to a angle to remove initial turn time and drift
- *
- * @param rotation rotation to set module steer to
- * @param drive drivetrain to be used
- */
- public SimplePresetSteerAngles(Drivetrain drive, Rotation2d rotation) {
- super(() -> {
- drive.setStateDeadband(false);
- drive.setModuleStates(new SwerveModuleState[]{
- new SwerveModuleState(0, rotation),
- new SwerveModuleState(0, rotation),
- new SwerveModuleState(0, rotation),
- new SwerveModuleState(0, rotation)
- }, true);
- drive.setStateDeadband(true);
- }, drive);
- drive.setStateDeadband(true);
- }
+ /**
+ * sets the angle of module steer to a angle to remove initial turn time and drift
+ *
+ * @param rotation rotation to set module steer to
+ * @param drive drivetrain to be used
+ */
+ public SimplePresetSteerAngles(Drivetrain drive, Rotation2d rotation) {
+ super(
+ () -> {
+ drive.setStateDeadband(false);
+ drive.setModuleStates(
+ new SwerveModuleState[] {
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation),
+ new SwerveModuleState(0, rotation)
+ },
+ true);
+ drive.setStateDeadband(true);
+ },
+ drive);
+ drive.setStateDeadband(true);
+ }
}
package frc.robot.commands.drive_comm;
-
import org.littletonrobotics.junction.Logger;
-import com.ctre.phoenix6.SignalLogger;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.util.SysId;
-/**
- * A command to run all 4 SysId routines on the drivetrain
-*/
+/** A command to run all 4 SysId routines on the drivetrain */
public class SysIDDriveCommand extends SequentialCommandGroup {
- private Config config = new Config();
- private SysId sysId;
- public SysIDDriveCommand(Drivetrain drive) {
- config = new Config(
+ private Config config = new Config();
+ private SysId sysId;
+
+ public SysIDDriveCommand(Drivetrain drive) {
+ config =
+ new Config(
Units.Volts.of(0.5).per(Units.Seconds),
Units.Volts.of(3),
Units.Seconds.of(5),
- (state) -> Logger.recordOutput("SysIdTestState", state.toString())
- );
- Rotation2d[] angles = {
- Rotation2d.fromDegrees(0),//-45-180
- Rotation2d.fromDegrees(0),//45
- Rotation2d.fromDegrees(0),//45+180
- Rotation2d.fromDegrees(0),//-45
- };
- sysId = new SysId(
+ (state) -> Logger.recordOutput("SysIdTestState", state.toString()));
+ Rotation2d[] angles = {
+ Rotation2d.fromDegrees(0), // -45-180
+ Rotation2d.fromDegrees(0), // 45
+ Rotation2d.fromDegrees(0), // 45+180
+ Rotation2d.fromDegrees(0), // -45
+ };
+ sysId =
+ new SysId(
"Drivetrain",
- x ->{
- drive.setAngleMotors(angles);
- drive.setDriveVoltages(x);
- },
+ x -> {
+ drive.setAngleMotors(angles);
+ drive.setDriveVoltages(x);
+ },
drive,
- config
- );
- addCommands(
- sysId.runQuasisStatic(Direction.kForward),
- new WaitCommand(0.5),
- sysId.runQuasisStatic(Direction.kReverse),
- new WaitCommand(0.5),
- sysId.runDynamic(Direction.kForward),
- new WaitCommand(0.5),
- sysId.runDynamic(Direction.kReverse)
- );
- }
-
-
-
+ config);
+ addCommands(
+ sysId.runQuasisStatic(Direction.kForward),
+ new WaitCommand(0.5),
+ sysId.runQuasisStatic(Direction.kReverse),
+ new WaitCommand(0.5),
+ sysId.runDynamic(Direction.kForward),
+ new WaitCommand(0.5),
+ sysId.runDynamic(Direction.kReverse));
+ }
}
import frc.robot.constants.swerve.DriveConstants;
import frc.robot.subsystems.drivetrain.Drivetrain;
-/**
- * Sets all module angles to a given trajectory's initial angle.
- */
+/** Sets all module angles to a given trajectory's initial angle. */
public class TrajectoryPresetSteerAngles extends InstantCommand {
- /*
- * make sure to add wait command after called to give time to correct
- */
- public TrajectoryPresetSteerAngles(Drivetrain drive, Trajectory trajectory) {
- super(
- () -> {
-
- // 0.01 is the time between trajectory samples, in seconds
- // Can be replaced for any small number, but it should be the same as the time between all uses
- double time = 0.01;
-
- drive.setStateDeadband(false);
-
- Pose2d initialPose = trajectory.getInitialPose();
- State sample = trajectory.sample(time);
- Pose2d nextPose = sample.poseMeters;
-
- double xVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getCos();
- double yVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getSin();
- double angularVelo = (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time;
-
- ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelo);
- chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation());
-
- SwerveModuleState[] swerveModuleStates = DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds);
- for (SwerveModuleState swerveModuleState : swerveModuleStates) {
- swerveModuleState.speedMetersPerSecond = 0;
- }
- drive.setModuleStates(swerveModuleStates, true);
- drive.setStateDeadband(true);
- },
- drive
- );
-
- }
+ /*
+ * make sure to add wait command after called to give time to correct
+ */
+ public TrajectoryPresetSteerAngles(Drivetrain drive, Trajectory trajectory) {
+ super(
+ () -> {
+
+ // 0.01 is the time between trajectory samples, in seconds
+ // Can be replaced for any small number, but it should be the same as the time between all
+ // uses
+ double time = 0.01;
+
+ drive.setStateDeadband(false);
+
+ Pose2d initialPose = trajectory.getInitialPose();
+ State sample = trajectory.sample(time);
+ Pose2d nextPose = sample.poseMeters;
+
+ double xVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getCos();
+ double yVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getSin();
+ double angularVelo =
+ (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time;
+
+ ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelo);
+ chassisSpeeds =
+ ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation());
+
+ SwerveModuleState[] swerveModuleStates =
+ DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds);
+ for (SwerveModuleState swerveModuleState : swerveModuleStates) {
+ swerveModuleState.speedMetersPerSecond = 0;
+ }
+ drive.setModuleStates(swerveModuleStates, true);
+ drive.setStateDeadband(true);
+ },
+ drive);
+ }
}
import frc.robot.subsystems.PowerControl.Battery;
import frc.robot.subsystems.PowerControl.EMABreaker;
-
public class PowerControl extends Command {
- // my beautiful power control subsystems
- private EMABreaker breaker;
- private Battery battery;
- // TODO: add subsystems back when implementing logic:
- // the real subsystems
- // private Drivetrain drivetrain;
- // private Shooter shooter;
- // private Turret turret;
- // private Hood hood;
- // private Intake intake;
- // private Spindexer spindexer;
+ // my beautiful power control subsystems
+ private EMABreaker breaker;
+ private Battery battery;
+ // TODO: add subsystems back when implementing logic:
+ // the real subsystems
+ // private Drivetrain drivetrain;
+ // private Shooter shooter;
+ // private Turret turret;
+ // private Hood hood;
+ // private Intake intake;
+ // private Spindexer spindexer;
- public SeverityLevel severityLevel;
+ public SeverityLevel severityLevel;
- public enum SeverityLevel {
- SEVERITY_LVL_ZERO,
- SEVERITY_LVL_ONE,
- SEVERITY_LVL_TWO,
- SEVERITY_LVL_THREE,
- SEVERITY_LVL_FOUR,
- SEVERITY_LVL_FIVE,
- }
+ public enum SeverityLevel {
+ SEVERITY_LVL_ZERO,
+ SEVERITY_LVL_ONE,
+ SEVERITY_LVL_TWO,
+ SEVERITY_LVL_THREE,
+ SEVERITY_LVL_FOUR,
+ SEVERITY_LVL_FIVE,
+ }
- public PowerControl(
- EMABreaker breaker, // pc
- Battery battery // pc
- // Drivetrain drivetrain, // main draw
- // Shooter shooter, // aiming (vital)
- // Turret turret, // aiming
- // Hood hood, // aiming
- // Intake intake, // bps
- // Spindexer spindexer // bps
- ) {
- this.breaker = breaker;
- this.battery = battery;
- // this.drivetrain = drivetrain;
- // this.shooter = shooter;
- // this.turret = turret;
- // this.hood = hood;
- // this.intake = intake;
- // this.spindexer = spindexer;
+ public PowerControl(
+ EMABreaker breaker, // pc
+ Battery battery // pc
+ // Drivetrain drivetrain, // main draw
+ // Shooter shooter, // aiming (vital)
+ // Turret turret, // aiming
+ // Hood hood, // aiming
+ // Intake intake, // bps
+ // Spindexer spindexer // bps
+ ) {
+ this.breaker = breaker;
+ this.battery = battery;
+ // this.drivetrain = drivetrain;
+ // this.shooter = shooter;
+ // this.turret = turret;
+ // this.hood = hood;
+ // this.intake = intake;
+ // this.spindexer = spindexer;
- addRequirements(breaker, battery); // not sure if I'll need requirement access for setting new current limits
- }
+ addRequirements(
+ breaker,
+ battery); // not sure if I'll need requirement access for setting new current limits
+ }
- @Override
- public void initialize() {
- severityLevel = SeverityLevel.SEVERITY_LVL_ZERO;
- }
+ @Override
+ public void initialize() {
+ severityLevel = SeverityLevel.SEVERITY_LVL_ZERO;
+ }
- @Override
- public void execute() {
- double[] worstFilter = breaker.percentageMaxUsage();
- double percentage = worstFilter[0]; // percentage of current average until we trip breaker
- double tau = worstFilter[1]; // how quickly this issue is happenning and if we need to respond quickly
+ @Override
+ public void execute() {
+ double[] worstFilter = breaker.percentageMaxUsage();
+ double percentage = worstFilter[0]; // percentage of current average until we trip breaker
+ double tau =
+ worstFilter[1]; // how quickly this issue is happenning and if we need to respond quickly
- // Some logic here
- }
+ // Some logic here
+ }
- @Override
- public void end(boolean interupted) {
- severityLevel = SeverityLevel.SEVERITY_LVL_ZERO; // in the case of disabling this command we shoud reset its effects
- }
+ @Override
+ public void end(boolean interupted) {
+ severityLevel =
+ SeverityLevel
+ .SEVERITY_LVL_ZERO; // in the case of disabling this command we shoud reset its effects
+ }
}
-
import frc.robot.util.Vision.DetectedObject;
public class AcquireGamePiece extends SequentialCommandGroup {
- /**
- * Intakes a game piece
- *
- * @param gamePiece The supplier for the game piece to intake
- * @param drive The drivetrain
- */
- public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive){
- // TODO: Replace DoNothing with next year's intake command
- addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive)));
- }
-}
\ No newline at end of file
+ /**
+ * Intakes a game piece
+ *
+ * @param gamePiece The supplier for the game piece to intake
+ * @param drive The drivetrain
+ */
+ public AcquireGamePiece(Supplier<DetectedObject> gamePiece, Drivetrain drive) {
+ // TODO: Replace DoNothing with next year's intake command
+ addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive)));
+ }
+}
import frc.robot.util.Vision.DetectedObject;
public class AimAtGamePiece extends DefaultDriveCommand {
- private Supplier<DetectedObject> objectSupplier;
- private static int ticksSinceLastObject;
- private static DetectedObject cachedObject;
-
-
- public AimAtGamePiece(Drivetrain drive, BaseDriverConfig driver, Supplier<DetectedObject> objectSupplier){
- super(drive, driver);
- this.objectSupplier = objectSupplier;
+ private Supplier<DetectedObject> objectSupplier;
+ private static int ticksSinceLastObject;
+ private static DetectedObject cachedObject;
+
+ public AimAtGamePiece(
+ Drivetrain drive, BaseDriverConfig driver, Supplier<DetectedObject> objectSupplier) {
+ super(drive, driver);
+ this.objectSupplier = objectSupplier;
+ }
+
+ @Override
+ public void initialize() {
+ cachedObject = null;
+ ticksSinceLastObject = 0;
+ super.initialize();
+ }
+
+ @Override
+ protected void drive(ChassisSpeeds speeds) {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
+ super.drive(speeds);
+ return;
}
+ DetectedObject object = objectSupplier.get();
- @Override
- public void initialize() {
- cachedObject = null;
- ticksSinceLastObject = 0;
- super.initialize();
- }
-
- @Override
- protected void drive(ChassisSpeeds speeds){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED){
- super.drive(speeds);
- return;
- }
- DetectedObject object = objectSupplier.get();
-
- if(object == null || !object.isGamePiece()) {
- if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
- object = cachedObject;
- } else {
- super.drive(speeds);
- return;
- }
- ticksSinceLastObject++;
- } else {
- ticksSinceLastObject = 0;
- cachedObject = object;
- }
-
- // System.out.println("objangle " + object.getAngle());
- swerve.driveHeading(
- speeds.vxMetersPerSecond,
- speeds.vyMetersPerSecond,
- MathUtil.angleModulus(object.getAngle()),
- true);
+ if (object == null || !object.isGamePiece()) {
+ if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
+ object = cachedObject;
+ } else {
+ super.drive(speeds);
+ return;
+ }
+ ticksSinceLastObject++;
+ } else {
+ ticksSinceLastObject = 0;
+ cachedObject = object;
}
+
+ // System.out.println("objangle " + object.getAngle());
+ swerve.driveHeading(
+ speeds.vxMetersPerSecond,
+ speeds.vyMetersPerSecond,
+ MathUtil.angleModulus(object.getAngle()),
+ true);
+ }
}
import frc.robot.constants.FieldConstants;
import frc.robot.subsystems.drivetrain.Drivetrain;
-/**
- * Aims the robot at the closest April tag
- */
+/** Aims the robot at the closest April tag */
public class AimAtTag extends Command {
private Drivetrain drive;
private PIDController pid;
/**
* Aims the robot at the closest April tag
+ *
* @param drive The drivetrain
*/
- public AimAtTag(Drivetrain drive){
+ public AimAtTag(Drivetrain drive) {
this.drive = drive;
// Copy drive PID and changetolerance
- pid = new PIDController(
- drive.getRotationController().getP(),
- drive.getRotationController().getI(),
- drive.getRotationController().getD()
- );
+ pid =
+ new PIDController(
+ drive.getRotationController().getP(),
+ drive.getRotationController().getI(),
+ drive.getRotationController().getD());
pid.setTolerance(Units.degreesToRadians(1));
addRequirements(drive);
}
- /**
- * Gets the closest tag and sets the setpoint to aim at it
- */
+ /** Gets the closest tag and sets the setpoint to aim at it */
@Override
- public void initialize(){
+ public void initialize() {
double dist = Double.POSITIVE_INFINITY;
Translation2d closest = new Translation2d();
Translation2d driveTranslation = drive.getPose().getTranslation();
- for(AprilTag tag : FieldConstants.field.getTags()){
+ for (AprilTag tag : FieldConstants.field.getTags()) {
Translation2d translation = tag.pose.toPose2d().getTranslation();
double dist2 = driveTranslation.getDistance(translation);
- if(dist2 < dist){
+ if (dist2 < dist) {
dist = dist2;
closest = translation;
}
}
pid.reset();
- pid.setSetpoint(Math.atan2(closest.getY() - driveTranslation.getY(), closest.getX() - driveTranslation.getX()));
+ pid.setSetpoint(
+ Math.atan2(
+ closest.getY() - driveTranslation.getY(), closest.getX() - driveTranslation.getX()));
}
-
- /**
- * Runs the PID
- */
+
+ /** Runs the PID */
@Override
public void execute() {
double angle = drive.getPose().getRotation().getRadians();
- // If the distance between the angles is more than 180 degrees, use an identical angle ±360 degrees
- if(angle - pid.getSetpoint() > Math.PI){
- angle -= 2*Math.PI;
- }else if(angle - pid.getSetpoint() < -Math.PI){
- angle += 2*Math.PI;
+ // If the distance between the angles is more than 180 degrees, use an identical angle ±360
+ // degrees
+ if (angle - pid.getSetpoint() > Math.PI) {
+ angle -= 2 * Math.PI;
+ } else if (angle - pid.getSetpoint() < -Math.PI) {
+ angle += 2 * Math.PI;
}
double speed = pid.calculate(angle);
drive.drive(0, 0, speed, true, false);
/**
* Stops the drivetrain
+ *
* @param interrupted If the command is interrupted
*/
@Override
/**
* Returns if the command is finished
+ *
* @return If the PID is at the setpoint
*/
@Override
return pid.atSetpoint();
}
}
-
import frc.robot.util.MathUtils;
import frc.robot.util.Vision.Vision;
-/**
- * Calculates standard deviations for vision
- */
+/** Calculates standard deviations for vision */
public class CalculateStdDevs extends Command {
private final Vision vision;
private ArrayList<Pose2d> poses;
/**
* Constructor for CalculateStdDevs
- * @param posesToUse the amount of poses to take the standard deviation of. More poses will take more time.
+ *
+ * @param posesToUse the amount of poses to take the standard deviation of. More poses will take
+ * more time.
* @param vision The vision
*/
public CalculateStdDevs(int posesToUse, Vision vision, Drivetrain drive) {
endTimer = new Timer();
}
- /**
- * Resets the pose array
- */
+ /** Resets the pose array */
@Override
public void initialize() {
// create the ArrayList of poses to store
- // an ArrayList prevents issues if the command ends early, and makes checking if the command has finished easy
+ // an ArrayList prevents issues if the command ends early, and makes checking if the command has
+ // finished easy
poses = new ArrayList<Pose2d>();
drive.setVisionEnabled(false);
}
- /**
- * Adds a pose to the array
- */
+ /** Adds a pose to the array */
@Override
public void execute() {
Pose2d pose = vision.getPose2d(drive.getPose());
endTimer.reset();
// add the pose to our data
poses.add(pose);
- if(poses.size()%10==0){
- System.out.printf("%.0f%% done\n", ((double)poses.size())/arrayLength * 100);
+ if (poses.size() % 10 == 0) {
+ System.out.printf("%.0f%% done\n", ((double) poses.size()) / arrayLength * 100);
}
} else {
endTimer.start();
}
}
- /**
- * Calculates the standard deviation
- */
+ /** Calculates the standard deviation */
@Override
public void end(boolean interrupted) {
drive.setVisionEnabled(true);
// If the array is empty, don't try to calculate std devs
if (poses.size() == 0) {
- System.out.println("There are no poses in the array\nTry again where the robot can see an April tag.");
+ System.out.println(
+ "There are no poses in the array\nTry again where the robot can see an April tag.");
return;
}
-
+
// create arrays of the poses by X, Y, and Rotation for calculations
double[] xArray = new double[poses.size()];
double[] yArray = new double[poses.size()];
double stdDevX = MathUtils.stdDev(xArray);
double stdDevY = MathUtils.stdDev(yArray);
double stdDevRot = MathUtils.stdDev(rotArray);
-
+
// Find distance to tag
double distance;
- try{
- distance = vision.getEstimatedPoses(drive.getPose()).get(0).targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm();
- }catch(Exception e){
- System.out.println("Could not see a target");
- distance = -1;
+ try {
+ distance =
+ vision
+ .getEstimatedPoses(drive.getPose())
+ .get(0)
+ .targetsUsed
+ .get(0)
+ .getBestCameraToTarget()
+ .getTranslation()
+ .getNorm();
+ } catch (Exception e) {
+ System.out.println("Could not see a target");
+ distance = -1;
}
-
+
// Print and log values
- System.out.printf("Standard deviation values:\nX: %.5f\nY: %.5f\nRotation: %.5f\nDistance: %.5f\n",
- stdDevX, stdDevY, stdDevRot, distance);
+ System.out.printf(
+ "Standard deviation values:\nX: %.5f\nY: %.5f\nRotation: %.5f\nDistance: %.5f\n",
+ stdDevX, stdDevY, stdDevRot, distance);
}
/**
* Returns if the command is finished
+ *
* @return If the array is full
*/
@Override
public boolean isFinished() {
return poses.size() >= arrayLength;
}
-}
\ No newline at end of file
+}
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.util.Vision.DetectedObject;
-/**
- * Moves toward the detected object
- */
+/** Moves toward the detected object */
public class DriveToGamePiece extends DriveToPose {
private static boolean constantUpdate = true;
private static int ticksSinceLastObject;
/**
* Moves toward the detected object
+ *
* @param detectedObject The supplier for the detected object to use
* @param drive The drivetrain
*/
public DriveToGamePiece(Supplier<DetectedObject> detectedObject, Drivetrain drive) {
- super(drive,
- () -> getPose(detectedObject, drive)
- );
+ super(drive, () -> getPose(detectedObject, drive));
updateTarget = constantUpdate;
}
super.initialize();
}
- public static Pose2d getPose(Supplier<DetectedObject> supplier, Drivetrain drive){
+ public static Pose2d getPose(Supplier<DetectedObject> supplier, Drivetrain drive) {
DetectedObject object = supplier.get();
- if(object == null || !object.isGamePiece()) {
+ if (object == null || !object.isGamePiece()) {
if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) {
object = cachedObject;
} else {
Rotation2d rotation = new Rotation2d(MathUtil.angleModulus(object.getAngle()));
Translation2d objectTranslation = object.pose.toPose2d().getTranslation();
Translation2d diff = objectTranslation.minus(drive.getPose().getTranslation());
- Translation2d translation = objectTranslation.minus(diff.times(DriveConstants.ROBOT_WIDTH_WITH_BUMPERS/2/diff.getNorm()));
+ Translation2d translation =
+ objectTranslation.minus(
+ diff.times(DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2 / diff.getNorm()));
return new Pose2d(translation, rotation);
}
-}
\ No newline at end of file
+}
import frc.robot.subsystems.drivetrain.Drivetrain;
public class GoToPose2 extends Command {
- private static final double MIN_ACCEL = 2;
-
private final Supplier<Pose2d> poseSupplier;
- private final Drivetrain drive;
- private Pose2d pose;
- private double vx;
- private double vy;
- private Pose2d error;
+ private static final double MIN_ACCEL = 2;
+ private final Supplier<Pose2d> poseSupplier;
+ private final Drivetrain drive;
+ private Pose2d pose;
+ private double vx;
+ private double vy;
+ private Pose2d error;
- public GoToPose2(Supplier<Pose2d> poseSupplier, Drivetrain drive){
- this.poseSupplier = poseSupplier;
- this.drive = drive;
- addRequirements(drive);
- }
+ public GoToPose2(Supplier<Pose2d> poseSupplier, Drivetrain drive) {
+ this.poseSupplier = poseSupplier;
+ this.drive = drive;
+ addRequirements(drive);
+ }
- @Override
- public void initialize(){
- pose = poseSupplier.get();
- ChassisSpeeds v = drive.getChassisSpeeds();
- vx = v.vxMetersPerSecond;
- vy = v.vyMetersPerSecond;
- error = null;
- }
+ @Override
+ public void initialize() {
+ pose = poseSupplier.get();
+ ChassisSpeeds v = drive.getChassisSpeeds();
+ vx = v.vxMetersPerSecond;
+ vy = v.vyMetersPerSecond;
+ error = null;
+ }
- @Override
- public void execute(){
- if(pose == null){
- return;
- }
- Pose2d drivePose = drive.getPose();
- error = drivePose.relativeTo(pose);
- double ax = calcAccel(vx, error.getX());
- double ay = calcAccel(vy, error.getY());
- if(Math.abs(ax) < MIN_ACCEL && Math.abs(error.getX()) > 0.01){
- ax = -Math.signum(error.getX())*MIN_ACCEL;
- }
- if(Math.abs(ay) < MIN_ACCEL && Math.abs(error.getY()) > 0.01){
- ay = -Math.signum(error.getY())*MIN_ACCEL;
- }
- vx += ax*Constants.LOOP_TIME;
- vy += ay*Constants.LOOP_TIME;
- Translation2d v = new Translation2d(vx, vy).rotateBy(pose.getRotation());
- drive.driveHeading(v.getX(), v.getY(), pose.getRotation().getRadians(), true);
+ @Override
+ public void execute() {
+ if (pose == null) {
+ return;
}
-
- @Override
- public void end(boolean interrupted){
- drive.stop();
+ Pose2d drivePose = drive.getPose();
+ error = drivePose.relativeTo(pose);
+ double ax = calcAccel(vx, error.getX());
+ double ay = calcAccel(vy, error.getY());
+ if (Math.abs(ax) < MIN_ACCEL && Math.abs(error.getX()) > 0.01) {
+ ax = -Math.signum(error.getX()) * MIN_ACCEL;
}
-
- @Override
- public boolean isFinished(){
- return pose == null || error != null && error.getTranslation().getNorm() < 0.01;
+ if (Math.abs(ay) < MIN_ACCEL && Math.abs(error.getY()) > 0.01) {
+ ay = -Math.signum(error.getY()) * MIN_ACCEL;
}
+ vx += ax * Constants.LOOP_TIME;
+ vy += ay * Constants.LOOP_TIME;
+ Translation2d v = new Translation2d(vx, vy).rotateBy(pose.getRotation());
+ drive.driveHeading(v.getX(), v.getY(), pose.getRotation().getRadians(), true);
+ }
- private double calcAccel(double v, double x){
- if(Math.abs(x) < 0.001 || Math.abs(Math.signum(v) - Math.signum(x)) < 0.5){
- return 0;
- }
- double a = v*v/2/x;
- double a2 = -v/Constants.LOOP_TIME;
- if(Math.abs(a2) < Math.abs(a)){
- return a2;
- }
- return a;
+ @Override
+ public void end(boolean interrupted) {
+ drive.stop();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return pose == null || error != null && error.getTranslation().getNorm() < 0.01;
+ }
+
+ private double calcAccel(double v, double x) {
+ if (Math.abs(x) < 0.001 || Math.abs(Math.signum(v) - Math.signum(x)) < 0.5) {
+ return 0;
+ }
+ double a = v * v / 2 / x;
+ double a2 = -v / Constants.LOOP_TIME;
+ if (Math.abs(a2) < Math.abs(a)) {
+ return a2;
}
+ return a;
+ }
}
import frc.robot.util.Vision.DetectedObject;
public class LogVision extends Command {
- private Supplier<DetectedObject> objectSupplier;
- public LogVision(Supplier<DetectedObject> objectSupplier){
- this.objectSupplier = objectSupplier;
- }
+ private Supplier<DetectedObject> objectSupplier;
- @Override
- public void execute() {
- DetectedObject object = this.objectSupplier.get();
- if (object != null) {
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Vision/object_angle", object.getAngle());
- Logger.recordOutput("Vision/object_distance", object.getDistance());
- }
- }
- }
+ public LogVision(Supplier<DetectedObject> objectSupplier) {
+ this.objectSupplier = objectSupplier;
+ }
- @Override
- public boolean runsWhenDisabled() {
- return true;
+ @Override
+ public void execute() {
+ DetectedObject object = this.objectSupplier.get();
+ if (object != null) {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Vision/object_angle", object.getAngle());
+ Logger.recordOutput("Vision/object_distance", object.getDistance());
+ }
}
+ }
- @Override
- public boolean isFinished() {
- return false;
- }
-
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
}
import frc.robot.util.Vision.DetectedObject;
import frc.robot.util.Vision.Vision;
-/**
- * Adds data from object detection vision to SmartDashboard
- */
-public class ReturnData extends Command{
- private final Vision vision;
+/** Adds data from object detection vision to SmartDashboard */
+public class ReturnData extends Command {
+ private final Vision vision;
private final Timer timer = new Timer();
/**
* Adds data from object detection vision to Smartdashboard
+ *
* @param vision The vision
*/
- public ReturnData(Vision vision){
+ public ReturnData(Vision vision) {
this.vision = vision;
}
@Override
- public void initialize(){
+ public void initialize() {
timer.reset();
timer.start();
}
-
- /**
- * Adds the data to SmartDashboard
- */
+
+ /** Adds the data to SmartDashboard */
@Override
public void execute() {
- if(timer.advanceIfElapsed(2)){
- double[] xOffset = vision.getHorizontalOffset();
- double[] yOffset = vision.getVerticalOffset();
- // long[] objectClass = vision.getDetectedObjectClass();
+ if (timer.advanceIfElapsed(2)) {
+ double[] xOffset = vision.getHorizontalOffset();
+ double[] yOffset = vision.getVerticalOffset();
+ // long[] objectClass = vision.getDetectedObjectClass();
- // //put the offsets and area on SmartDashboard for testing
- // SmartDashboard.putNumberArray("Object X offsets degrees", xOffset);
- // SmartDashboard.putNumberArray("Object Y offsets degrees", yOffset);
- // SmartDashboard.putNumberArray("Object Distances", vision.getDistance());
+ // //put the offsets and area on SmartDashboard for testing
+ // SmartDashboard.putNumberArray("Object X offsets degrees", xOffset);
+ // SmartDashboard.putNumberArray("Object Y offsets degrees", yOffset);
+ // SmartDashboard.putNumberArray("Object Distances", vision.getDistance());
- DetectedObject bestGamePiece = vision.getBestGamePiece(Math.PI, false);
- if(bestGamePiece!=null){
- // SmartDashboard.putString("Vision best game piece", bestGamePiece.toString());
- System.out.println("\n\nBest game piece: "+bestGamePiece);
- }
-
- if ((xOffset.length != 0) == (yOffset.length != 0)) {
- for (int i = 0; i < xOffset.length; i++) {
- System.out.printf("\nx: %.2f, y: %.2f, type: %s\n", xOffset[i], yOffset[i], DetectedObject.getType(0));
- DetectedObject object = new DetectedObject(Units.degreesToRadians(xOffset[i]), Units.degreesToRadians(yOffset[i]), 0, VisionConstants.OBJECT_DETECTION_CAMERAS.get(0));
- System.out.printf("Object: %s\nDistance: %.2f, Angle: %.2f\n", object, object.getDistance(), Units.radiansToDegrees(object.getAngle()));
- }
- }else {
- System.out.println("One of the arrays is empty!");
+ DetectedObject bestGamePiece = vision.getBestGamePiece(Math.PI, false);
+ if (bestGamePiece != null) {
+ // SmartDashboard.putString("Vision best game piece", bestGamePiece.toString());
+ System.out.println("\n\nBest game piece: " + bestGamePiece);
+ }
+
+ if ((xOffset.length != 0) == (yOffset.length != 0)) {
+ for (int i = 0; i < xOffset.length; i++) {
+ System.out.printf(
+ "\nx: %.2f, y: %.2f, type: %s\n", xOffset[i], yOffset[i], DetectedObject.getType(0));
+ DetectedObject object =
+ new DetectedObject(
+ Units.degreesToRadians(xOffset[i]),
+ Units.degreesToRadians(yOffset[i]),
+ 0,
+ VisionConstants.OBJECT_DETECTION_CAMERAS.get(0));
+ System.out.printf(
+ "Object: %s\nDistance: %.2f, Angle: %.2f\n",
+ object, object.getDistance(), Units.radiansToDegrees(object.getAngle()));
}
+ } else {
+ System.out.println("One of the arrays is empty!");
}
}
+ }
/**
* Does nothing
+ *
* @param interrupted If the command is interrupted
*/
@Override
- public void end(boolean interrupted) {
-
- }
+ public void end(boolean interrupted) {}
/**
* Returns if the command is finished
+ *
* @retrun Always false (command never finishes)
*/
@Override
public boolean isFinished() {
- return false;
+ return false;
}
-
-
}
-
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import frc.robot.constants.VisionConstants;
-/**
- * Shutdown all Orange Pis listed by hostname in
- * {@link frc.robot.constants.VisionConstants}
- */
+/** Shutdown all Orange Pis listed by hostname in {@link frc.robot.constants.VisionConstants} */
public class ShutdownAllPis extends ParallelCommandGroup {
- public ShutdownAllPis() {
- ShutdownOrangePi[] commands =
- new ShutdownOrangePi[VisionConstants.ORANGEPI_HOSTNAMES.length];
- for (int i = 0; i < commands.length; i++) {
- commands[i] = new ShutdownOrangePi(VisionConstants.ORANGEPI_HOSTNAMES[i]);
- }
+ public ShutdownAllPis() {
+ ShutdownOrangePi[] commands = new ShutdownOrangePi[VisionConstants.ORANGEPI_HOSTNAMES.length];
+ for (int i = 0; i < commands.length; i++) {
+ commands[i] = new ShutdownOrangePi(VisionConstants.ORANGEPI_HOSTNAMES[i]);
+ }
- addCommands(commands);
- }
+ addCommands(commands);
+ }
}
import frc.robot.constants.VisionConstants;
/**
- * Run the ssh command to shutdown a single Orange Pi.
- * Uses the username and password set in {@link frc.robot.constants.VisionConstants}.
+ * Run the ssh command to shutdown a single Orange Pi. Uses the username and password set in {@link
+ * frc.robot.constants.VisionConstants}.
*/
public class ShutdownOrangePi extends Command {
- private String hostname;
- private Process process;
-
- /**
- * @param hostname The hostname or IP of the orangepi to shut down.
- */
- public ShutdownOrangePi(String hostname) {
- assert hostname != null;
- this.hostname = hostname;
- }
-
- @Override
- public boolean runsWhenDisabled() {
- return true;
- }
-
- @Override
- public void initialize() {
- if (Robot.isSimulation()) {
- // needs to run on an actual roborio because of architecture-specific binaries
- System.out.println("Would shut down OrangePi at " + hostname + " if this was real.");
- return;
- }
-
- try {
- String initialPath = Filesystem.getDeployDirectory() + "/sshpass";
- Path initalPathPath = Path.of(initialPath);
- String binPath = "/home/lvuser/sshpass2";
- Path binPathPath = Path.of(binPath);
- // copies to be able to get executable permissions on the new binary
- Files.copy(initalPathPath, binPathPath, StandardCopyOption.REPLACE_EXISTING);
- Files.setPosixFilePermissions(binPathPath, PosixFilePermissions.fromString("rwxr-xr-x"));
-
- String[] commandString = new String[] {
- binPath,
- "-p", "raspberry",
- "ssh",
- "-o", "StrictHostKeyChecking=no",
- VisionConstants.ORANGEPI_USERNAME + "@" + hostname,
- "sudo", "shutdown", "now"
- };
-
- this.process = Runtime.getRuntime().exec(commandString);
- } catch (Exception e) {
- String message = e.getMessage() == null ? "unknown" : e.getMessage();
- System.out.println("Failed to shutdown OrangePi. Reason: " + e.getClass() + " -- " + message);
- }
- }
-
- @Override
- public void execute() {
- if (this.process == null) return;
-
- try {
- InputStream stdout = this.process.getInputStream();
- InputStream stderr = this.process.getErrorStream();
-
- int remainingStdoutBytes = stdout.available();
- int remainingStderrBytes = stderr.available();
-
- if (remainingStdoutBytes > 0) {
- byte[] stdoutBytes = stdout.readNBytes(remainingStdoutBytes);
- System.out.println("OPI: " + new String(stdoutBytes, StandardCharsets.UTF_8));
- }
-
- if (remainingStderrBytes > 0) {
- byte[] stderrBytes = stderr.readNBytes(remainingStderrBytes);
- System.err.println("OPI: " + new String(stderrBytes, StandardCharsets.UTF_8));
- }
- } catch (IOException e) {}
- }
-
- @Override
- public boolean isFinished() {
- return this.process == null || !this.process.isAlive();
- }
-
- @Override
- public void end(boolean interrupted) {
- if (this.process == null) return;
-
- if (this.process.isAlive()) {
- this.process.destroy(); // end the process if we've been interrupted
- } else {
- // only grab exit value if the process has had time to exit
- int exitValue = this.process.exitValue();
- if (exitValue != 0) // abnormal termination
- System.out.println("OrangePi shutdown of " + hostname + " failed with exit code " + exitValue + ".");
- else
- System.out.println("OrangePi shutdown of " + hostname + " succesful.");
- }
- }
+ private String hostname;
+ private Process process;
+
+ /**
+ * @param hostname The hostname or IP of the orangepi to shut down.
+ */
+ public ShutdownOrangePi(String hostname) {
+ assert hostname != null;
+ this.hostname = hostname;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public void initialize() {
+ if (Robot.isSimulation()) {
+ // needs to run on an actual roborio because of architecture-specific binaries
+ System.out.println("Would shut down OrangePi at " + hostname + " if this was real.");
+ return;
+ }
+
+ try {
+ String initialPath = Filesystem.getDeployDirectory() + "/sshpass";
+ Path initalPathPath = Path.of(initialPath);
+ String binPath = "/home/lvuser/sshpass2";
+ Path binPathPath = Path.of(binPath);
+ // copies to be able to get executable permissions on the new binary
+ Files.copy(initalPathPath, binPathPath, StandardCopyOption.REPLACE_EXISTING);
+ Files.setPosixFilePermissions(binPathPath, PosixFilePermissions.fromString("rwxr-xr-x"));
+
+ String[] commandString =
+ new String[] {
+ binPath,
+ "-p",
+ "raspberry",
+ "ssh",
+ "-o",
+ "StrictHostKeyChecking=no",
+ VisionConstants.ORANGEPI_USERNAME + "@" + hostname,
+ "sudo",
+ "shutdown",
+ "now"
+ };
+
+ this.process = Runtime.getRuntime().exec(commandString);
+ } catch (Exception e) {
+ String message = e.getMessage() == null ? "unknown" : e.getMessage();
+ System.out.println("Failed to shutdown OrangePi. Reason: " + e.getClass() + " -- " + message);
+ }
+ }
+
+ @Override
+ public void execute() {
+ if (this.process == null) return;
+
+ try {
+ InputStream stdout = this.process.getInputStream();
+ InputStream stderr = this.process.getErrorStream();
+
+ int remainingStdoutBytes = stdout.available();
+ int remainingStderrBytes = stderr.available();
+
+ if (remainingStdoutBytes > 0) {
+ byte[] stdoutBytes = stdout.readNBytes(remainingStdoutBytes);
+ System.out.println("OPI: " + new String(stdoutBytes, StandardCharsets.UTF_8));
+ }
+
+ if (remainingStderrBytes > 0) {
+ byte[] stderrBytes = stderr.readNBytes(remainingStderrBytes);
+ System.err.println("OPI: " + new String(stderrBytes, StandardCharsets.UTF_8));
+ }
+ } catch (IOException e) {
+ }
+ }
+
+ @Override
+ public boolean isFinished() {
+ return this.process == null || !this.process.isAlive();
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ if (this.process == null) return;
+
+ if (this.process.isAlive()) {
+ this.process.destroy(); // end the process if we've been interrupted
+ } else {
+ // only grab exit value if the process has had time to exit
+ int exitValue = this.process.exitValue();
+ if (exitValue != 0) // abnormal termination
+ System.out.println(
+ "OrangePi shutdown of " + hostname + " failed with exit code " + exitValue + ".");
+ else System.out.println("OrangePi shutdown of " + hostname + " succesful.");
+ }
+ }
}
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.util.Vision.Vision;
-/**
- * Gathers data on the distance limits of the camera used for vision.
- */
+/** Gathers data on the distance limits of the camera used for vision. */
public class TestVisionDistance extends Command {
private final Drivetrain drive;
private final Vision vision;
/**
* Constructor for TestVisionDistance
+ *
* @param speed What speed to move at, negative if backward
* @param drive The drivetrain
* @param vision The vision
*/
- public TestVisionDistance(double speed, Drivetrain drive, Vision vision){
+ public TestVisionDistance(double speed, Drivetrain drive, Vision vision) {
addRequirements(drive);
this.drive = drive;
this.speed = speed;
this.vision = vision;
}
- /**
- * Starts the timers and disables vision for odometry
- */
+ /** Starts the timers and disables vision for odometry */
@Override
public void initialize() {
}
/**
- * Drives the robot, finds the pose from the drivetrain and vision, and someimes prints the distances
+ * Drives the robot, finds the pose from the drivetrain and vision, and someimes prints the
+ * distances
*/
@Override
public void execute() {
// If the camera can see the apriltag
if (newestPose != null) {
- //update current pose
+ // update current pose
currentPose = newestPose;
// reset the timer
endTimer.reset();
SmartDashboard.putNumber("Vision test drive distance", driveDistance);
SmartDashboard.putNumber("Vision test vision distnace", visionDistance);
SmartDashboard.putNumber("Vision test error", visionDistance - driveDistance);
- SmartDashboard.putNumber("Vision test % error", (visionDistance-driveDistance) / driveDistance * 100);
+ SmartDashboard.putNumber(
+ "Vision test % error", (visionDistance - driveDistance) / driveDistance * 100);
}
// If kPrintDelay seconds have passed, print the data
if (printTimer.advanceIfElapsed(PRINT_DELAY)) {
- System.out.printf("\nDrive dist: %.2f\nVision dist: %.2f\nError: %.2f\n %% error: %.2f\n",
- driveDistance, visionDistance,
- visionDistance-driveDistance, (visionDistance-driveDistance) / driveDistance * 100
- );
+ System.out.printf(
+ "\nDrive dist: %.2f\nVision dist: %.2f\nError: %.2f\n %% error: %.2f\n",
+ driveDistance,
+ visionDistance,
+ visionDistance - driveDistance,
+ (visionDistance - driveDistance) / driveDistance * 100);
}
} else {
endTimer.start();
}
}
- /**
- * Re-enables vision and stops the robot
- */
+ /** Re-enables vision and stops the robot */
@Override
public void end(boolean interrupted) {
drive.setVisionEnabled(true);
/**
* Returns if the command is finished
+ *
* @return If the end delay has elapsed
*/
@Override
public boolean isFinished() {
return endTimer.hasElapsed(END_DELAY);
}
-}
\ No newline at end of file
+}
import edu.wpi.first.math.system.plant.DCMotor;
import frc.robot.constants.swerve.DriveConstants;
-/**
- * Container class for auto constants.
- */
+/** Container class for auto constants. */
public class AutoConstants {
- // Pathplanner output folder should be src/main/deploy/pathplanner
- public static final String TRAJECTORY_DIRECTORY = "pathplanner/paths/";
+ // Pathplanner output folder should be src/main/deploy/pathplanner
+ public static final String TRAJECTORY_DIRECTORY = "pathplanner/paths/";
- public static final double MAX_AUTO_SPEED = 5.2; // m/s
- public static final double MAX_AUTO_ACCEL = 4.8; // m/s^2
+ public static final double MAX_AUTO_SPEED = 5.2; // m/s
+ public static final double MAX_AUTO_ACCEL = 4.8; // m/s^2
- public static RobotConfig CONFIG;
- public static final PPHolonomicDriveController AUTO_CONTROLLER = new PPHolonomicDriveController(
- new PIDConstants(3.5, 0.0, 1.0), // Translation PID constants
- new PIDConstants(4.0, 0.0, 1.0) // Rotation PID constants
- );
-
+ public static RobotConfig CONFIG;
+ public static final PPHolonomicDriveController AUTO_CONTROLLER =
+ new PPHolonomicDriveController(
+ new PIDConstants(3.5, 0.0, 1.0), // Translation PID constants
+ new PIDConstants(4.0, 0.0, 1.0) // Rotation PID constants
+ );
-
- static {
- try{
- CONFIG = RobotConfig.fromGUISettings();
- }catch(Exception e){
- e.printStackTrace();
- // Although these values are probably wrong and auto might not work correctly, at least it won't cause NullPointerExceptions
- CONFIG = new RobotConfig(50, 0.5, new ModuleConfig(DriveConstants.WHEEL_RADIUS, MAX_AUTO_SPEED, DriveConstants.COSF, DCMotor.getKrakenX60(1).withReduction(DriveConstants.DRIVE_GEAR_RATIO), DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT, 1), DriveConstants.MODULE_LOCATIONS);
- }
+ static {
+ try {
+ CONFIG = RobotConfig.fromGUISettings();
+ } catch (Exception e) {
+ e.printStackTrace();
+ // Although these values are probably wrong and auto might not work correctly, at least it
+ // won't cause NullPointerExceptions
+ CONFIG =
+ new RobotConfig(
+ 50,
+ 0.5,
+ new ModuleConfig(
+ DriveConstants.WHEEL_RADIUS,
+ MAX_AUTO_SPEED,
+ DriveConstants.COSF,
+ DCMotor.getKrakenX60(1).withReduction(DriveConstants.DRIVE_GEAR_RATIO),
+ DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT,
+ 1),
+ DriveConstants.MODULE_LOCATIONS);
}
+ }
}
public class Constants {
- // constants:
-
- public static final double GRAVITY_ACCELERATION = 9.8;
- public static final double ROBOT_VOLTAGE = 12.0;
- public static final double LOOP_TIME = 0.02;
-
- // CAN bus names
- public static final CANBus CANIVORE_CAN = new CANBus("CANivore");
- public static final CANBus CANIVORE_SUB = new CANBus("CANivoreSub");
- public static final CANBus RIO_CAN = new CANBus("rio");
-
- // Logging
- public static final boolean USE_TELEMETRY = true;
-
- // this would disable all logger calls
- public static final boolean DISABLE_LOGGING = true;
- public static final boolean DISABLE_SMART_DASHBOARD = true; // wont disable auto picker
-
- public static enum Mode {
- /** Running on a real robot. */
- REAL,
-
- /** Running a physics simulator. */
- SIM,
-
- /** Replaying from a log file. */
- REPLAY
- }
-
- // Kraken Speed
- public static double MAX_RPM = 5800.0; // Rotations per minute
-
- /*
- * Talon Stator / Supply Limits explanation
- * Supply current is current that's being drawn at the input bus voltage. Stator
- * current is current that's being drawn by the motor.
- * Supply limiting (supported by Talon FX and SRX) is useful for preventing
- * breakers from tripping in the PDP.
- * Stator limiting (supported by Talon FX) is useful for limiting
- * acceleration/heat.
- */
-
- // These are the default values
-
- // Stator
- public static final boolean TALONFX_STATOR_LIMIT_ENABLE = false; // enabled?
- public static final double TALONFX_STATOR_CURRENT_LIMIT = 100; // Limit(amp)
- public static final double TALONFX_STATOR_TRIGGER_THRESHOLD = 100; // Trigger Threshold(amp)
- public static final double TALONFX_STATOR_TRIGGER_DURATION = 0; // Trigger Threshold Time(s)
-
- // Supply
- public static final boolean TALONFX_SUPPLY_LIMIT_ENABLE = false; // enabled?
- public static final double TALONFX_SUPPLY_CURRENT_LIMIT = 40; // Limit(amp), current to hold after trigger hit
- public static final double TALONFX_SUPPLY_TRIGGER_THRESHOLD = 55; // (amp), amps to activate trigger
- public static final double TALONFX_SUPPLY_TRIGGER_DURATION = 3; // (s), how long after trigger before reducing
-
- // OIConstants:
-
- public static final int DRIVER_JOY = 0;
- public static final int OPERATOR_JOY = 1;
- public static final int TEST_JOY = 2;
- public static final int MANUAL_JOY = 3;
- public static final double DEFAULT_DEADBAND = 0.00005;
-
- public static final double TRANSLATIONAL_DEADBAND = 0.01;
-
- public static final double ROTATION_DEADBAND = 0.01;
-
- public static final double HEADING_DEADBAND = 0.05;
- public static final double HEADING_SLEWRATE = 10;
-
- //Modes
- public static final Mode SIM_MODE = Mode.SIM;
- public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE;
-
- // Enables 3D logs of mechanisms
- public static final boolean LOG_MECHANISMS = true;
-
- // Network setting for vision
- public static final String VISION_CAMERA_HOST = "10.9.72.12";
+ // constants:
+
+ public static final double GRAVITY_ACCELERATION = 9.8;
+ public static final double ROBOT_VOLTAGE = 12.0;
+ public static final double LOOP_TIME = 0.02;
+
+ // CAN bus names
+ public static final CANBus CANIVORE_CAN = new CANBus("CANivore");
+ public static final CANBus CANIVORE_SUB = new CANBus("CANivoreSub");
+ public static final CANBus RIO_CAN = new CANBus("rio");
+
+ // Logging
+ public static final boolean USE_TELEMETRY = true;
+
+ // this would disable all logger calls
+ public static final boolean DISABLE_LOGGING = true;
+ public static final boolean DISABLE_SMART_DASHBOARD = true; // wont disable auto picker
+
+ public static enum Mode {
+ /** Running on a real robot. */
+ REAL,
+
+ /** Running a physics simulator. */
+ SIM,
+
+ /** Replaying from a log file. */
+ REPLAY
+ }
+
+ // Kraken Speed
+ public static double MAX_RPM = 5800.0; // Rotations per minute
+
+ /*
+ * Talon Stator / Supply Limits explanation
+ * Supply current is current that's being drawn at the input bus voltage. Stator
+ * current is current that's being drawn by the motor.
+ * Supply limiting (supported by Talon FX and SRX) is useful for preventing
+ * breakers from tripping in the PDP.
+ * Stator limiting (supported by Talon FX) is useful for limiting
+ * acceleration/heat.
+ */
+
+ // These are the default values
+
+ // Stator
+ public static final boolean TALONFX_STATOR_LIMIT_ENABLE = false; // enabled?
+ public static final double TALONFX_STATOR_CURRENT_LIMIT = 100; // Limit(amp)
+ public static final double TALONFX_STATOR_TRIGGER_THRESHOLD = 100; // Trigger Threshold(amp)
+ public static final double TALONFX_STATOR_TRIGGER_DURATION = 0; // Trigger Threshold Time(s)
+
+ // Supply
+ public static final boolean TALONFX_SUPPLY_LIMIT_ENABLE = false; // enabled?
+ public static final double TALONFX_SUPPLY_CURRENT_LIMIT =
+ 40; // Limit(amp), current to hold after trigger hit
+ public static final double TALONFX_SUPPLY_TRIGGER_THRESHOLD =
+ 55; // (amp), amps to activate trigger
+ public static final double TALONFX_SUPPLY_TRIGGER_DURATION =
+ 3; // (s), how long after trigger before reducing
+
+ // OIConstants:
+
+ public static final int DRIVER_JOY = 0;
+ public static final int OPERATOR_JOY = 1;
+ public static final int TEST_JOY = 2;
+ public static final int MANUAL_JOY = 3;
+ public static final double DEFAULT_DEADBAND = 0.00005;
+
+ public static final double TRANSLATIONAL_DEADBAND = 0.01;
+
+ public static final double ROTATION_DEADBAND = 0.01;
+
+ public static final double HEADING_DEADBAND = 0.05;
+ public static final double HEADING_SLEWRATE = 10;
+
+ // Modes
+ public static final Mode SIM_MODE = Mode.SIM;
+ public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE;
+
+ // Enables 3D logs of mechanisms
+ public static final boolean LOG_MECHANISMS = true;
+
+ // Network setting for vision
+ public static final String VISION_CAMERA_HOST = "10.9.72.12";
}
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import frc.robot.Robot;
-import frc.robot.constants.swerve.DriveConstants;
public class FieldConstants {
/** Apriltag layout for 2026 REBUILT */
- public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+ public static final AprilTagFieldLayout field =
+ AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
/** Width of the field [meters] */
public static final double FIELD_LENGTH = field.getFieldLength();
+
/** Height of the field [meters] */
public static final double FIELD_WIDTH = field.getFieldWidth();
-
}
package frc.robot.constants;
-/**
- * constants for gyro bias estimation and correction via vision.
- */
+/** constants for gyro bias estimation and correction via vision. */
public class GyroBiasConstants {
- /** minimum samples before applying correction */
- public static final int MIN_SAMPLES = 10;
+ /** minimum samples before applying correction */
+ public static final int MIN_SAMPLES = 10;
- /** maximum angle difference to accept in radians */
- public static final double MAX_ANGLE_DIFF_RAD = Math.toRadians(45);
+ /** maximum angle difference to accept in radians */
+ public static final double MAX_ANGLE_DIFF_RAD = Math.toRadians(45);
- /** minimum correction to apply in radians */
- public static final double MIN_CORRECTION_RAD = Math.toRadians(0.1);
+ /** minimum correction to apply in radians */
+ public static final double MIN_CORRECTION_RAD = Math.toRadians(0.1);
- /** fraction of the correction to apply (0.0 to 1.0) */
- public static final double CORRECTION_FRACTION = 0.2;
+ /** fraction of the correction to apply (0.0 to 1.0) */
+ public static final double CORRECTION_FRACTION = 0.2;
- /** maximum correction per cycle in radians */
- public static final double MAX_CORRECTION_PER_CYCLE_RAD = Math.toRadians(5);
+ /** maximum correction per cycle in radians */
+ public static final double MAX_CORRECTION_PER_CYCLE_RAD = Math.toRadians(5);
- /** alpha for exponential moving average 0.0 to 1.0, higher is more responsive */
- public static final double EMA_ALPHA = 0.3;
+ /** alpha for exponential moving average 0.0 to 1.0, higher is more responsive */
+ public static final double EMA_ALPHA = 0.3;
- /** min total weight required for weighted average */
- public static final double MIN_TOTAL_WEIGHT = 3.0;
+ /** min total weight required for weighted average */
+ public static final double MIN_TOTAL_WEIGHT = 3.0;
}
package frc.robot.constants;
public class IdConstants {
- // Drivetrain
- public static final int DRIVE_FRONT_LEFT_ID = 1;
- public static final int STEER_FRONT_LEFT_ID = 2;
- public static final int ENCODER_FRONT_LEFT_ID = 3;
- public static final int DRIVE_FRONT_RIGHT_ID = 10;
- public static final int STEER_FRONT_RIGHT_ID = 11;
- public static final int ENCODER_FRONT_RIGHT_ID = 12;
- public static final int DRIVE_BACK_LEFT_ID = 7;
- public static final int STEER_BACK_LEFT_ID = 8;
- public static final int ENCODER_BACK_LEFT_ID = 9;
- public static final int DRIVE_BACK_RIGHT_ID = 4;
- public static final int STEER_BACK_RIGHT_ID = 5;
- public static final int ENCODER_BACK_RIGHT_ID = 6;
- public static final int PIGEON = 13;
+ // Drivetrain
+ public static final int DRIVE_FRONT_LEFT_ID = 1;
+ public static final int STEER_FRONT_LEFT_ID = 2;
+ public static final int ENCODER_FRONT_LEFT_ID = 3;
+ public static final int DRIVE_FRONT_RIGHT_ID = 10;
+ public static final int STEER_FRONT_RIGHT_ID = 11;
+ public static final int ENCODER_FRONT_RIGHT_ID = 12;
+ public static final int DRIVE_BACK_LEFT_ID = 7;
+ public static final int STEER_BACK_LEFT_ID = 8;
+ public static final int ENCODER_BACK_LEFT_ID = 9;
+ public static final int DRIVE_BACK_RIGHT_ID = 4;
+ public static final int STEER_BACK_RIGHT_ID = 5;
+ public static final int ENCODER_BACK_RIGHT_ID = 6;
+ public static final int PIGEON = 13;
- // LEDs
- public static final int CANDLE_ID = 1;
+ // LEDs
+ public static final int CANDLE_ID = 1;
- // Turret
- public static final int TURRET_MOTOR_ID = 5;
- public static final int TURRET_ENCODER_LEFT_ID = 6;
- public static final int TURRET_ENCODER_RIGHT_ID = 7;
+ // Turret
+ public static final int TURRET_MOTOR_ID = 5;
+ public static final int TURRET_ENCODER_LEFT_ID = 6;
+ public static final int TURRET_ENCODER_RIGHT_ID = 7;
- // Shooter
- public static final int SHOOTER_LEFT_ID = 9;
- public static final int SHOOTER_RIGHT_ID = 10;
+ // Shooter
+ public static final int SHOOTER_LEFT_ID = 9;
+ public static final int SHOOTER_RIGHT_ID = 10;
- // Hood
- public static final int HOOD_ID = 11;
+ // Hood
+ public static final int HOOD_ID = 11;
- // Spindexer
- public static final int SPINDEXER_ONE_ID = 4;
- public static final int SPINDEXER_TWO_ID = 8;
+ // Spindexer
+ public static final int SPINDEXER_ONE_ID = 4;
+ public static final int SPINDEXER_TWO_ID = 8;
- // Intake
- public static final int RIGHT_MOTOR_ID = 1;
- public static final int LEFT_MOTOR_ID = 2;
- public static final int ROLLER_MOTOR_ID = 3;
+ // Intake
+ public static final int RIGHT_MOTOR_ID = 1;
+ public static final int LEFT_MOTOR_ID = 2;
+ public static final int ROLLER_MOTOR_ID = 3;
}
package frc.robot.constants;
-/**
- * Container class for test constants.
- */
+/** Container class for test constants. */
public class TestConstants {
- public static final double POSE_TRANSFORM_TRANSLATION_ERROR = 0.6;
-}
\ No newline at end of file
+ public static final double POSE_TRANSFORM_TRANSLATION_ERROR = 0.6;
+}
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
-/**
- * Container class for vision constants.
- */
+/** Container class for vision constants. */
public class VisionConstants {
- /**
- * If April tag vision is enabled on the robot
- */
- public static final boolean ENABLED = true;
-
- /**
- * If object detection should be enabled
- */
- public static final boolean OBJECT_DETECTION_ENABLED = false;
-
- /** If odometry should be updated using vision during auto */
- public static final boolean ENABLED_AUTO = true;
-
- /**
- * If odometry should be updated using vision while running the GoToPose,
- * GoToPosePID, and DriveToPose commands in teleop
- */
- public static final boolean ENABLED_GO_TO_POSE = true;
-
- /** If vision should be simulated */
- public static final boolean ENABLED_SIM = false;
-
- /** If vision should only return values if it can see 2 good targets */
- public static final boolean ONLY_USE_2_TAGS = false;
-
- /** PoseStrategy to use in pose estimation */
- public static final PoseStrategy POSE_STRATEGY = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
-
- /** Fallback PoseStrategy if MultiTag doesn't work */
- public static final PoseStrategy MULTITAG_FALLBACK_STRATEGY = PoseStrategy.LOWEST_AMBIGUITY;
-
- /**
- * Any April tags we always want to ignore. To ignore a tag, put its id in this
- * array.
- */
- public static final int[] TAGS_TO_IGNORE = {};
-
- /**
- * If multiple cameras return different poses, they will be ignored if the
- * difference between them is greater than this
- */
- public static final double MAX_POSE_DIFFERENCE = 0.2;
-
- /**
- * The maximum distance to the tag to use
- */
- public static final double MAX_DISTANCE = 6;
-
- /** If vision should use manual calculations (yawFunction-based vs referencePose-based). Changed to false to support gyro bias correction. */
- public static final boolean USE_MANUAL_CALCULATIONS = false;
-
- // <ol start="0"> did not work
- /**
- * Which version of driver assist to use. This would be an enum, except there is
- * no short and descriptive name for all of these.
- * <p>
- * The options are:
- * <p>
- * 0: Disable driver assist
- * <p>
- * 1: Completely remove the component of the driver's input that is not toward
- * the object
- * <p>
- * 2: Interpolate between the next achievable driver speed and a speed
- * calculated using trapezoid profiles
- * <p>
- * 3-5: Add a speed perpendicular to the driver input; there are 3 similar but
- * different calculations for this
- */
- public static final int DRIVER_ASSIST_MODE = 5;
-
- /**
- * The number to multiply the distance to the April tag by.
- * <p>
- * Only affects manual calculations.
- * <p>
- * To find this, set it to 1 and measure the actual distance and the calculated
- * distance.
- * <p>
- * This should not be needed, and it is only here because it improved the
- * accuracy of vision in the 2023 fall semester
- */
- public static final double DISTANCE_SCALE = 1;
-
- /**
- * The standard deviations to use for vision
- */
- public static final Matrix<N3, N1> VISION_STD_DEVS = VecBuilder.fill(
- 0.3, // x in meters (default=0.9)
- 0.3, // y in meters (default=0.9)
- 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
- // correctly it is unnecessary to correct it with vision
- );
-
- /**
- * The standard deviations to use for vision when the wheels slip
- */
- public static final Matrix<N3, N1> VISION_STD_DEVS_2 = VecBuilder.fill(
- 0.01, // x in meters (default=0.9)
- 0.01, // y in meters (default=0.9)
- 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
- // correctly it is unnecessary to correct it with vision
- );
-
- /**
- * The highest ambiguity to use. Ambiguities higher than this will be ignored.
- * <p>
- * Only affects calculations using PhotonVision, not manual calculations.
- */
- public static final double HIGHEST_AMBIGUITY = 0.05;
-
- public static final int MAX_EMPTY_TICKS = 10;
-
- /**
- * The camera poses
- * <p>
- * Everything is in meters and radians
- * <p>
- * 0 for all numbers is center of the robot, on the ground, looking straight
- * toward the front
- * <p>
- * + X: Front of Robot
- * <p>
- * + Y: Left of Robot
- * <p>
- * + Z: Top of Robot
- * <p>
- * + Pitch: Down
- * <p>
- * + Yaw: Counterclockwise
- */
- public static final ArrayList<Pair<String, Transform3d>> APRIL_TAG_CAMERAS = new ArrayList<Pair<String, Transform3d>>(
- List.of(
- new Pair<String, Transform3d>(
- "CameraFrontLeft",
- new Transform3d(
- new Translation3d(Units.inchesToMeters(-8.47),
- Units.inchesToMeters(11.54),
- Units.inchesToMeters(17.7)),
- new Rotation3d(0, Units.degreesToRadians(-22.0),
- Units.degreesToRadians(55.0)))),
- new Pair<String, Transform3d>(
- "CameraFrontRight",
- new Transform3d(
- new Translation3d(Units.inchesToMeters(-8.47),
- Units.inchesToMeters(-11.54),
- Units.inchesToMeters(17.7)),
- new Rotation3d(0, Units.degreesToRadians(-22.0),
- Units.degreesToRadians(-55.0)))),
- new Pair<String, Transform3d>(
- "CameraBackLeft",
- new Transform3d(
- new Translation3d(Units.inchesToMeters(-10.91),
- Units.inchesToMeters(12),
- Units.inchesToMeters(17.66)),
- new Rotation3d(0, Units.degreesToRadians(-22.0),
- Units.degreesToRadians(145.0)))),
- new Pair<String, Transform3d>(
- "CameraBackRight",
- new Transform3d(
- new Translation3d(Units.inchesToMeters(-10.91),
- Units.inchesToMeters(-12),
- Units.inchesToMeters(17.66)),
- new Rotation3d(0, Units.degreesToRadians(-22.0),
- Units.degreesToRadians(-145.0))))
- ));
-
- /**
- * The transformations from the robot to object detection cameras
- */
- public static final ArrayList<Transform3d> OBJECT_DETECTION_CAMERAS = new ArrayList<>(List.of(
- new Transform3d(
- new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)),
- new Rotation3d(0, Units.degreesToRadians(20), 0))));
-
- // used to cleanly shutdown the OrangePi
- public static final String[] ORANGEPI_HOSTNAMES = { "photonfront.local", "photonback.local" };
- public static final String ORANGEPI_USERNAME = "pi";
- public static final String ORANGEPI_PASSWORD = "raspberry";
+ /** If April tag vision is enabled on the robot */
+ public static final boolean ENABLED = true;
+
+ /** If object detection should be enabled */
+ public static final boolean OBJECT_DETECTION_ENABLED = false;
+
+ /** If odometry should be updated using vision during auto */
+ public static final boolean ENABLED_AUTO = true;
+
+ /**
+ * If odometry should be updated using vision while running the GoToPose, GoToPosePID, and
+ * DriveToPose commands in teleop
+ */
+ public static final boolean ENABLED_GO_TO_POSE = true;
+
+ /** If vision should be simulated */
+ public static final boolean ENABLED_SIM = false;
+
+ /** If vision should only return values if it can see 2 good targets */
+ public static final boolean ONLY_USE_2_TAGS = false;
+
+ /** PoseStrategy to use in pose estimation */
+ public static final PoseStrategy POSE_STRATEGY = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
+
+ /** Fallback PoseStrategy if MultiTag doesn't work */
+ public static final PoseStrategy MULTITAG_FALLBACK_STRATEGY = PoseStrategy.LOWEST_AMBIGUITY;
+
+ /** Any April tags we always want to ignore. To ignore a tag, put its id in this array. */
+ public static final int[] TAGS_TO_IGNORE = {};
+
+ /**
+ * If multiple cameras return different poses, they will be ignored if the difference between them
+ * is greater than this
+ */
+ public static final double MAX_POSE_DIFFERENCE = 0.2;
+
+ /** The maximum distance to the tag to use */
+ public static final double MAX_DISTANCE = 6;
+
+ /**
+ * If vision should use manual calculations (yawFunction-based vs referencePose-based). Changed to
+ * false to support gyro bias correction.
+ */
+ public static final boolean USE_MANUAL_CALCULATIONS = false;
+
+ // <ol start="0"> did not work
+ /**
+ * Which version of driver assist to use. This would be an enum, except there is no short and
+ * descriptive name for all of these.
+ *
+ * <p>The options are:
+ *
+ * <p>0: Disable driver assist
+ *
+ * <p>1: Completely remove the component of the driver's input that is not toward the object
+ *
+ * <p>2: Interpolate between the next achievable driver speed and a speed calculated using
+ * trapezoid profiles
+ *
+ * <p>3-5: Add a speed perpendicular to the driver input; there are 3 similar but different
+ * calculations for this
+ */
+ public static final int DRIVER_ASSIST_MODE = 5;
+
+ /**
+ * The number to multiply the distance to the April tag by.
+ *
+ * <p>Only affects manual calculations.
+ *
+ * <p>To find this, set it to 1 and measure the actual distance and the calculated distance.
+ *
+ * <p>This should not be needed, and it is only here because it improved the accuracy of vision in
+ * the 2023 fall semester
+ */
+ public static final double DISTANCE_SCALE = 1;
+
+ /** The standard deviations to use for vision */
+ public static final Matrix<N3, N1> VISION_STD_DEVS =
+ VecBuilder.fill(
+ 0.3, // x in meters (default=0.9)
+ 0.3, // y in meters (default=0.9)
+ 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
+ // correctly it is unnecessary to correct it with vision
+ );
+
+ /** The standard deviations to use for vision when the wheels slip */
+ public static final Matrix<N3, N1> VISION_STD_DEVS_2 =
+ VecBuilder.fill(
+ 0.01, // x in meters (default=0.9)
+ 0.01, // y in meters (default=0.9)
+ 0.9 // heading in radians. The gyroscope is very accurate, so as long as it is reset
+ // correctly it is unnecessary to correct it with vision
+ );
+
+ /**
+ * The highest ambiguity to use. Ambiguities higher than this will be ignored.
+ *
+ * <p>Only affects calculations using PhotonVision, not manual calculations.
+ */
+ public static final double HIGHEST_AMBIGUITY = 0.05;
+
+ public static final int MAX_EMPTY_TICKS = 10;
+
+ /**
+ * The camera poses
+ *
+ * <p>Everything is in meters and radians
+ *
+ * <p>0 for all numbers is center of the robot, on the ground, looking straight toward the front
+ *
+ * <p>+ X: Front of Robot
+ *
+ * <p>+ Y: Left of Robot
+ *
+ * <p>+ Z: Top of Robot
+ *
+ * <p>+ Pitch: Down
+ *
+ * <p>+ Yaw: Counterclockwise
+ */
+ public static final ArrayList<Pair<String, Transform3d>> APRIL_TAG_CAMERAS =
+ new ArrayList<Pair<String, Transform3d>>(
+ List.of(
+ new Pair<String, Transform3d>(
+ "CameraFrontLeft",
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-8.47),
+ Units.inchesToMeters(11.54),
+ Units.inchesToMeters(17.7)),
+ new Rotation3d(
+ 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(55.0)))),
+ new Pair<String, Transform3d>(
+ "CameraFrontRight",
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-8.47),
+ Units.inchesToMeters(-11.54),
+ Units.inchesToMeters(17.7)),
+ new Rotation3d(
+ 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-55.0)))),
+ new Pair<String, Transform3d>(
+ "CameraBackLeft",
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-10.91),
+ Units.inchesToMeters(12),
+ Units.inchesToMeters(17.66)),
+ new Rotation3d(
+ 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(145.0)))),
+ new Pair<String, Transform3d>(
+ "CameraBackRight",
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-10.91),
+ Units.inchesToMeters(-12),
+ Units.inchesToMeters(17.66)),
+ new Rotation3d(
+ 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-145.0))))));
+
+ /** The transformations from the robot to object detection cameras */
+ public static final ArrayList<Transform3d> OBJECT_DETECTION_CAMERAS =
+ new ArrayList<>(
+ List.of(
+ new Transform3d(
+ new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)),
+ new Rotation3d(0, Units.degreesToRadians(20), 0))));
+
+ // used to cleanly shutdown the OrangePi
+ public static final String[] ORANGEPI_HOSTNAMES = {"photonfront.local", "photonback.local"};
+ public static final String ORANGEPI_USERNAME = "pi";
+ public static final String ORANGEPI_PASSWORD = "raspberry";
}
import lib.COTSFalconSwerveConstants;
/**
- * Global constants are, by default, for the competition robot.
- * Global constants get changed in the update method if the RobotId detected is not the competition robot.
+ * Global constants are, by default, for the competition robot. Global constants get changed in the
+ * update method if the RobotId detected is not the competition robot.
*/
public class DriveConstants {
- /**
- * The robot's width with its bumpers on.
- * <p>
- * The frame width is 26.5 inches, and each bumper is 3.25 inches.
- */
- public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.83185; // 32.75 inches in meters
-
- public static double ROBOT_MASS = Units.lbsToKilograms(111.6 + 13 + 13.4 + 5.0);
-
- /** Radius of the drive wheels [meters]. */
- public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95);
-
- public static double WHEEL_MOI = 0.000326 * ROBOT_MASS;
-
-
- /** Distance between the left and right wheels [meters]. */
- // from center of wheels btw
- public static double TRACK_WIDTH = Units.inchesToMeters(20.75);//22.75 swerve bot, 20.75 comp bot
-
- // Mk4i gear ratios
- // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
- // standard gear ratios
- // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
- // changes 14-tooth pinion to 16-tooth pinion -- (50.0 / 14.0) becomes (50.0 / 16.0).
- /** Drive gear ratio for an Mk4i with L2-Plus gearing */
- public static double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0);
- // all MK4i modules have the same steering gear ratio
- public static double STEER_GEAR_RATIO = 150.0 / 7.0;
-
- /** Theoretical maximum speed of the robot based on maximum motor RPM, gear ratio, and wheel radius */
- public static final double MAX_SPEED = 4.5;
-
- // Need to convert tangential velocity (the m/s of the edge of the robot) to angular velocity (the radians/s of the robot)
- // To do so, divide by the radius. The radius is the diagonal of the square chassis, diagonal = sqrt(2) * side_length.
- public static final double MAX_ANGULAR_SPEED = MAX_SPEED / ((TRACK_WIDTH / 2) * Math.sqrt(2));
-
- public static final double COSF = 1.5;
-
- // The maximum acceleration of the robot, limited by friction
- public static final double MAX_LINEAR_ACCEL = COSF * Constants.GRAVITY_ACCELERATION;
- // The maximum amount a drive motor can accelerate, independant of friction
- // This does nothing if greater than LINEAR_ACCEL
- public static final double MAX_DRIVE_ACCEL = MAX_LINEAR_ACCEL;
- // The maximum angular acceleration of the robot
- public static final double MAX_ANGULAR_ACCEL = MAX_LINEAR_ACCEL / TRACK_WIDTH * Math.sqrt(2);
-
- /**
- * If this is false, Drivetrain will use the previous setpoint to calculate the new setpoint.
- * <p> If this is true, Drivetrain will use the actual current setpoint instead.
- */
- public static final boolean USE_ACTUAL_SPEED = false;
-
- /**
- * Disables the deadband and optimization for the modules.
- * SwerveSetpointGenerator adds its own optimization and deadband, and the controllers also have a deadband.
- * Setting this to true fixes bugs caused by using hte actual current state.
- */
- public static final boolean DISABLE_DEADBAND_AND_OPTIMIZATION = false;
-
- public static final Rotation2d STARTING_HEADING = new Rotation2d();
-
- public static final Translation2d[] MODULE_LOCATIONS = {
- new Translation2d(DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
- new Translation2d(DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2),
- new Translation2d(-DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
- new Translation2d(-DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2)
- };
-
- public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(MODULE_LOCATIONS);
-
- /**
- * Default values for SwerveCompetition drivetrain
- * Sets to correct value later if robotID is different
- */
- public static double STEER_OFFSET_FRONT_LEFT = 302.646;
- public static double STEER_OFFSET_FRONT_RIGHT = 103.039+180;
- public static double STEER_OFFSET_BACK_LEFT = 165.49+90;
- public static double STEER_OFFSET_BACK_RIGHT = 73.563;
-
- // Heading PID.
- public static final double HEADING_P = 5.5;
- public static final double HEADING_D = 0;
-
- public static final double HEADING_TOLERANCE = Units.degreesToRadians(1.5);
-
- // Translational PID
- // TODO: Tune this better (low priority since we aren't using it in 2025)
- public static final double TRANSLATIONAL_P = 1;
- public static final double TRANSLATIONAL_D = 0.001;
-
- //The PIDs for PathPlanner Command
- public static final double PATH_PLANNER_HEADING_P = 3.5/2;
- public static final double PATH_PLANNER_HEADING_D = 0;
-
- public static final double PATH_PLANNER_TRANSLATIONAL_P = 6/2;
- public static final double PATH_PLANNER_TRANSLATIONAL_D = 0;
-
- // CAN
- public static CANBus DRIVE_MOTOR_CAN = Constants.CANIVORE_CAN;
- public static CANBus STEER_MOTOR_CAN = Constants.CANIVORE_CAN;
- public static CANBus STEER_ENCODER_CAN = Constants.CANIVORE_CAN;
- public static CANBus PIGEON_CAN = Constants.CANIVORE_CAN;
-
-
- public static COTSFalconSwerveConstants MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
-
- /* Swerve Current Limiting */
- public static final int STEER_CONTINUOUS_CURRENT_LIMIT = 15;
- public static final int STEER_PEAK_CURRENT_LIMIT = 15;
- public static final double STEER_PEAK_CURRENT_DURATION = 0.01;
- public static final boolean STEER_ENABLE_CURRENT_LIMIT = true;
-
- public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 40;
- public static final int DRIVE_PEAK_CURRENT_LIMIT = 40;
- public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01;
- public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true;
-
- /* Motor inversions */
- public static final InvertedValue INVERT_DRIVE_MOTOR = InvertedValue.CounterClockwise_Positive;
- public static InvertedValue INVERT_STEER_MOTOR = InvertedValue.Clockwise_Positive;
-
- /* Neutral Modes */
- public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake;
- public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake;
-
- /* Gyro mount pose roll in deg (180.0 if placed under the robot) */
- public static double GYRO_MOUNT_POSE_ROLL = 0.0;
-
- /* Drive Motor PID Values */
- public static final double[] P_VALUES = {
- 0.3,
- 0.3,
- 0.3,
- 0.3
- };
- public static final double[] I_VALUES = {
- 0,
- 0,
- 0,
- 0
- };
- public static final double[] D_VALUES = {
- 0,
- 0,
- 0,
- 0
- };
- /* Drive Motor Characterization Values
- * Divide SYSID values by 12 to convert from volts to percent output for CTRE */
- public static final double[] S_VALUES = {
- 0.11,
- 0.11,
- 0.11,
- 0.11
- };
- public static final double[] V_VALUES = {
- 0.11079,
- 0.10718,
- 0.11009,
- 0.1164
- };
- public static final double[] A_VALUES = {
- 0.005482,
- 0.0049593,
- 0.010156,
- 0.0065708
- };
- /* Ramp values for drive motors in open loop driving. */
- // Open loop prevents throttle from changing too quickly.
- // It will limit it to time given (in seconds) to go from zero to full throttle.
- // A small open loop ramp (0.25) helps with tread wear, tipping, etc
- public static final double OPEN_LOOP_RAMP = 0.1;
-
- // limits maximum rate of change for motor
- public static final double CLOSE_LOOP_RAMP = 0.0;
-
- public static final double WHEEL_CIRCUMFERENCE = 2*Math.PI*WHEEL_RADIUS;
-
- public static final boolean INVERT_GYRO = false; // Make sure gyro is CCW+ CW-
-
- public static final double SLOW_DRIVE_FACTOR = 0.2;
- public static final double SLOW_ROT_FACTOR = 0.1;
-
- public static final ModuleLimits MODULE_LIMITS = new ModuleLimits(MAX_SPEED, MAX_DRIVE_ACCEL, COSF, Units.rotationsPerMinuteToRadiansPerSecond(Constants.MAX_RPM / STEER_GEAR_RATIO));
-
- /**
- * Updates the constants if the RobotId is not the default SwerveCompetition robot.
- */
- public static void update(RobotId robotId) {
- if (robotId == RobotId.PrimeJr) {
- STEER_OFFSET_FRONT_LEFT = 187.64+180; // module zero
- STEER_OFFSET_FRONT_RIGHT = 162+180+180; // module one
- STEER_OFFSET_BACK_LEFT = 196.3+180; // module two
- STEER_OFFSET_BACK_RIGHT = 357+180+180; // module three
-
- // MK5n
- INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
-
- // Gear ratios
- //DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); //R2 Ratio
- DRIVE_GEAR_RATIO = (54.0 / 12.0) * (25.0 / 32.0) * (30.0 / 15.0); //R1 Ratio
- STEER_GEAR_RATIO = 287.0 / 11.0;
-
- // Gyro is mounted under the robot
- GYRO_MOUNT_POSE_ROLL = 180.0;
-
- MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
-
- } else if(robotId == RobotId.TwinBot){
- STEER_OFFSET_FRONT_LEFT = 131.201172;
- STEER_OFFSET_FRONT_RIGHT = 247.324219;
- STEER_OFFSET_BACK_LEFT = 39.814463;
- STEER_OFFSET_BACK_RIGHT = 294.873047;
-
- // MK5n gear ratio
- INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
-
- DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0);
- STEER_GEAR_RATIO = 287.0 / 11.0;
-
- MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
-
- } else if(robotId == RobotId.SwerveCompetition){
- STEER_OFFSET_FRONT_LEFT = 302.646;
- STEER_OFFSET_FRONT_RIGHT = 103.039+180;
- STEER_OFFSET_BACK_LEFT = 165.49+90;
- STEER_OFFSET_BACK_RIGHT = 73.563;
-
- MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
-
- } else if(robotId == RobotId.BetaBot) {
- STEER_OFFSET_FRONT_LEFT = 193.884-180;
- STEER_OFFSET_FRONT_RIGHT = 110.914;
- STEER_OFFSET_BACK_LEFT = 128.054+180;
- STEER_OFFSET_BACK_RIGHT = 107.43;
-
- MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
-
- } else if (robotId == RobotId.Vivace) {
- STEER_OFFSET_FRONT_LEFT = 100.184+180;
- STEER_OFFSET_FRONT_RIGHT = 224.293;
- STEER_OFFSET_BACK_LEFT = 304.795-180;
- STEER_OFFSET_BACK_RIGHT = 201.177-180;
-
- ROBOT_MASS = 50;
- WHEEL_MOI = 0.000326 * ROBOT_MASS;
-
- MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
-
- } else if (robotId == RobotId.Vertigo) {
- STEER_OFFSET_FRONT_LEFT = Units.radiansToDegrees(3.43);
- STEER_OFFSET_FRONT_RIGHT = Units.radiansToDegrees(1.91) + 180;
- STEER_OFFSET_BACK_LEFT = Units.radiansToDegrees(2.28);
- STEER_OFFSET_BACK_RIGHT = Units.radiansToDegrees(5.03);
-
- DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
-
- ROBOT_MASS = 20;
-
- WHEEL_MOI = 0.000326 * ROBOT_MASS;
-
- // Falcon Speed
- Constants.MAX_RPM = 6080.0;
-
- MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
-
- } else if (robotId == RobotId.Phil) {
- ROBOT_MASS = 30;
- WHEEL_MOI = 0.000326 * ROBOT_MASS;
-
- STEER_OFFSET_FRONT_LEFT = 121.463+180;
- STEER_OFFSET_FRONT_RIGHT = 284.242;
- STEER_OFFSET_BACK_LEFT = 157.676;
- STEER_OFFSET_BACK_RIGHT = 77.199;
-
- DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
-
- MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
-
- } else{
- MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
-
- }
- }
+ /**
+ * The robot's width with its bumpers on.
+ *
+ * <p>The frame width is 26.5 inches, and each bumper is 3.25 inches.
+ */
+ public static final double ROBOT_WIDTH_WITH_BUMPERS = 0.83185; // 32.75 inches in meters
+
+ public static double ROBOT_MASS = Units.lbsToKilograms(111.6 + 13 + 13.4 + 5.0);
+
+ /** Radius of the drive wheels [meters]. */
+ public static final double WHEEL_RADIUS = Units.inchesToMeters(1.95);
+
+ public static double WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ /** Distance between the left and right wheels [meters]. */
+ // from center of wheels btw
+ public static double TRACK_WIDTH =
+ Units.inchesToMeters(20.75); // 22.75 swerve bot, 20.75 comp bot
+
+ // Mk4i gear ratios
+ // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+ // standard gear ratios
+ // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
+ // changes 14-tooth pinion to 16-tooth pinion -- (50.0 / 14.0) becomes (50.0 / 16.0).
+ /** Drive gear ratio for an Mk4i with L2-Plus gearing */
+ public static double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+ // all MK4i modules have the same steering gear ratio
+ public static double STEER_GEAR_RATIO = 150.0 / 7.0;
+
+ /**
+ * Theoretical maximum speed of the robot based on maximum motor RPM, gear ratio, and wheel radius
+ */
+ public static final double MAX_SPEED = 4.5;
+
+ // Need to convert tangential velocity (the m/s of the edge of the robot) to angular velocity (the
+ // radians/s of the robot)
+ // To do so, divide by the radius. The radius is the diagonal of the square chassis, diagonal =
+ // sqrt(2) * side_length.
+ public static final double MAX_ANGULAR_SPEED = MAX_SPEED / ((TRACK_WIDTH / 2) * Math.sqrt(2));
+
+ public static final double COSF = 1.5;
+
+ // The maximum acceleration of the robot, limited by friction
+ public static final double MAX_LINEAR_ACCEL = COSF * Constants.GRAVITY_ACCELERATION;
+ // The maximum amount a drive motor can accelerate, independant of friction
+ // This does nothing if greater than LINEAR_ACCEL
+ public static final double MAX_DRIVE_ACCEL = MAX_LINEAR_ACCEL;
+ // The maximum angular acceleration of the robot
+ public static final double MAX_ANGULAR_ACCEL = MAX_LINEAR_ACCEL / TRACK_WIDTH * Math.sqrt(2);
+
+ /**
+ * If this is false, Drivetrain will use the previous setpoint to calculate the new setpoint.
+ *
+ * <p>If this is true, Drivetrain will use the actual current setpoint instead.
+ */
+ public static final boolean USE_ACTUAL_SPEED = false;
+
+ /**
+ * Disables the deadband and optimization for the modules. SwerveSetpointGenerator adds its own
+ * optimization and deadband, and the controllers also have a deadband. Setting this to true fixes
+ * bugs caused by using hte actual current state.
+ */
+ public static final boolean DISABLE_DEADBAND_AND_OPTIMIZATION = false;
+
+ public static final Rotation2d STARTING_HEADING = new Rotation2d();
+
+ public static final Translation2d[] MODULE_LOCATIONS = {
+ new Translation2d(DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(-DriveConstants.TRACK_WIDTH / 2, DriveConstants.TRACK_WIDTH / 2),
+ new Translation2d(-DriveConstants.TRACK_WIDTH / 2, -DriveConstants.TRACK_WIDTH / 2)
+ };
+
+ public static final SwerveDriveKinematics KINEMATICS =
+ new SwerveDriveKinematics(MODULE_LOCATIONS);
+
+ /**
+ * Default values for SwerveCompetition drivetrain Sets to correct value later if robotID is
+ * different
+ */
+ public static double STEER_OFFSET_FRONT_LEFT = 302.646;
+
+ public static double STEER_OFFSET_FRONT_RIGHT = 103.039 + 180;
+ public static double STEER_OFFSET_BACK_LEFT = 165.49 + 90;
+ public static double STEER_OFFSET_BACK_RIGHT = 73.563;
+
+ // Heading PID.
+ public static final double HEADING_P = 5.5;
+ public static final double HEADING_D = 0;
+
+ public static final double HEADING_TOLERANCE = Units.degreesToRadians(1.5);
+
+ // Translational PID
+ // TODO: Tune this better (low priority since we aren't using it in 2025)
+ public static final double TRANSLATIONAL_P = 1;
+ public static final double TRANSLATIONAL_D = 0.001;
+
+ // The PIDs for PathPlanner Command
+ public static final double PATH_PLANNER_HEADING_P = 3.5 / 2;
+ public static final double PATH_PLANNER_HEADING_D = 0;
+
+ public static final double PATH_PLANNER_TRANSLATIONAL_P = 6 / 2;
+ public static final double PATH_PLANNER_TRANSLATIONAL_D = 0;
+
+ // CAN
+ public static CANBus DRIVE_MOTOR_CAN = Constants.CANIVORE_CAN;
+ public static CANBus STEER_MOTOR_CAN = Constants.CANIVORE_CAN;
+ public static CANBus STEER_ENCODER_CAN = Constants.CANIVORE_CAN;
+ public static CANBus PIGEON_CAN = Constants.CANIVORE_CAN;
+
+ public static COTSFalconSwerveConstants MODULE_CONSTANTS =
+ COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ /* Swerve Current Limiting */
+ public static final int STEER_CONTINUOUS_CURRENT_LIMIT = 15;
+ public static final int STEER_PEAK_CURRENT_LIMIT = 15;
+ public static final double STEER_PEAK_CURRENT_DURATION = 0.01;
+ public static final boolean STEER_ENABLE_CURRENT_LIMIT = true;
+
+ public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 40;
+ public static final int DRIVE_PEAK_CURRENT_LIMIT = 40;
+ public static final double DRIVE_PEAK_CURRENT_DURATION = 0.01;
+ public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true;
+
+ /* Motor inversions */
+ public static final InvertedValue INVERT_DRIVE_MOTOR = InvertedValue.CounterClockwise_Positive;
+ public static InvertedValue INVERT_STEER_MOTOR = InvertedValue.Clockwise_Positive;
+
+ /* Neutral Modes */
+ public static final NeutralModeValue DRIVE_NEUTRAL_MODE = NeutralModeValue.Brake;
+ public static final NeutralModeValue STEER_NEUTRAL_MODE = NeutralModeValue.Brake;
+
+ /* Gyro mount pose roll in deg (180.0 if placed under the robot) */
+ public static double GYRO_MOUNT_POSE_ROLL = 0.0;
+
+ /* Drive Motor PID Values */
+ public static final double[] P_VALUES = {0.3, 0.3, 0.3, 0.3};
+ public static final double[] I_VALUES = {0, 0, 0, 0};
+ public static final double[] D_VALUES = {0, 0, 0, 0};
+ /* Drive Motor Characterization Values
+ * Divide SYSID values by 12 to convert from volts to percent output for CTRE */
+ public static final double[] S_VALUES = {0.11, 0.11, 0.11, 0.11};
+ public static final double[] V_VALUES = {0.11079, 0.10718, 0.11009, 0.1164};
+ public static final double[] A_VALUES = {0.005482, 0.0049593, 0.010156, 0.0065708};
+ /* Ramp values for drive motors in open loop driving. */
+ // Open loop prevents throttle from changing too quickly.
+ // It will limit it to time given (in seconds) to go from zero to full throttle.
+ // A small open loop ramp (0.25) helps with tread wear, tipping, etc
+ public static final double OPEN_LOOP_RAMP = 0.1;
+
+ // limits maximum rate of change for motor
+ public static final double CLOSE_LOOP_RAMP = 0.0;
+
+ public static final double WHEEL_CIRCUMFERENCE = 2 * Math.PI * WHEEL_RADIUS;
+
+ public static final boolean INVERT_GYRO = false; // Make sure gyro is CCW+ CW-
+
+ public static final double SLOW_DRIVE_FACTOR = 0.2;
+ public static final double SLOW_ROT_FACTOR = 0.1;
+
+ public static final ModuleLimits MODULE_LIMITS =
+ new ModuleLimits(
+ MAX_SPEED,
+ MAX_DRIVE_ACCEL,
+ COSF,
+ Units.rotationsPerMinuteToRadiansPerSecond(Constants.MAX_RPM / STEER_GEAR_RATIO));
+
+ /** Updates the constants if the RobotId is not the default SwerveCompetition robot. */
+ public static void update(RobotId robotId) {
+ if (robotId == RobotId.PrimeJr) {
+ STEER_OFFSET_FRONT_LEFT = 187.64 + 180; // module zero
+ STEER_OFFSET_FRONT_RIGHT = 162 + 180 + 180; // module one
+ STEER_OFFSET_BACK_LEFT = 196.3 + 180; // module two
+ STEER_OFFSET_BACK_RIGHT = 357 + 180 + 180; // module three
+
+ // MK5n
+ INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
+
+ // Gear ratios
+ // DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); //R2 Ratio
+ DRIVE_GEAR_RATIO = (54.0 / 12.0) * (25.0 / 32.0) * (30.0 / 15.0); // R1 Ratio
+ STEER_GEAR_RATIO = 287.0 / 11.0;
+
+ // Gyro is mounted under the robot
+ GYRO_MOUNT_POSE_ROLL = 180.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.TwinBot) {
+ STEER_OFFSET_FRONT_LEFT = 131.201172;
+ STEER_OFFSET_FRONT_RIGHT = 247.324219;
+ STEER_OFFSET_BACK_LEFT = 39.814463;
+ STEER_OFFSET_BACK_RIGHT = 294.873047;
+
+ // MK5n gear ratio
+ INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive;
+
+ DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0);
+ STEER_GEAR_RATIO = 287.0 / 11.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK5n(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.SwerveCompetition) {
+ STEER_OFFSET_FRONT_LEFT = 302.646;
+ STEER_OFFSET_FRONT_RIGHT = 103.039 + 180;
+ STEER_OFFSET_BACK_LEFT = 165.49 + 90;
+ STEER_OFFSET_BACK_RIGHT = 73.563;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.BetaBot) {
+ STEER_OFFSET_FRONT_LEFT = 193.884 - 180;
+ STEER_OFFSET_FRONT_RIGHT = 110.914;
+ STEER_OFFSET_BACK_LEFT = 128.054 + 180;
+ STEER_OFFSET_BACK_RIGHT = 107.43;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Vivace) {
+ STEER_OFFSET_FRONT_LEFT = 100.184 + 180;
+ STEER_OFFSET_FRONT_RIGHT = 224.293;
+ STEER_OFFSET_BACK_LEFT = 304.795 - 180;
+ STEER_OFFSET_BACK_RIGHT = 201.177 - 180;
+
+ ROBOT_MASS = 50;
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Vertigo) {
+ STEER_OFFSET_FRONT_LEFT = Units.radiansToDegrees(3.43);
+ STEER_OFFSET_FRONT_RIGHT = Units.radiansToDegrees(1.91) + 180;
+ STEER_OFFSET_BACK_LEFT = Units.radiansToDegrees(2.28);
+ STEER_OFFSET_BACK_RIGHT = Units.radiansToDegrees(5.03);
+
+ DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+ ROBOT_MASS = 20;
+
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ // Falcon Speed
+ Constants.MAX_RPM = 6080.0;
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else if (robotId == RobotId.Phil) {
+ ROBOT_MASS = 30;
+ WHEEL_MOI = 0.000326 * ROBOT_MASS;
+
+ STEER_OFFSET_FRONT_LEFT = 121.463 + 180;
+ STEER_OFFSET_FRONT_RIGHT = 284.242;
+ STEER_OFFSET_BACK_LEFT = 157.676;
+ STEER_OFFSET_BACK_RIGHT = 77.199;
+
+ DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+
+ } else {
+ MODULE_CONSTANTS = COTSFalconSwerveConstants.SDSMK4i(DRIVE_GEAR_RATIO);
+ }
+ }
}
import frc.robot.constants.IdConstants;
/**
- * Container class for module constants, defined using constants from {@link DriveConstants}
- * .
+ * Container class for module constants, defined using constants from {@link DriveConstants} .
*
* @see DriveConstants
*/
public enum ModuleConstants {
+ FRONT_LEFT(
+ IdConstants.DRIVE_FRONT_LEFT_ID,
+ IdConstants.STEER_FRONT_LEFT_ID,
+ IdConstants.ENCODER_FRONT_LEFT_ID,
+ DriveConstants.STEER_OFFSET_FRONT_LEFT,
+ ModuleType.FRONT_LEFT,
+ DriveConstants.S_VALUES[0],
+ DriveConstants.V_VALUES[0],
+ DriveConstants.A_VALUES[0],
+ DriveConstants.P_VALUES[0],
+ DriveConstants.I_VALUES[0],
+ DriveConstants.D_VALUES[0]),
+ FRONT_RIGHT(
+ IdConstants.DRIVE_FRONT_RIGHT_ID,
+ IdConstants.STEER_FRONT_RIGHT_ID,
+ IdConstants.ENCODER_FRONT_RIGHT_ID,
+ DriveConstants.STEER_OFFSET_FRONT_RIGHT,
+ ModuleType.FRONT_RIGHT,
+ DriveConstants.S_VALUES[1],
+ DriveConstants.V_VALUES[1],
+ DriveConstants.A_VALUES[1],
+ DriveConstants.P_VALUES[1],
+ DriveConstants.I_VALUES[1],
+ DriveConstants.D_VALUES[1]),
+ BACK_LEFT(
+ IdConstants.DRIVE_BACK_LEFT_ID,
+ IdConstants.STEER_BACK_LEFT_ID,
+ IdConstants.ENCODER_BACK_LEFT_ID,
+ DriveConstants.STEER_OFFSET_BACK_LEFT,
+ ModuleType.BACK_LEFT,
+ DriveConstants.S_VALUES[2],
+ DriveConstants.V_VALUES[2],
+ DriveConstants.A_VALUES[2],
+ DriveConstants.P_VALUES[2],
+ DriveConstants.I_VALUES[2],
+ DriveConstants.D_VALUES[2]),
+ BACK_RIGHT(
+ IdConstants.DRIVE_BACK_RIGHT_ID,
+ IdConstants.STEER_BACK_RIGHT_ID,
+ IdConstants.ENCODER_BACK_RIGHT_ID,
+ DriveConstants.STEER_OFFSET_BACK_RIGHT,
+ ModuleType.BACK_RIGHT,
+ DriveConstants.S_VALUES[3],
+ DriveConstants.V_VALUES[3],
+ DriveConstants.A_VALUES[3],
+ DriveConstants.P_VALUES[3],
+ DriveConstants.I_VALUES[3],
+ DriveConstants.D_VALUES[3]),
- FRONT_LEFT(
- IdConstants.DRIVE_FRONT_LEFT_ID,
- IdConstants.STEER_FRONT_LEFT_ID,
- IdConstants.ENCODER_FRONT_LEFT_ID,
- DriveConstants.STEER_OFFSET_FRONT_LEFT,
- ModuleType.FRONT_LEFT,
- DriveConstants.S_VALUES[0],
- DriveConstants.V_VALUES[0],
- DriveConstants.A_VALUES[0],
- DriveConstants.P_VALUES[0],
- DriveConstants.I_VALUES[0],
- DriveConstants.D_VALUES[0]
- ),
- FRONT_RIGHT(
- IdConstants.DRIVE_FRONT_RIGHT_ID,
- IdConstants.STEER_FRONT_RIGHT_ID,
- IdConstants.ENCODER_FRONT_RIGHT_ID,
- DriveConstants.STEER_OFFSET_FRONT_RIGHT,
- ModuleType.FRONT_RIGHT,
- DriveConstants.S_VALUES[1],
- DriveConstants.V_VALUES[1],
- DriveConstants.A_VALUES[1],
- DriveConstants.P_VALUES[1],
- DriveConstants.I_VALUES[1],
- DriveConstants.D_VALUES[1]
- ),
- BACK_LEFT(
- IdConstants.DRIVE_BACK_LEFT_ID,
- IdConstants.STEER_BACK_LEFT_ID,
- IdConstants.ENCODER_BACK_LEFT_ID,
- DriveConstants.STEER_OFFSET_BACK_LEFT,
- ModuleType.BACK_LEFT,
- DriveConstants.S_VALUES[2],
- DriveConstants.V_VALUES[2],
- DriveConstants.A_VALUES[2],
- DriveConstants.P_VALUES[2],
- DriveConstants.I_VALUES[2],
- DriveConstants.D_VALUES[2]
- ),
- BACK_RIGHT(
- IdConstants.DRIVE_BACK_RIGHT_ID,
- IdConstants.STEER_BACK_RIGHT_ID,
- IdConstants.ENCODER_BACK_RIGHT_ID,
- DriveConstants.STEER_OFFSET_BACK_RIGHT,
- ModuleType.BACK_RIGHT,
- DriveConstants.S_VALUES[3],
- DriveConstants.V_VALUES[3],
- DriveConstants.A_VALUES[3],
- DriveConstants.P_VALUES[3],
- DriveConstants.I_VALUES[3],
- DriveConstants.D_VALUES[3]
- ),
-
- NONE(0, 0, 0, 0.0, ModuleType.NONE,0,0,0,0,0,0);
-
- private final int drivePort;
- private final int steerPort;
- private final int encoderPort;
- private final double steerOffset;
- private final double ks;
- private final double kv;
- private final double ka;
- private final double driveP;
- private final double driveI;
- private final double driveD;
- private final ModuleType type;
-
- ModuleConstants(
- int drivePort,
- int steerPort,
- int encoderPort,
- double steerOffset,
- ModuleType type,
- double ks,
- double kv,
- double ka,
- double driveP,
- double driveI,
- double driveD
-
- ) {
- this.drivePort = drivePort;
- this.steerPort = steerPort;
- this.encoderPort = encoderPort;
- this.steerOffset = steerOffset;
- this.type = type;
- this.ks =ks;
- this.kv= kv;
- this.ka = ka;
- this.driveP =driveP;
- this.driveI = driveI;
- this.driveD = driveD;
- }
-
- public int getDrivePort() {
- return drivePort;
- }
-
- public int getSteerPort() {
- return steerPort;
- }
-
- public int getEncoderPort() {
- return encoderPort;
- }
-
- public double getSteerOffset() {
- return steerOffset;
- }
-
- public ModuleType getType() {
- return type;
- }
- public double getDriveS(){
- return ks;
- }
- public double getDriveV(){
- return kv;
- }
- public double getDriveA(){
- return ka;
- }
- public double getDriveP(){
- return driveP;
- }
- public double getDriveI(){
- return driveI;
- }
- public double getDriveD(){
- return driveD;
- }
-
-}
\ No newline at end of file
+ NONE(0, 0, 0, 0.0, ModuleType.NONE, 0, 0, 0, 0, 0, 0);
+
+ private final int drivePort;
+ private final int steerPort;
+ private final int encoderPort;
+ private final double steerOffset;
+ private final double ks;
+ private final double kv;
+ private final double ka;
+ private final double driveP;
+ private final double driveI;
+ private final double driveD;
+ private final ModuleType type;
+
+ ModuleConstants(
+ int drivePort,
+ int steerPort,
+ int encoderPort,
+ double steerOffset,
+ ModuleType type,
+ double ks,
+ double kv,
+ double ka,
+ double driveP,
+ double driveI,
+ double driveD) {
+
+ this.drivePort = drivePort;
+ this.steerPort = steerPort;
+ this.encoderPort = encoderPort;
+ this.steerOffset = steerOffset;
+ this.type = type;
+ this.ks = ks;
+ this.kv = kv;
+ this.ka = ka;
+ this.driveP = driveP;
+ this.driveI = driveI;
+ this.driveD = driveD;
+ }
+
+ public int getDrivePort() {
+ return drivePort;
+ }
+
+ public int getSteerPort() {
+ return steerPort;
+ }
+
+ public int getEncoderPort() {
+ return encoderPort;
+ }
+
+ public double getSteerOffset() {
+ return steerOffset;
+ }
+
+ public ModuleType getType() {
+ return type;
+ }
+
+ public double getDriveS() {
+ return ks;
+ }
+
+ public double getDriveV() {
+ return kv;
+ }
+
+ public double getDriveA() {
+ return ka;
+ }
+
+ public double getDriveP() {
+ return driveP;
+ }
+
+ public double getDriveI() {
+ return driveI;
+ }
+
+ public double getDriveD() {
+ return driveD;
+ }
+}
/**
* Represents the type for a module on the robot.
- * <p/>
- * IDs:
- * 0 - FRONT_LEFT
- * 1 - FRONT_RIGHT
- * 2 - BACK_LEFT
- * 3 - BACK_RIGHT
+ *
+ * <p>IDs: 0 - FRONT_LEFT 1 - FRONT_RIGHT 2 - BACK_LEFT 3 - BACK_RIGHT
*/
public enum ModuleType {
- FRONT_LEFT,
- FRONT_RIGHT,
- BACK_LEFT,
- BACK_RIGHT,
- NONE;
+ FRONT_LEFT,
+ FRONT_RIGHT,
+ BACK_LEFT,
+ BACK_RIGHT,
+ NONE;
- public final byte id;
+ public final byte id;
- ModuleType() {
- this.id = id();
- }
+ ModuleType() {
+ this.id = id();
+ }
- private byte id() {
- if (this == NONE)
- return -1;
- // This is a trick that relies on the order the enums are defined.
- return (byte) this.ordinal();
- }
-}
\ No newline at end of file
+ private byte id() {
+ if (this == NONE) return -1;
+ // This is a trick that relies on the order the enums are defined.
+ return (byte) this.ordinal();
+ }
+}
import frc.robot.util.DynamicSlewRateLimiter;
import frc.robot.util.MathUtils;
-/**
- * Abstract class for different controller types.
- */
+/** Abstract class for different controller types. */
public abstract class BaseDriverConfig {
- protected final Drivetrain drive;
-
- private double previousHeading = 0;
-
- private final DynamicSlewRateLimiter headingLimiter = new DynamicSlewRateLimiter(Constants.HEADING_SLEWRATE);
-
- /**
- * @param drive the drivetrain instance
- * @param controllerTab the shuffleboard controller tab
- * @param shuffleboardUpdates whether to update the shuffleboard
- */
- public BaseDriverConfig(Drivetrain drive) {
- headingLimiter.setContinuousLimits(-Math.PI, Math.PI);
- headingLimiter.enableContinuous(true);
- this.drive = drive;
- }
-
- public double getForwardTranslation() {
- double forward = getRawForwardTranslation();
- return forward * DriveConstants.MAX_SPEED * Math.min(1,RobotController.getBatteryVoltage()/12) * MathUtil.applyDeadband(Math.sqrt(forward*forward + Math.pow(getRawSideTranslation(), 2)), Constants.TRANSLATIONAL_DEADBAND);
- }
-
- public double getSideTranslation() {
- double side = getRawSideTranslation();
- return side * DriveConstants.MAX_SPEED * Math.min(1,RobotController.getBatteryVoltage()/12) * MathUtil.applyDeadband(Math.sqrt(side*side + Math.pow(getRawForwardTranslation(), 2)), Constants.TRANSLATIONAL_DEADBAND);
- }
-
- public double getRotation() {
- return MathUtils.expoMS(MathUtil.applyDeadband(getRawRotation(), Constants.ROTATION_DEADBAND), 2)
- * DriveConstants.MAX_ANGULAR_SPEED * Math.min(1, RobotController.getBatteryVoltage()/12);
- }
-
- public double getHeading() {
- if (getRawHeadingMagnitude() <= Constants.HEADING_DEADBAND)
- return headingLimiter.calculate(previousHeading, 1e-6);
- previousHeading = headingLimiter.calculate(getRawHeadingAngle(),
- MathUtils.expoMS(getRawHeadingMagnitude(), 2));
- return previousHeading;
- }
-
- protected Drivetrain getDrivetrain() {
- return drive;
- }
-
- /**
- * Configures the controls for the controller.
- */
- public abstract void configureControls();
-
- public abstract double getRawSideTranslation();
-
- public abstract double getRawForwardTranslation();
-
- public abstract double getRawRotation();
-
- public abstract double getRawHeadingAngle();
-
- public abstract double getRawHeadingMagnitude();
-
- public abstract boolean getIsSlowMode();
-
- public abstract boolean getIsAlign();
-
-}
\ No newline at end of file
+ protected final Drivetrain drive;
+
+ private double previousHeading = 0;
+
+ private final DynamicSlewRateLimiter headingLimiter =
+ new DynamicSlewRateLimiter(Constants.HEADING_SLEWRATE);
+
+ /**
+ * @param drive the drivetrain instance
+ * @param controllerTab the shuffleboard controller tab
+ * @param shuffleboardUpdates whether to update the shuffleboard
+ */
+ public BaseDriverConfig(Drivetrain drive) {
+ headingLimiter.setContinuousLimits(-Math.PI, Math.PI);
+ headingLimiter.enableContinuous(true);
+ this.drive = drive;
+ }
+
+ public double getForwardTranslation() {
+ double forward = getRawForwardTranslation();
+ return forward
+ * DriveConstants.MAX_SPEED
+ * Math.min(1, RobotController.getBatteryVoltage() / 12)
+ * MathUtil.applyDeadband(
+ Math.sqrt(forward * forward + Math.pow(getRawSideTranslation(), 2)),
+ Constants.TRANSLATIONAL_DEADBAND);
+ }
+
+ public double getSideTranslation() {
+ double side = getRawSideTranslation();
+ return side
+ * DriveConstants.MAX_SPEED
+ * Math.min(1, RobotController.getBatteryVoltage() / 12)
+ * MathUtil.applyDeadband(
+ Math.sqrt(side * side + Math.pow(getRawForwardTranslation(), 2)),
+ Constants.TRANSLATIONAL_DEADBAND);
+ }
+
+ public double getRotation() {
+ return MathUtils.expoMS(
+ MathUtil.applyDeadband(getRawRotation(), Constants.ROTATION_DEADBAND), 2)
+ * DriveConstants.MAX_ANGULAR_SPEED
+ * Math.min(1, RobotController.getBatteryVoltage() / 12);
+ }
+
+ public double getHeading() {
+ if (getRawHeadingMagnitude() <= Constants.HEADING_DEADBAND)
+ return headingLimiter.calculate(previousHeading, 1e-6);
+ previousHeading =
+ headingLimiter.calculate(
+ getRawHeadingAngle(), MathUtils.expoMS(getRawHeadingMagnitude(), 2));
+ return previousHeading;
+ }
+
+ protected Drivetrain getDrivetrain() {
+ return drive;
+ }
+
+ /** Configures the controls for the controller. */
+ public abstract void configureControls();
+
+ public abstract double getRawSideTranslation();
+
+ public abstract double getRawForwardTranslation();
+
+ public abstract double getRawRotation();
+
+ public abstract double getRawHeadingAngle();
+
+ public abstract double getRawHeadingMagnitude();
+
+ public abstract boolean getIsSlowMode();
+
+ public abstract boolean getIsAlign();
+}
import lib.controllers.Ex3DProController.Ex3DProAxis;
import lib.controllers.Ex3DProController.Ex3DProButton;
-/**
- * Driver controls for the Ex3D Pro controller.
- */
+/** Driver controls for the Ex3D Pro controller. */
public class Ex3DProDriverConfig extends BaseDriverConfig {
- private final Ex3DProController kDriver = new Ex3DProController(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = kDriver.get(Ex3DProButton.B11);
+ private final Ex3DProController kDriver = new Ex3DProController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = kDriver.get(Ex3DProButton.B11);
- public Ex3DProDriverConfig(Drivetrain drive) {
- super(drive);
- }
+ public Ex3DProDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
- @Override
- public void configureControls() {
- kDriver.get(Ex3DProButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
- kDriver.get(Ex3DProButton.B2).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
- }
+ @Override
+ public void configureControls() {
+ kDriver.get(Ex3DProButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
+ kDriver
+ .get(Ex3DProButton.B2)
+ .onTrue(
+ new InstantCommand(
+ () -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
+ }
- @Override
- public double getRawSideTranslation() {
- return -kDriver.get(Ex3DProAxis.X);
- }
+ @Override
+ public double getRawSideTranslation() {
+ return -kDriver.get(Ex3DProAxis.X);
+ }
- @Override
- public double getRawForwardTranslation() {
- return -kDriver.get(Ex3DProAxis.Y);
- }
+ @Override
+ public double getRawForwardTranslation() {
+ return -kDriver.get(Ex3DProAxis.Y);
+ }
- @Override
- public double getRawRotation() {
- return kDriver.get(Ex3DProAxis.Z);
- }
+ @Override
+ public double getRawRotation() {
+ return kDriver.get(Ex3DProAxis.Z);
+ }
- @Override
- public double getRawHeadingAngle() {
- return kDriver.get(Ex3DProAxis.Z) * Math.PI;
- }
+ @Override
+ public double getRawHeadingAngle() {
+ return kDriver.get(Ex3DProAxis.Z) * Math.PI;
+ }
- @Override
- public double getRawHeadingMagnitude() {
- return kDriver.get(Ex3DProAxis.SLIDER);
- }
+ @Override
+ public double getRawHeadingMagnitude() {
+ return kDriver.get(Ex3DProAxis.SLIDER);
+ }
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
- }
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
- @Override
- public boolean getIsAlign() {
- return false;
- }
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
}
import lib.controllers.GameController.Axis;
import lib.controllers.GameController.Button;
-/**
- * Driver controls for the generic game controller.
- */
+/** Driver controls for the generic game controller. */
public class GameControllerDriverConfig extends BaseDriverConfig {
private final GameController driver = new GameController(Constants.DRIVER_JOY);
private final BooleanSupplier slowModeSupplier = driver.get(Button.RIGHT_JOY);
@Override
public void configureControls() {
// Reset yaw to be away from driver
- driver.get(Button.START).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+ driver
+ .get(Button.START)
+ .onTrue(
+ new InstantCommand(
+ () ->
+ super.getDrivetrain()
+ .setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
// Cancel commands
- driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
+ driver
+ .get(driver.RIGHT_TRIGGER_BUTTON)
+ .onTrue(
+ new InstantCommand(
+ () -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
}
@Override
import lib.controllers.MadCatzController.MadCatzAxis;
import lib.controllers.MadCatzController.MadCatzButton;
-/**
- * Driver controls for the MadCatz controller.
- */
+/** Driver controls for the MadCatz controller. */
public class MadCatzDriverConfig extends BaseDriverConfig {
- private final MadCatzController kDriver = new MadCatzController(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = kDriver.get(MadCatzButton.B6);
+ private final MadCatzController kDriver = new MadCatzController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = kDriver.get(MadCatzButton.B6);
- public MadCatzDriverConfig(Drivetrain drive) {
- super(drive);
- }
+ public MadCatzDriverConfig(Drivetrain drive) {
+ super(drive);
+ }
- @Override
- public void configureControls() {
- kDriver.get(MadCatzButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
- kDriver.get(MadCatzButton.B2).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
- }
+ @Override
+ public void configureControls() {
+ kDriver.get(MadCatzButton.B1).whileTrue(new SetFormationX(super.getDrivetrain()));
+ kDriver
+ .get(MadCatzButton.B2)
+ .onTrue(
+ new InstantCommand(
+ () -> super.getDrivetrain().setYaw(DriveConstants.STARTING_HEADING)));
+ }
- @Override
- public double getRawSideTranslation() {
- return kDriver.get(MadCatzAxis.X);
- }
+ @Override
+ public double getRawSideTranslation() {
+ return kDriver.get(MadCatzAxis.X);
+ }
- @Override
- public double getRawForwardTranslation() {
- return -kDriver.get(MadCatzAxis.Y);
- }
+ @Override
+ public double getRawForwardTranslation() {
+ return -kDriver.get(MadCatzAxis.Y);
+ }
- @Override
- public double getRawRotation() {
- return kDriver.get(MadCatzAxis.ZROTATE);
- }
+ @Override
+ public double getRawRotation() {
+ return kDriver.get(MadCatzAxis.ZROTATE);
+ }
- @Override
- public double getRawHeadingAngle() {
- return kDriver.get(MadCatzAxis.ZROTATE) * Math.PI;
- }
+ @Override
+ public double getRawHeadingAngle() {
+ return kDriver.get(MadCatzAxis.ZROTATE) * Math.PI;
+ }
- @Override
- public double getRawHeadingMagnitude() {
- return kDriver.get(MadCatzAxis.SLIDER);
- }
+ @Override
+ public double getRawHeadingMagnitude() {
+ return kDriver.get(MadCatzAxis.SLIDER);
+ }
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
- }
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
- @Override
- public boolean getIsAlign() {
- return false;
- }
-
-}
\ No newline at end of file
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+}
import frc.robot.subsystems.drivetrain.Drivetrain;
import lib.controllers.GameController;
-/**
- * Controls for the operator, which are almost a duplicate of most of the driver's controls
- */
+/** Controls for the operator, which are almost a duplicate of most of the driver's controls */
public class Operator {
- private final GameController driver = new GameController(Constants.OPERATOR_JOY);
-
- private final Drivetrain drive;
-
- public Operator(Drivetrain drive) {
- this.drive = drive;
- }
-
- public void configureControls() {
- // Cancel commands, could be removed if the operator doesn't need this button
- driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
- drive.setIsAlign(false);
- drive.setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
- }
-
-
- public Trigger getRightTrigger(){
- return new Trigger(driver.RIGHT_TRIGGER_BUTTON);
- }
- public Trigger getLeftTrigger(){
- return new Trigger(driver.LEFT_TRIGGER_BUTTON);
- }
- public GameController getGameController(){
- return driver;
- }
+ private final GameController driver = new GameController(Constants.OPERATOR_JOY);
+
+ private final Drivetrain drive;
+
+ public Operator(Drivetrain drive) {
+ this.drive = drive;
+ }
+
+ public void configureControls() {
+ // Cancel commands, could be removed if the operator doesn't need this button
+ driver
+ .get(driver.RIGHT_TRIGGER_BUTTON)
+ .onTrue(
+ new InstantCommand(
+ () -> {
+ drive.setIsAlign(false);
+ drive.setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+ }
+
+ public Trigger getRightTrigger() {
+ return new Trigger(driver.RIGHT_TRIGGER_BUTTON);
+ }
+
+ public Trigger getLeftTrigger() {
+ return new Trigger(driver.LEFT_TRIGGER_BUTTON);
+ }
+
+ public GameController getGameController() {
+ return driver;
+ }
}
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.Robot;
-import frc.robot.commands.drive_comm.SysIDDriveCommand;
import frc.robot.constants.Constants;
import frc.robot.subsystems.drivetrain.Drivetrain;
import lib.controllers.PS5Controller;
-import lib.controllers.PS5Controller.DPad;
import lib.controllers.PS5Controller.PS5Axis;
import lib.controllers.PS5Controller.PS5Button;
-/**
- * Driver controls for the PS5 controller
- */
+/** Driver controls for the PS5 controller */
public class PS5ControllerDriverConfig extends BaseDriverConfig {
private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
private final BooleanSupplier slowModeSupplier = () -> false;
public void configureControls() {
// Reset the yaw. Mainly useful for testing/driver practice
- controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+ controller
+ .get(PS5Button.CREATE)
+ .onTrue(
+ new InstantCommand(
+ () ->
+ getDrivetrain()
+ .setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
// Cancel commands
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
+ controller
+ .get(PS5Button.RB)
+ .onTrue(
+ new InstantCommand(
+ () -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
}
@Override
@Override
public double getRawHeadingAngle() {
- return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+ return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y))
+ - Math.PI / 2;
}
@Override
import lib.controllers.GameController.DPad;
/**
- * Driver config for PS5 controllers using Xbox 360 emulation mode.
- * This lets SCUF and other PS5 controllers work with WPILib rumble.
- *
- * Setup:
- * - download DSX (https://dualsensex.com/download/)
- * - install ViGEmBus driver (if app doesn't auto prompt)
- * - in dsx, set "controller emulation" to Xbox 360
- * - ensure rumble is enabled in dsx settings
- * - once code is depoloyed, change controller to "Xbox 360" in driverstation
+ * Driver config for PS5 controllers using Xbox 360 emulation mode. This lets SCUF and other PS5
+ * controllers work with WPILib rumble.
+ *
+ * <p>Setup: - download DSX (https://dualsensex.com/download/) - install ViGEmBus driver (if app
+ * doesn't auto prompt) - in dsx, set "controller emulation" to Xbox 360 - ensure rumble is enabled
+ * in dsx settings - once code is depoloyed, change controller to "Xbox 360" in driverstation
*/
-
public class PS5XboxModeDriverConfig extends BaseDriverConfig {
private final GameController controller = new GameController(Constants.DRIVER_JOY);
private final BooleanSupplier slowModeSupplier = () -> false;
private final Axis LEFT_Y = Axis.LEFT_Y;
private final Axis RIGHT_X = Axis.RIGHT_X;
private final Axis RIGHT_Y = Axis.RIGHT_Y;
+
// private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
// private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
public void configureControls() {
// Reset the yaw. Mainly useful for testing/driver practice
- controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+ controller
+ .get(CREATE)
+ .onTrue(
+ new InstantCommand(
+ () ->
+ getDrivetrain()
+ .setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
// Cancel commands
- controller.get(RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
+ controller
+ .get(RB)
+ .onTrue(
+ new InstantCommand(
+ () -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
// Align wheels
- controller.get(DPad.RIGHT).onTrue(new FunctionalCommand(
- () -> getDrivetrain().setStateDeadband(false),
- getDrivetrain()::alignWheels,
- interrupted -> getDrivetrain().setStateDeadband(true),
- () -> false, getDrivetrain()).withTimeout(2));
+ controller
+ .get(DPad.RIGHT)
+ .onTrue(
+ new FunctionalCommand(
+ () -> getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted -> getDrivetrain().setStateDeadband(true),
+ () -> false,
+ getDrivetrain())
+ .withTimeout(2));
// Rumble test
- controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup(
- new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
- new WaitCommand(0.5),
- new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
+ controller
+ .get(RIGHT_JOY)
+ .onTrue(
+ new SequentialCommandGroup(
+ new InstantCommand(
+ () -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
+ new WaitCommand(0.5),
+ new InstantCommand(
+ () -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
}
@Override
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
public class LED extends SubsystemBase {
- private CANdle candle;
- public static final int stripLength = 67;
-
- /// Hz
- public static final int FLASH_RATE = 4;
-
- private Color color;
-
- public LED() {
- candle = new CANdle(IdConstants.CANDLE_ID, Constants.RIO_CAN);
- CANdleConfigurator configurator = candle.getConfigurator();
-
- LEDConfigs ledConf = new LEDConfigs()
- .withStripType(StripTypeValue.GRB)
- .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)
- .withBrightnessScalar(1);
-
- CANdleFeaturesConfigs featureConf = new CANdleFeaturesConfigs()
- .withEnable5VRail(Enable5VRailValue.Enabled) // Turns off LEDs
- .withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)
- .withVBatOutputMode(VBatOutputModeValue.On);
-
- configurator.apply(featureConf);
- configurator.apply(ledConf);
-
- setColor();
-
- candle.clearAllAnimations();
- lightsOff();
-
- // System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
- }
-
- public void setColor() {
- var alliance = DriverStation.getAlliance();
- if (alliance.isEmpty()) {
- color = Color.kOrangeRed;
- } else if (alliance.get() == Alliance.Red) {
- color = Color.kRed;
- } else if (alliance.get() == Alliance.Blue) {
- color = Color.kBlue;
- } else {
- color = Color.kOrangeRed;
- }
- }
-
- private enum State { OFF, ON, AUTO, SLOW, FAST, ENDGAME };
-
- private State lastState = State.OFF;
- private boolean forceOff = false;
-
- @Override
- public void periodic() {
- State targetState = State.ON;
- // if (underSecsToFlip(5)) targetState = State.SLOW;
- // if (underSecsToFlip(1)) targetState = State.FAST;
- if (DriverStation.isAutonomous()) targetState = State.AUTO;
- if (DriverStation.getMatchTime() < 30) targetState = State.ENDGAME;
- if (forceOff) targetState = State.OFF;
-
- if (targetState != lastState) {
- switch (targetState) {
- case OFF: lightsOff(); break;
- case ON: setStatic(); break;
- case AUTO: setTwinkle(); break;
- case SLOW: setStrobe(); break;
- case FAST: setFastStrobe(); break;
- case ENDGAME: setRainbow(); break;
- }
- lastState = targetState;
- }
- }
-
- public void setFire() {
- candle.clearAllAnimations();
- candle.setControl(new FireAnimation(8, 8 + stripLength).withSparking(0.5));
- }
-
- public void setRainbow() {
- candle.clearAllAnimations();
- candle.setControl(new RainbowAnimation(8, 8 + stripLength));
- }
-
- public void setRgbFadeAnimation() {
- candle.clearAllAnimations();
- candle.setControl(new RgbFadeAnimation(8, 8 + stripLength));
- }
-
- public void setTwinkle() {
- candle.clearAllAnimations();
- candle.setControl(new TwinkleAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kViolet)));
- }
-
- public void setColorFlow() {
- candle.clearAllAnimations();
- candle.setControl(new ColorFlowAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kAzure)));
- }
-
- public void setStrobe() {
- candle.clearAllAnimations();
- candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE).withColor(new RGBWColor(color)));
- }
-
- public void setFastStrobe() {
- candle.clearAllAnimations();
- candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE * 4).withColor(new RGBWColor(color)));
- }
-
- public void setStatic() {
- candle.clearAllAnimations();
- candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(color)));
- }
-
- public void lightsOff() {
- candle.clearAllAnimations();
- candle.setControl(new SolidColor(8 , 8 + stripLength).withColor(new RGBWColor(0, 0, 0, 0)));
- }
+ private CANdle candle;
+ public static final int stripLength = 67;
+
+ /// Hz
+ public static final int FLASH_RATE = 4;
+
+ private Color color;
+
+ public LED() {
+ candle = new CANdle(IdConstants.CANDLE_ID, Constants.RIO_CAN);
+ CANdleConfigurator configurator = candle.getConfigurator();
+
+ LEDConfigs ledConf =
+ new LEDConfigs()
+ .withStripType(StripTypeValue.GRB)
+ .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)
+ .withBrightnessScalar(1);
+
+ CANdleFeaturesConfigs featureConf =
+ new CANdleFeaturesConfigs()
+ .withEnable5VRail(Enable5VRailValue.Enabled) // Turns off LEDs
+ .withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)
+ .withVBatOutputMode(VBatOutputModeValue.On);
+
+ configurator.apply(featureConf);
+ configurator.apply(ledConf);
+
+ setColor();
+
+ candle.clearAllAnimations();
+ lightsOff();
+
+ // System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
+ }
+
+ public void setColor() {
+ var alliance = DriverStation.getAlliance();
+ if (alliance.isEmpty()) {
+ color = Color.kOrangeRed;
+ } else if (alliance.get() == Alliance.Red) {
+ color = Color.kRed;
+ } else if (alliance.get() == Alliance.Blue) {
+ color = Color.kBlue;
+ } else {
+ color = Color.kOrangeRed;
+ }
+ }
+
+ private enum State {
+ OFF,
+ ON,
+ AUTO,
+ SLOW,
+ FAST,
+ ENDGAME
+ };
+
+ private State lastState = State.OFF;
+ private boolean forceOff = false;
+
+ @Override
+ public void periodic() {
+ State targetState = State.ON;
+ // if (underSecsToFlip(5)) targetState = State.SLOW;
+ // if (underSecsToFlip(1)) targetState = State.FAST;
+ if (DriverStation.isAutonomous()) targetState = State.AUTO;
+ if (DriverStation.getMatchTime() < 30) targetState = State.ENDGAME;
+ if (forceOff) targetState = State.OFF;
+
+ if (targetState != lastState) {
+ switch (targetState) {
+ case OFF:
+ lightsOff();
+ break;
+ case ON:
+ setStatic();
+ break;
+ case AUTO:
+ setTwinkle();
+ break;
+ case SLOW:
+ setStrobe();
+ break;
+ case FAST:
+ setFastStrobe();
+ break;
+ case ENDGAME:
+ setRainbow();
+ break;
+ }
+ lastState = targetState;
+ }
+ }
+
+ public void setFire() {
+ candle.clearAllAnimations();
+ candle.setControl(new FireAnimation(8, 8 + stripLength).withSparking(0.5));
+ }
+
+ public void setRainbow() {
+ candle.clearAllAnimations();
+ candle.setControl(new RainbowAnimation(8, 8 + stripLength));
+ }
+
+ public void setRgbFadeAnimation() {
+ candle.clearAllAnimations();
+ candle.setControl(new RgbFadeAnimation(8, 8 + stripLength));
+ }
+
+ public void setTwinkle() {
+ candle.clearAllAnimations();
+ candle.setControl(
+ new TwinkleAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kViolet)));
+ }
+
+ public void setColorFlow() {
+ candle.clearAllAnimations();
+ candle.setControl(
+ new ColorFlowAnimation(8, 8 + stripLength).withColor(new RGBWColor(Color.kAzure)));
+ }
+
+ public void setStrobe() {
+ candle.clearAllAnimations();
+ candle.setControl(
+ new StrobeAnimation(8, 8 + stripLength)
+ .withFrameRate(FLASH_RATE)
+ .withColor(new RGBWColor(color)));
+ }
+
+ public void setFastStrobe() {
+ candle.clearAllAnimations();
+ candle.setControl(
+ new StrobeAnimation(8, 8 + stripLength)
+ .withFrameRate(FLASH_RATE * 4)
+ .withColor(new RGBWColor(color)));
+ }
+
+ public void setStatic() {
+ candle.clearAllAnimations();
+ candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(color)));
+ }
+
+ public void lightsOff() {
+ candle.clearAllAnimations();
+ candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(0, 0, 0, 0)));
+ }
}
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Battery extends SubsystemBase {
- private double voltage;
-
- public Battery() {
- updateVoltageFromBattery();
- }
-
- private void updateVoltageFromBattery() {
- voltage = RobotController.getBatteryVoltage();
- }
-
- public double getVoltage() {
- return voltage;
- }
-
- public double voltsTillBrownOut() {
- return voltage - RobotController.getBrownoutVoltage();
- }
-
- public double toBrownOut() {
- // percent of volts we've got left over what we had to start with
- return voltsTillBrownOut() / (BatteryConstants.MAX_STARTING_VOLTS - RobotController.getBrownoutVoltage());
- }
-
- @Override
- public void periodic() {
- updateVoltageFromBattery();
- }
+ private double voltage;
+
+ public Battery() {
+ updateVoltageFromBattery();
+ }
+
+ private void updateVoltageFromBattery() {
+ voltage = RobotController.getBatteryVoltage();
+ }
+
+ public double getVoltage() {
+ return voltage;
+ }
+
+ public double voltsTillBrownOut() {
+ return voltage - RobotController.getBrownoutVoltage();
+ }
+
+ public double toBrownOut() {
+ // percent of volts we've got left over what we had to start with
+ return voltsTillBrownOut()
+ / (BatteryConstants.MAX_STARTING_VOLTS - RobotController.getBrownoutVoltage());
+ }
+
+ @Override
+ public void periodic() {
+ updateVoltageFromBattery();
+ }
}
package frc.robot.subsystems.PowerControl;
public class BatteryConstants {
- public static final double MAX_STARTING_VOLTS = 12.5; // V
+ public static final double MAX_STARTING_VOLTS = 12.5; // V
}
import java.util.Map;
public class BreakerConstants {
- public static final Map<Double, Double> THRESHOLDS = new LinkedHashMap<>();
- static {
- THRESHOLDS.put(1.0, 6.0 * 120); // breaker default at 120
- THRESHOLDS.put(4.0, 3.4 * 120);
- THRESHOLDS.put(10.0, 2.0 * 120);
- THRESHOLDS.put(20.0, 1.6 * 120);
- THRESHOLDS.put(30.0, 1.5 * 120);
- }
+ public static final Map<Double, Double> THRESHOLDS = new LinkedHashMap<>();
- public static final double WARNING_PERCENTAGE = 0.6; // percent that the system reacts to approaching thresholds
+ static {
+ THRESHOLDS.put(1.0, 6.0 * 120); // breaker default at 120
+ THRESHOLDS.put(4.0, 3.4 * 120);
+ THRESHOLDS.put(10.0, 2.0 * 120);
+ THRESHOLDS.put(20.0, 1.6 * 120);
+ THRESHOLDS.put(30.0, 1.5 * 120);
+ }
- // ports
- public static int[] DRIVETRAIN_PORTS = { 8, 9, 10, 11, 18, 19, 0, 1 }; // bls, bld, fld, fls, frs, frd, brd, brs
- public static int[] TURRET_PORTS = { 2 };
- public static int[] INTAKE_PORTS = { 15, 14, 13 }; // right, left, roller
- public static int[] SHOOTER_PORTS = { 3, 4 }; // left, right
- public static int[] HOOD_PORTS = { 5 }; // shooter
- public static int[] SPINDEXER_PORTS = { 12 }; // spindexer (unupdated on sheets)
+ public static final double WARNING_PERCENTAGE =
+ 0.6; // percent that the system reacts to approaching thresholds
+
+ // ports
+ public static int[] DRIVETRAIN_PORTS = {
+ 8, 9, 10, 11, 18, 19, 0, 1
+ }; // bls, bld, fld, fls, frs, frd, brd, brs
+ public static int[] TURRET_PORTS = {2};
+ public static int[] INTAKE_PORTS = {15, 14, 13}; // right, left, roller
+ public static int[] SHOOTER_PORTS = {3, 4}; // left, right
+ public static int[] HOOD_PORTS = {5}; // shooter
+ public static int[] SPINDEXER_PORTS = {12}; // spindexer (unupdated on sheets)
}
public class EMABreaker extends SubsystemBase {
- private static class Current {
- double tau;
- double alpha; // how much of the error we correct per loop
- double average = 0;
- double threshold;
+ private static class Current {
+ double tau;
+ double alpha; // how much of the error we correct per loop
+ double average = 0;
+ double threshold;
+ }
+
+ PowerDistribution pDis = new PowerDistribution();
+
+ double[] subsystemCurrents;
+
+ private List<Current> filters =
+ new ArrayList<>(); // contains currents with their alphas and thresholds
+ private List<Current> subsystems = new ArrayList<>();
+
+ public EMABreaker() {
+ for (Map.Entry<Double, Double> entry : BreakerConstants.THRESHOLDS.entrySet()) {
+ double tau = entry.getKey(); // sec
+ double threshold = entry.getValue(); // A
+
+ Current w = new Current(); // create a filter for the threshold
+ w.tau = tau;
+ w.threshold = threshold;
+ w.alpha =
+ 1
+ - Math.exp(
+ -Constants.LOOP_TIME
+ / tau); // 1 - e^(-0.02/1) = 0.0198, 1 - e^(-0.02/2) = 0.00995
+
+ filters.add(w);
}
- PowerDistribution pDis = new PowerDistribution();
+ // subsystems
+ for (int i = 0; i < pDis.getNumChannels(); i++) {
+ double tau = 1.0;
+ double threshold = i;
+ Current w = new Current();
+ w.tau = tau;
+ w.threshold = threshold;
+ w.alpha = 1 - Math.exp(-Constants.LOOP_TIME / tau); // 1 - e^(-0.02/1) = 0.
- double[] subsystemCurrents;
-
- private List<Current> filters = new ArrayList<>(); // contains currents with their alphas and thresholds
- private List<Current> subsystems = new ArrayList<>();
-
- public EMABreaker() {
- for (Map.Entry<Double, Double> entry : BreakerConstants.THRESHOLDS.entrySet()) {
- double tau = entry.getKey(); // sec
- double threshold = entry.getValue(); // A
-
- Current w = new Current(); // create a filter for the threshold
- w.tau = tau;
- w.threshold = threshold;
- w.alpha = 1 - Math.exp(-Constants.LOOP_TIME / tau); // 1 - e^(-0.02/1) = 0.0198, 1 - e^(-0.02/2) = 0.00995
-
- filters.add(w);
- }
-
- // subsystems
- for (int i = 0; i < pDis.getNumChannels(); i++) {
- double tau = 1.0;
- double threshold = i;
- Current w = new Current();
- w.tau = tau;
- w.threshold = threshold;
- w.alpha = 1 - Math.exp(-Constants.LOOP_TIME / tau); // 1 - e^(-0.02/1) = 0.
-
- subsystems.add(w);
- }
+ subsystems.add(w);
}
-
- @Override
- public void periodic() {
- double current = getCurrentFromPowerDistribution();
- // this is total current averages
- for (Current f : filters) {
- // new avg = old avg + fractionAlpha * difference
- f.average += f.alpha * (current - f.average);
- Logger.recordOutput("Breaker/IntervalAverage/" + f.tau, f.average);
- }
-
- // this is getting currents coming out of all the ports from PDH (big thing
- // under robot all the wires come out of)
- subsystemCurrents = getAllCurrentFromPowerDistribution();
-
- // this should average out all ports
- for (Current s : subsystems) {
- s.average += s.alpha * (subsystemCurrents[(int) s.threshold] - s.average);
- }
-
- // this should use updated port averages and sum them to get drivetrain average
- // draw for 1 tau (can add more later)
- Logger.recordOutput("Breaker/DrivetrainAverageDraw", getAverageCurrentDraw(BreakerConstants.DRIVETRAIN_PORTS));
- Logger.recordOutput("Breaker/SpindexerDraw", getAverageCurrentDraw(BreakerConstants.SPINDEXER_PORTS));
- Logger.recordOutput("Breaker/ShooterDraw", getAverageCurrentDraw(BreakerConstants.SHOOTER_PORTS));
- Logger.recordOutput("Breaker/IntakeDraw", getAverageCurrentDraw(BreakerConstants.INTAKE_PORTS));
- Logger.recordOutput("Breaker/TurretDraw", getAverageCurrentDraw(BreakerConstants.TURRET_PORTS));
- Logger.recordOutput("Breaker/HoodDraw", getAverageCurrentDraw(BreakerConstants.HOOD_PORTS));
-
- // total stuff
- Logger.recordOutput("Breaker/TotalCurrent", current);
- Logger.recordOutput("Breaker/CurrentWarning", isInWarning());
+ }
+
+ @Override
+ public void periodic() {
+ double current = getCurrentFromPowerDistribution();
+ // this is total current averages
+ for (Current f : filters) {
+ // new avg = old avg + fractionAlpha * difference
+ f.average += f.alpha * (current - f.average);
+ Logger.recordOutput("Breaker/IntervalAverage/" + f.tau, f.average);
}
- public double getAverageCurrentDraw(int[] ports) {
- double sum = 0;
- for (int number : ports) {
- sum += subsystems.get(number).average;
- }
- return sum;
- }
+ // this is getting currents coming out of all the ports from PDH (big thing
+ // under robot all the wires come out of)
+ subsystemCurrents = getAllCurrentFromPowerDistribution();
- public double getCurrentFromPowerDistribution() {
- return pDis.getTotalCurrent(); // not using .getCurrent() and then an arguement for the port you can get just
- // one port
+ // this should average out all ports
+ for (Current s : subsystems) {
+ s.average += s.alpha * (subsystemCurrents[(int) s.threshold] - s.average);
}
- public double[] getAllCurrentFromPowerDistribution() {
- return pDis.getAllCurrents();
+ // this should use updated port averages and sum them to get drivetrain average
+ // draw for 1 tau (can add more later)
+ Logger.recordOutput(
+ "Breaker/DrivetrainAverageDraw", getAverageCurrentDraw(BreakerConstants.DRIVETRAIN_PORTS));
+ Logger.recordOutput(
+ "Breaker/SpindexerDraw", getAverageCurrentDraw(BreakerConstants.SPINDEXER_PORTS));
+ Logger.recordOutput(
+ "Breaker/ShooterDraw", getAverageCurrentDraw(BreakerConstants.SHOOTER_PORTS));
+ Logger.recordOutput("Breaker/IntakeDraw", getAverageCurrentDraw(BreakerConstants.INTAKE_PORTS));
+ Logger.recordOutput("Breaker/TurretDraw", getAverageCurrentDraw(BreakerConstants.TURRET_PORTS));
+ Logger.recordOutput("Breaker/HoodDraw", getAverageCurrentDraw(BreakerConstants.HOOD_PORTS));
+
+ // total stuff
+ Logger.recordOutput("Breaker/TotalCurrent", current);
+ Logger.recordOutput("Breaker/CurrentWarning", isInWarning());
+ }
+
+ public double getAverageCurrentDraw(int[] ports) {
+ double sum = 0;
+ for (int number : ports) {
+ sum += subsystems.get(number).average;
}
-
- public boolean isInWarning() {
- for (Current f : filters) {
- if (f.average > f.threshold * BreakerConstants.WARNING_PERCENTAGE) {
- return true; // uh oh
- }
- }
- return false;
+ return sum;
+ }
+
+ public double getCurrentFromPowerDistribution() {
+ return pDis
+ .getTotalCurrent(); // not using .getCurrent() and then an arguement for the port you can
+ // get just
+ // one port
+ }
+
+ public double[] getAllCurrentFromPowerDistribution() {
+ return pDis.getAllCurrents();
+ }
+
+ public boolean isInWarning() {
+ for (Current f : filters) {
+ if (f.average > f.threshold * BreakerConstants.WARNING_PERCENTAGE) {
+ return true; // uh oh
+ }
}
-
- // returns an average of the filters
- public double percentageAverageUsage() {
- double sumAvg = 0;
- for (Current f : filters) {
- sumAvg += f.average / f.threshold; // gets percentage of us
- }
- return sumAvg / filters.size(); // average across filters
+ return false;
+ }
+
+ // returns an average of the filters
+ public double percentageAverageUsage() {
+ double sumAvg = 0;
+ for (Current f : filters) {
+ sumAvg += f.average / f.threshold; // gets percentage of us
}
-
- // gives the worst case filter
- public double[] percentageMaxUsage() {
- Current worst = filters.get(0); // returns worst (default to tau filter)
- for (Current f : filters) {
- if (f.average / f.threshold > worst.average / worst.threshold) {
- worst = f;
- }
- }
- double[] returnValue = { worst.average / worst.threshold, worst.tau };
- return returnValue;
+ return sumAvg / filters.size(); // average across filters
+ }
+
+ // gives the worst case filter
+ public double[] percentageMaxUsage() {
+ Current worst = filters.get(0); // returns worst (default to tau filter)
+ for (Current f : filters) {
+ if (f.average / f.threshold > worst.average / worst.threshold) {
+ worst = f;
+ }
}
+ double[] returnValue = {worst.average / worst.threshold, worst.tau};
+ return returnValue;
+ }
}
/**
* Represents a swerve drive style drivetrain.
- * <p>
- * Module IDs are:
- * 1: Front left
- * 2: Front right
- * 3: Back left
- * 4: Back right
+ *
+ * <p>Module IDs are: 1: Front left 2: Front right 3: Back left 4: Back right
*/
public class Drivetrain extends SubsystemBase {
- protected final Module[] modules;
+ protected final Module[] modules;
- private final GyroIO gyroIO;
- private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
+ private final GyroIO gyroIO;
+ private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
- public static Lock odometryLock = new ReentrantLock();
+ public static Lock odometryLock = new ReentrantLock();
- private SwerveSetpoint currentSetpoint = new SwerveSetpoint(
- new ChassisSpeeds(),
- new SwerveModuleState[] {
- new SwerveModuleState(),
- new SwerveModuleState(),
- new SwerveModuleState(),
- new SwerveModuleState()
- });
- // Odometry
- private final SwerveDrivePoseEstimator poseEstimator;
+ private SwerveSetpoint currentSetpoint =
+ new SwerveSetpoint(
+ new ChassisSpeeds(),
+ new SwerveModuleState[] {
+ new SwerveModuleState(),
+ new SwerveModuleState(),
+ new SwerveModuleState(),
+ new SwerveModuleState()
+ });
+ // Odometry
+ private final SwerveDrivePoseEstimator poseEstimator;
- // Vision
- private final Vision vision;
+ // Vision
+ private final Vision vision;
- // PID Controllers for chassis movement
- private final PIDController xController;
- private final PIDController yController;
- private final PIDController rotationController;
+ // PID Controllers for chassis movement
+ private final PIDController xController;
+ private final PIDController yController;
+ private final PIDController rotationController;
- // If vision is enabled for drivetrain odometry updating
- // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED
- // instead
- private boolean visionEnabled = true;
+ // If vision is enabled for drivetrain odometry updating
+ // DO NOT CHANGE THIS HERE TO DISABLE VISION, change VisionConstants.ENABLED
+ // instead
+ private boolean visionEnabled = true;
- // Disables vision for the first few seconds after deploying
- private Timer visionEnableTimer = new Timer();
+ // Disables vision for the first few seconds after deploying
+ private Timer visionEnableTimer = new Timer();
- // If the robot should algin to the angle
- private boolean isAlign = false;
- // Angle to align to, can be null
- private Double alignAngle = null;
- // used for drift control
- private double currentHeading = 0;
- // used for drift control
- private boolean drive_turning = false;
+ // If the robot should algin to the angle
+ private boolean isAlign = false;
+ // Angle to align to, can be null
+ private Double alignAngle = null;
+ // used for drift control
+ private double currentHeading = 0;
+ // used for drift control
+ private boolean drive_turning = false;
- private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+ private SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
- // The pose supplier to drive to
-
private Supplier<Pose2d> desiredPoSupplier = () -> null;
+ // The pose supplier to drive to
+ private Supplier<Pose2d> desiredPoSupplier = () -> null;
- private SwerveModulePose modulePoses;
+ private SwerveModulePose modulePoses;
- // The previous pose to reset to if the current pose gets too far off the field
- private Pose2d prevPose = new Pose2d();
+ // The previous pose to reset to if the current pose gets too far off the field
+ private Pose2d prevPose = new Pose2d();
- private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];;
+ private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ ;
- private boolean slipped = false;
+ private boolean slipped = false;
- private double previousAngularVelocity = 0;
+ private double previousAngularVelocity = 0;
- private double centerOfMassHeight = 0;
+ private double centerOfMassHeight = 0;
- private Rotation2d rawGyroRotation = new Rotation2d();
+ private Rotation2d rawGyroRotation = new Rotation2d();
- // for vision yaw correction
- private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator();
+ // for vision yaw correction
+ private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator();
- private final Field2d field = new Field2d();
+ private final Field2d field = new Field2d();
- /**
- * Creates a new Swerve Style Drivetrain.
- */
- public Drivetrain(Vision vision, GyroIO gyroIO) {
- this.vision = vision;
+ /** Creates a new Swerve Style Drivetrain. */
+ public Drivetrain(Vision vision, GyroIO gyroIO) {
+ this.vision = vision;
- modules = new Module[4];
- this.gyroIO = gyroIO;
- ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4);
+ modules = new Module[4];
+ this.gyroIO = gyroIO;
+ ModuleConstants[] constants = Arrays.copyOfRange(ModuleConstants.values(), 0, 4);
- if (RobotBase.isReal()) {
- Arrays.stream(constants).forEach(moduleConstants -> {
+ if (RobotBase.isReal()) {
+ Arrays.stream(constants)
+ .forEach(
+ moduleConstants -> {
modules[moduleConstants.ordinal()] = new Module(moduleConstants);
- });
- } else {
- Arrays.stream(constants).forEach(moduleConstants -> {
+ });
+ } else {
+ Arrays.stream(constants)
+ .forEach(
+ moduleConstants -> {
modules[moduleConstants.ordinal()] = new ModuleSim(moduleConstants);
- });
- }
-
- /*
- * By pausing init for a second before setting module offsets, we avoid a bug
- * with inverting motors.
- * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info.
- */
- Timer.delay(1.0);
- resetModulesToAbsolute();
- gyroIO.updateInputs(gyroInputs);
- poseEstimator = new SwerveDrivePoseEstimator(
- DriveConstants.KINEMATICS,
- gyroInputs.yawPosition,
- updateModulePositions(),
- new Pose2d(),
- // Defaults, except trust pigeon more
- VecBuilder.fill(0.1, 0.1, 0),
- VisionConstants.VISION_STD_DEVS);
- poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS);
-
- // initialize PID controllers
- xController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
- yController = new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
- rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D);
- rotationController.enableContinuousInput(-Math.PI, Math.PI);
- rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25));
-
- PhoenixOdometryThread.getInstance().start();
-
- modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS);
-
- PathPlannerLogging.setLogActivePathCallback(
- (activePath) -> {
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput(
- "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
- }
- });
- PathPlannerLogging.setLogTargetPoseCallback(
- (targetPose) -> {
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
- }
- });
-
- // PPLibTelemetry.enableCompetitionMode();
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putData("Field", field);
- }
-
- // addMusic();
-
- }
-
- public void setPose(Translation2d pose) {
- poseEstimator.resetTranslation(pose);
- }
-
- public void addMusic() {
- ArrayList<TalonFX> motors = new ArrayList<>();
- for (Module m: modules) {
- motors.add(m.getMotors()[0]);
- motors.add(m.getMotors()[1]);
- }
-
- TalonFX[] f = new TalonFX[8];
-
- SmartDashboard.putData("Chirp", new Music(motors.toArray(f)));
- }
-
- public void close() {
- // close each of the modules
- for (int i = 0; i < modules.length; i++) {
- modules[i].close();
- }
- }
-
- @Override
- public void periodic() {
- odometryLock.lock(); // Prevents odometry updates while reading data
- gyroIO.updateInputs(gyroInputs);
- Logger.processInputs("Drive/Gyro", gyroInputs);
- for (var module : modules) {
- module.periodic();
- }
- odometryLock.unlock();
- // Update odometry
- double[] sampleTimestamps = gyroInputs.odometryYawTimestamps; // All signals are sampled together
- int sampleCount = sampleTimestamps.length;
- SwerveModulePosition[][] positions = new SwerveModulePosition[4][];
- for (int i = 0; i < modules.length; i++) {
- positions[i] = modules[i].getOdometryPositions();
- sampleCount = Math.min(sampleCount, positions[i].length);
- }
-
- // cap samples per cycle, more gives little benefit
- final int MAX_SAMPLES_PER_CYCLE = 10;
- if (sampleCount > MAX_SAMPLES_PER_CYCLE) {
- sampleCount = MAX_SAMPLES_PER_CYCLE;
- }
-
- for (int i = 0; i < sampleCount; i++) {
- // Read wheel positions and deltas from each module
- SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
- for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
- modulePositions[moduleIndex] = positions[moduleIndex][i];
- }
- // Use the real gyro angle
- rawGyroRotation = gyroInputs.odometryYawPositions[i];
- // Apply update
- poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
- }
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
- }
- updateOdometryVision();
-
- field.setRobotPose(getPose());
- }
-
- // DRIVE
- /**
- * Method to drive the robot using joystick info.
- *
- * @param xSpeed speed of the robot in the x direction (forward) in m/s
- * @param ySpeed speed of the robot in the y direction (sideways) in m/s
- * @param rot angular rate of the robot in rad/s
- * @param fieldRelative whether the provided x and y speeds are relative to the
- * field
- * @param isOpenLoop whether to use velocity control for the drive motors
- */
- public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) {
- // rot = headingControl(rot, xSpeed, ySpeed);
- ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
- if (fieldRelative) {
- speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
- }
- setChassisSpeeds(speeds, isOpenLoop);
- }
-
- /**
- * Drives the robot using the provided x speed, y speed, and positional heading.
- *
- * @param xSpeed speed of the robot in the x direction (forward)
- * @param ySpeed speed of the robot in the y direction (sideways)
- * @param heading target heading of the robot in radians
- * @param fieldRelative whether the provided x and y speeds are relative to the
- * field
- */
- public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
- double rot = rotationController.calculate(getYaw().getRadians(), heading);
- ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
- if (fieldRelative) {
- speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
- }
- setChassisSpeeds(speeds, false);
+ });
}
- /**
- * Runs the PID controllers with the provided x, y, and rot values. Then, calls
- * {@link #drive(double, double, double, boolean, boolean)} using the PID
- * outputs.
- * This is based on the odometry of the chassis.
- *
- * @param x the position to move to in the x, in meters
- * @param y the position to move to in the y, in meters
- * @param rot the angle to move to, in radians
+ /*
+ * By pausing init for a second before setting module offsets, we avoid a bug
+ * with inverting motors.
+ * See https://github.com/Team364/BaseFalconSwerve/issues/8 for more info.
*/
- public void driveWithPID(double x, double y, double rot) {
- Pose2d pose = getPose();
- double xSpeed = xController.calculate(pose.getX(), x);
- double ySpeed = yController.calculate(pose.getY(), y);
- double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot);
- drive(xSpeed, ySpeed, rotRadians, true, false);
- }
-
- /**
- * Updates odometry using vision
- */
- public void updateOdometryVision() {
- // Start the timer if it hasn't started yet
- visionEnableTimer.start();
-
- // Update the swerve module poses
- modulePoses.update();
-
- if (modulePoses.slipped()) {
- slipped = true;
- }
-
- Pose2d pose2 = getPose();
-
- // Even if vision is disabled, it should still update inputs
- // This prevents it from storing a lot of unread results, and it could be useful
- // for replays
- if (vision != null) {
- vision.updateInputs();
- }
-
- if (VisionConstants.ENABLED) {
- if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) {
- vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
-
- if (vision.canSeeTag()) {
- slipped = false;
- modulePoses.reset();
-
- double currentGyroYaw = gyroInputs.yawPosition.getRadians();
-
- // to compare bias
- ArrayList<EstimatedRobotPose> visionPoses = vision.getEstimatedPoses(getPose());
-
- for (EstimatedRobotPose visionPose : visionPoses) {
- if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) {
- double visionYaw = visionPose.estimatedPose.getRotation().getZ();
-
- // gets at vision timestamp, not current gyro yaw
- double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds);
-
- if (!Double.isNaN(gyroYawAtTimestamp)) {
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp));
- Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw));
- }
- // use weighted observation
- gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0);
- }
- }
- }
-
- // check if we have enough samples
- if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) {
- double fullBias = gyroBiasEstimator.getAndResetBias();
- double bias = gyroBiasEstimator.applyPartialCorrection(fullBias);
-
- if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) {
- gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias));
- }
- }
+ Timer.delay(1.0);
+ resetModulesToAbsolute();
+ gyroIO.updateInputs(gyroInputs);
+ poseEstimator =
+ new SwerveDrivePoseEstimator(
+ DriveConstants.KINEMATICS,
+ gyroInputs.yawPosition,
+ updateModulePositions(),
+ new Pose2d(),
+ // Defaults, except trust pigeon more
+ VecBuilder.fill(0.1, 0.1, 0),
+ VisionConstants.VISION_STD_DEVS);
+ poseEstimator.setVisionMeasurementStdDevs(VisionConstants.VISION_STD_DEVS);
+
+ // initialize PID controllers
+ xController =
+ new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+ yController =
+ new PIDController(DriveConstants.TRANSLATIONAL_P, 0, DriveConstants.TRANSLATIONAL_D);
+ rotationController = new PIDController(DriveConstants.HEADING_P, 0, DriveConstants.HEADING_D);
+ rotationController.enableContinuousInput(-Math.PI, Math.PI);
+ rotationController.setTolerance(Units.degreesToRadians(0.25), Units.degreesToRadians(0.25));
+
+ PhoenixOdometryThread.getInstance().start();
+
+ modulePoses = new SwerveModulePose(this, DriveConstants.MODULE_LOCATIONS);
+
+ PathPlannerLogging.setLogActivePathCallback(
+ (activePath) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput(
+ "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
+ }
+ });
+ PathPlannerLogging.setLogTargetPoseCallback(
+ (targetPose) -> {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
+ }
+ });
+
+ // PPLibTelemetry.enableCompetitionMode();
+ if (!Constants.DISABLE_SMART_DASHBOARD) {
+ SmartDashboard.putData("Field", field);
+ }
+
+ // addMusic();
+
+ }
+
+ public void setPose(Translation2d pose) {
+ poseEstimator.resetTranslation(pose);
+ }
+
+ public void addMusic() {
+ ArrayList<TalonFX> motors = new ArrayList<>();
+ for (Module m : modules) {
+ motors.add(m.getMotors()[0]);
+ motors.add(m.getMotors()[1]);
+ }
+
+ TalonFX[] f = new TalonFX[8];
+
+ SmartDashboard.putData("Chirp", new Music(motors.toArray(f)));
+ }
+
+ public void close() {
+ // close each of the modules
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].close();
+ }
+ }
+
+ @Override
+ public void periodic() {
+ odometryLock.lock(); // Prevents odometry updates while reading data
+ gyroIO.updateInputs(gyroInputs);
+ Logger.processInputs("Drive/Gyro", gyroInputs);
+ for (var module : modules) {
+ module.periodic();
+ }
+ odometryLock.unlock();
+ // Update odometry
+ double[] sampleTimestamps =
+ gyroInputs.odometryYawTimestamps; // All signals are sampled together
+ int sampleCount = sampleTimestamps.length;
+ SwerveModulePosition[][] positions = new SwerveModulePosition[4][];
+ for (int i = 0; i < modules.length; i++) {
+ positions[i] = modules[i].getOdometryPositions();
+ sampleCount = Math.min(sampleCount, positions[i].length);
+ }
+
+ // cap samples per cycle, more gives little benefit
+ final int MAX_SAMPLES_PER_CYCLE = 10;
+ if (sampleCount > MAX_SAMPLES_PER_CYCLE) {
+ sampleCount = MAX_SAMPLES_PER_CYCLE;
+ }
+
+ for (int i = 0; i < sampleCount; i++) {
+ // Read wheel positions and deltas from each module
+ SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+ for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
+ modulePositions[moduleIndex] = positions[moduleIndex][i];
+ }
+ // Use the real gyro angle
+ rawGyroRotation = gyroInputs.odometryYawPositions[i];
+ // Apply update
+ poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
+ }
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses());
+ }
+ updateOdometryVision();
+
+ field.setRobotPose(getPose());
+ }
+
+ // DRIVE
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param xSpeed speed of the robot in the x direction (forward) in m/s
+ * @param ySpeed speed of the robot in the y direction (sideways) in m/s
+ * @param rot angular rate of the robot in rad/s
+ * @param fieldRelative whether the provided x and y speeds are relative to the field
+ * @param isOpenLoop whether to use velocity control for the drive motors
+ */
+ public void drive(
+ double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) {
+ // rot = headingControl(rot, xSpeed, ySpeed);
+ ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME);
+ if (fieldRelative) {
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+ }
+ setChassisSpeeds(speeds, isOpenLoop);
+ }
+
+ /**
+ * Drives the robot using the provided x speed, y speed, and positional heading.
+ *
+ * @param xSpeed speed of the robot in the x direction (forward)
+ * @param ySpeed speed of the robot in the y direction (sideways)
+ * @param heading target heading of the robot in radians
+ * @param fieldRelative whether the provided x and y speeds are relative to the field
+ */
+ public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
+ double rot = rotationController.calculate(getYaw().getRadians(), heading);
+ ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
+ if (fieldRelative) {
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
+ }
+ setChassisSpeeds(speeds, false);
+ }
+
+ /**
+ * Runs the PID controllers with the provided x, y, and rot values. Then, calls {@link
+ * #drive(double, double, double, boolean, boolean)} using the PID outputs. This is based on the
+ * odometry of the chassis.
+ *
+ * @param x the position to move to in the x, in meters
+ * @param y the position to move to in the y, in meters
+ * @param rot the angle to move to, in radians
+ */
+ public void driveWithPID(double x, double y, double rot) {
+ Pose2d pose = getPose();
+ double xSpeed = xController.calculate(pose.getX(), x);
+ double ySpeed = yController.calculate(pose.getY(), y);
+ double rotRadians = rotationController.calculate(pose.getRotation().getRadians(), rot);
+ drive(xSpeed, ySpeed, rotRadians, true, false);
+ }
+
+ /** Updates odometry using vision */
+ public void updateOdometryVision() {
+ // Start the timer if it hasn't started yet
+ visionEnableTimer.start();
+
+ // Update the swerve module poses
+ modulePoses.update();
+
+ if (modulePoses.slipped()) {
+ slipped = true;
+ }
+
+ Pose2d pose2 = getPose();
+
+ // Even if vision is disabled, it should still update inputs
+ // This prevents it from storing a lot of unread results, and it could be useful
+ // for replays
+ if (vision != null) {
+ vision.updateInputs();
+ }
+
+ if (VisionConstants.ENABLED) {
+ if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) {
+ vision.updateOdometry(
+ poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
+
+ if (vision.canSeeTag()) {
+ slipped = false;
+ modulePoses.reset();
+
+ double currentGyroYaw = gyroInputs.yawPosition.getRadians();
+
+ // to compare bias
+ ArrayList<EstimatedRobotPose> visionPoses = vision.getEstimatedPoses(getPose());
+
+ for (EstimatedRobotPose visionPose : visionPoses) {
+ if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) {
+ double visionYaw = visionPose.estimatedPose.getRotation().getZ();
+
+ // gets at vision timestamp, not current gyro yaw
+ double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds);
+
+ if (!Double.isNaN(gyroYawAtTimestamp)) {
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp));
+ Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw));
}
+ // use weighted observation
+ gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0);
+ }
}
- }
-
- Pose2d pose3 = getPose();
-
- // Reset the pose to a position on the field if it is too far off the field
- // This uses nearField() instead of onField() so we don't reset the odometry
- // when the wheels slip near the edge of the field
- // This is meant for poses that are caused by errors
- if (!Vision.nearField(prevPose)) {
- // If the pose at the beginning of the method is off the field, reset to a
- // position in the middle of the field
- // Use the rotation of the pose after updating odometry so the yaw is right
- prevPose = new Pose2d(FieldConstants.field.getFieldLength() / 2, FieldConstants.field.getFieldWidth() / 2,
- pose2.getRotation());
- resetOdometry(prevPose);
- } else if (!Vision.nearField(pose2)) {
- // if the drivetrain pose is off the field, reset our odometry to the pose
- // before(this is the right pose)
- // Keep the rotation from pose2 so yaw is correct for driver
- prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation());
- resetOdometry(prevPose);
- } else if (!Vision.nearField(pose3)) {
- // if our vision+drivetrain odometry isn't near the field, reset our odometry to
- // the pose before(this is the right pose)
- resetOdometry(pose2);
- prevPose = pose2;
- } else {
- // Set the previous pose to the current pose if we need to return to that
- prevPose = pose3;
- }
-
- // if (Robot.isSimulation()) {
- // pigeon.getSimState().addYaw(
- // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond
- // * Constants.LOOP_TIME));
- // }
- }
-
- /**
- * Stops all swerve modules.
- */
- public void stop() {
- Arrays.stream(modules).forEach(Module::stop);
- }
-
- // for current limit setting (brownout protection)
- public void applyNewModuleCurrents(
- double steerCurrentStator, double steerCurrentSupply,
- double driveCurrentStator, double driveCurrentSupply) {
- for (Module module : modules) { // iterate over our modules
- module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply);
- }
- }
-
- public double getSubsystemStatorCurrent() {
- double sum = 0;
- for (Module module : modules) {
- sum += module.getModuleStatorCurrent();
- }
- return sum;
- }
-
- public double getSubsystemSupplyCurrent() {
- double sum = 0;
- for (Module module : modules) {
- sum += module.getModuleSupplyCurrent();
- }
- return sum;
- }
-
- /**
- * Sets the desired states for all swerve modules.
- *
- * @param swerveModuleStates an array of module states to set swerve modules to.
- * Order of the array matters here!
- */
- public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
- // makes sure speeds of modules don't exceed maximum allowed
- SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED);
-
- for (int i = 0; i < 4; i++) {
- modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop);
- }
- }
-
- /**
- * Sets the chassis speeds of the robot.
- *
- * @param chassisSpeeds the target chassis speeds
- * @param isOpenLoop if open loop control should be used for the drive
- * velocity
- */
- public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) {
-
- if (DriveConstants.USE_ACTUAL_SPEED) {
- SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates());
- currentSetpoint = setpointGenerator.generateSetpoint(
- DriveConstants.MODULE_LIMITS,
- centerOfMassHeight,
- currentState, chassisSpeeds,
- Constants.LOOP_TIME);
- } else {
- currentSetpoint = setpointGenerator.generateSetpoint(
- DriveConstants.MODULE_LIMITS,
- centerOfMassHeight,
- currentSetpoint, chassisSpeeds,
- Constants.LOOP_TIME);
- }
-
- SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates();
- setModuleStates(swerveModuleStates, isOpenLoop);
- }
-
- public void setDriveVoltages(Voltage voltage) {
- for (int i = 0; i < modules.length; i++) {
- modules[i].setDriveVoltage(voltage);
- }
- }
-
- public void setAngleMotors(Rotation2d[] angles) {
- for (int i = 0; i < modules.length; i++) {
- modules[i].setAngle(angles[i]);
- }
- }
-
- /**
- * Returns the angular rate from the pigeon.
- *
- * @param id 0 for x, 1 for y, 2 for z
- * @return the rate in rads/s from the pigeon
- */
- public double getAngularRate(int id) {
- // double speed = 0;
- // switch(id){
- // case 0:
- // speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble();
- // break;
- // case 1:
- // speed = pigeon.getAngularVelocityYWorld().getValueAsDouble();
- // break;
- // case 2:
- // speed = pigeon.getAngularVelocityZWorld().getValueAsDouble();
- // break;
- // }
- // outputs in deg/s, so convert to rad/s
- return gyroInputs.yawVelocityRadPerSec;
- }
+ }
- /**
- * Updates and returns the array of SwerveModulePositions, which store the
- * distance travleled by the drive and the steer angle.
- *
- * @return An array of all swerve module positions
- */
- private SwerveModulePosition[] updateModulePositions() {
- return modulePositions = Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
- }
+ // check if we have enough samples
+ if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) {
+ double fullBias = gyroBiasEstimator.getAndResetBias();
+ double bias = gyroBiasEstimator.applyPartialCorrection(fullBias);
- /**
- * Gets an array of SwerveModulePositions, which store the distance travleled by
- * the drive and the steer angle.
- *
- * @return An array of all swerve module positions
- */
- public SwerveModulePosition[] getModulePositions() {
- return modulePositions;
- }
-
- /**
- * Enables or disables the state deadband for all swerve modules.
- * The state deadband determines if the robot will stop drive and steer motors
- * when inputted drive velocity is low.
- * It should be enabled for all regular driving, to prevent releasing the
- * controls from setting the angles.
- */
- public void setStateDeadband(boolean stateDeadBand) {
- Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand));
- }
-
- public void setOptimized(boolean optimized) {
- Arrays.stream(modules).forEach(module -> module.setOptimize(optimized));
- }
-
- public void setVisionEnabled(boolean enabled) {
- visionEnabled = enabled;
- }
-
- public void setIsAlign(boolean isAlign) {
- this.isAlign = isAlign;
- }
-
- public boolean getIsAlign() {
- return isAlign;
- }
-
- /**
- * Calculates chassis speed of drivetrain using the current SwerveModuleStates
- *
- * @return ChassisSpeeds object
- * This is often used as an input for other methods
- */
- public ChassisSpeeds getChassisSpeeds() {
- return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates());
- }
-
- /**
- * Gets the state of each module
- *
- * @return An array of 4 SwerveModuleStates
- */
- public SwerveModuleState[] getModuleStates() {
- return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
- }
-
- public SwerveSetpoint getCurrSetpoint() {
- return currentSetpoint;
- }
-
- /**
- * @return the yaw of the robot, aka heading, the direction it is facing
- */
- public Rotation2d getYaw() {
- return getPose().getRotation();
- }
-
- /**
- * @return an array of modules
- */
- public Module[] getModules() {
- return modules;
- }
-
- /**
- * gets gyro yaw at a specific timestamp with interpolation
- * this is used for timestamp-synchronized gyro/vision comparison.
- *
- * @param timestampSeconds the timestamp to get the gyro yaw at
- * @return the gyro yaw in radians, or Double.NaN if no valid data
- */
- private double
- getGyroYawAtTimestamp(double timestampSeconds) {
- return getPose().getRotation().getRadians();
- }
-
- /**
- * Resets the yaw of the robot.
- *
- * @param rotation the new yaw angle as Rotation2d
- */
- public void setYaw(Rotation2d rotation) {
- resetOdometry(new Pose2d(getPose().getTranslation(), rotation));
- }
-
- /**
- * Resets the odometry to the given pose.
- *
- * @param pose the pose to reset to.
- */
- public void resetOdometry(Pose2d pose) {
- // NOTE: must use pigeon yaw for odometer!
- currentHeading = pose.getRotation().getRadians();
- poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose);
- modulePoses.reset();
- }
-
- /**
- * @return the pose of the robot as estimated by the odometry
- */
- @AutoLogOutput(key = "Odometry/Robot")
- public Pose2d getPose() {
- return poseEstimator.getEstimatedPosition();
- }
-
- /**
- * Sets the angle to align to
- *
- * @param newAngle The new angle in radians, can be set to null
- */
- public void setAlignAngle(Double newAngle) {
- alignAngle = newAngle;
- }
-
- /**
- * Returns whether or not the robot is at the input align angle
- *
- * @return true if it within tolerance the align angle, false otherwise
- */
- public boolean atAlignAngle() {
- if (alignAngle == null) {
- return false;
- }
- double diff = Math.abs(alignAngle - getYaw().getRadians());
- return diff < DriveConstants.HEADING_TOLERANCE || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE;
- }
-
- /**
- * Gets the angle to align to
- *
- * @return The angle in radians
- */
- public double getAlignAngle() {
- if (alignAngle != null) {
- return alignAngle;
- }
- return 0;
- }
-
- /**
- * Sets vision to only use certain April tags
- *
- * @param ids An array of the tags to only use
- */
- public void onlyUseTags(int[] ids) {
- if (vision != null) {
- vision.onlyUse(ids);
- }
- }
-
- /**
- * Returns if vision has seen an April tag in the last frame
- *
- * @return true if vision saw a tag last frame or if vision is disabled
- */
- public boolean canSeeTag() {
- // if no vision system, then return true
- if (vision == null)
- return true;
-
- return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED;
- }
-
- /**
- * Gets the pose at a previous time
- *
- * @param timestamp The timestamp of the pose to get
- * @return The pose, null if there are no poses yet, or the current pose if
- * timestamp < 0
- */
- public Pose2d getPoseAt(double timestamp) {
- if (timestamp < 0) {
- return getPose();
- }
- Optional<Pose2d> pose = poseEstimator.sampleAt(timestamp);
- if (pose.isPresent()) {
- return pose.get();
- } else {
- return null;
- }
- }
-
- /**
- * Uses pigeon and rotational input to return a rotation that accounts for drift
- *
- * @return A rotation
- */
- public double headingControl(double rot, double xSpeed, double ySpeed) {
- if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)
- && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1))
- || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) {
- drive_turning = true;
- currentHeading = getYaw().getRadians();
- } else {
- drive_turning = false;
- }
- if (!drive_turning) {
- rotationController.setSetpoint(currentHeading);
- double output = rotationController.calculate(getYaw().getRadians());
- rot = Math.abs(output) > Math.abs(rot) ? output : rot;
+ if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) {
+ gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias));
+ }
+ }
}
- return rot;
- }
-
- /**
- * Resets the swerve modules from the absolute encoders
- */
- public void resetModulesToAbsolute() {
- Arrays.stream(modules).forEach(Module::resetToAbsolute);
- }
-
- // getters for the PID Controllers
- public PIDController getXController() {
- return xController;
- }
-
- public PIDController getYController() {
- return yController;
- }
-
- public PIDController getRotationController() {
- return rotationController;
- }
-
- /**
- * Set the desired pose to drive to
- * This will enable driver assist to go to the pose
- *
- * @param supplier The supplier for the desired pose, use ()->null to not use a
- * desired pose
- */
- public void setDesiredPose(Supplier<Pose2d> supplier) {
- desiredPoSupplier = supplier;
- }
-
- /**
- * Set the desired pose to drive to
- * This will enable driver assist to go to the pose
- *
- * @param pose The Pose2d to drive to
- */
- public void setDesiredPose(Pose2d pose) {
- setDesiredPose(() -> pose);
- }
-
- /**
- * Gets the current desired pose, or null if there is no desired pose
- *
- * @return The Pose2d if it exists, null otherwise
- */
- public Pose2d getDesiredPose() {
- return desiredPoSupplier.get();
- }
-
- public boolean atSetpoint() {
- Pose2d pose = getDesiredPose();
- return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025;
- }
-
- public SwerveModulePose getSwerveModulePose() {
- return modulePoses;
- }
-
- public double getAcceleration() {
- double accelX = gyroInputs.accelerationX;
- double accelY = gyroInputs.accelerationY;
-
- double angularVelocity = getAngularRate(3);
- double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME;
- previousAngularVelocity = angularVelocity;
-
- double pigeonOffsetX = 0.082677;
- double pigeonOffsetY = 0.030603444;
-
- double totalX = accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY;
- double totalY = accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX;
-
- return Math.hypot(totalX, totalY);
- }
-
- @AutoLogOutput(key = "Drivetrain/AccelerationFaults")
- public boolean accelerationOverMax() {
- return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL;
- }
-
- public void setCenterOfMass(double height) {
- centerOfMassHeight = height;
- }
-
- public void alignWheels() {
- SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0));
- setModuleStates(new SwerveModuleState[] {
- state, state, state, state
- }, false);
- }
+ }
+ }
+
+ Pose2d pose3 = getPose();
+
+ // Reset the pose to a position on the field if it is too far off the field
+ // This uses nearField() instead of onField() so we don't reset the odometry
+ // when the wheels slip near the edge of the field
+ // This is meant for poses that are caused by errors
+ if (!Vision.nearField(prevPose)) {
+ // If the pose at the beginning of the method is off the field, reset to a
+ // position in the middle of the field
+ // Use the rotation of the pose after updating odometry so the yaw is right
+ prevPose =
+ new Pose2d(
+ FieldConstants.field.getFieldLength() / 2,
+ FieldConstants.field.getFieldWidth() / 2,
+ pose2.getRotation());
+ resetOdometry(prevPose);
+ } else if (!Vision.nearField(pose2)) {
+ // if the drivetrain pose is off the field, reset our odometry to the pose
+ // before(this is the right pose)
+ // Keep the rotation from pose2 so yaw is correct for driver
+ prevPose = new Pose2d(prevPose.getTranslation(), pose2.getRotation());
+ resetOdometry(prevPose);
+ } else if (!Vision.nearField(pose3)) {
+ // if our vision+drivetrain odometry isn't near the field, reset our odometry to
+ // the pose before(this is the right pose)
+ resetOdometry(pose2);
+ prevPose = pose2;
+ } else {
+ // Set the previous pose to the current pose if we need to return to that
+ prevPose = pose3;
+ }
+
+ // if (Robot.isSimulation()) {
+ // pigeon.getSimState().addYaw(
+ // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond
+ // * Constants.LOOP_TIME));
+ // }
+ }
+
+ /** Stops all swerve modules. */
+ public void stop() {
+ Arrays.stream(modules).forEach(Module::stop);
+ }
+
+ // for current limit setting (brownout protection)
+ public void applyNewModuleCurrents(
+ double steerCurrentStator,
+ double steerCurrentSupply,
+ double driveCurrentStator,
+ double driveCurrentSupply) {
+ for (Module module : modules) { // iterate over our modules
+ module.setNewCurrentLimit(
+ steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply);
+ }
+ }
+
+ public double getSubsystemStatorCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleStatorCurrent();
+ }
+ return sum;
+ }
+
+ public double getSubsystemSupplyCurrent() {
+ double sum = 0;
+ for (Module module : modules) {
+ sum += module.getModuleSupplyCurrent();
+ }
+ return sum;
+ }
+
+ /**
+ * Sets the desired states for all swerve modules.
+ *
+ * @param swerveModuleStates an array of module states to set swerve modules to. Order of the
+ * array matters here!
+ */
+ public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
+ // makes sure speeds of modules don't exceed maximum allowed
+ SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED);
+
+ for (int i = 0; i < 4; i++) {
+ modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop);
+ }
+ }
+
+ /**
+ * Sets the chassis speeds of the robot.
+ *
+ * @param chassisSpeeds the target chassis speeds
+ * @param isOpenLoop if open loop control should be used for the drive velocity
+ */
+ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) {
+
+ if (DriveConstants.USE_ACTUAL_SPEED) {
+ SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates());
+ currentSetpoint =
+ setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ centerOfMassHeight,
+ currentState,
+ chassisSpeeds,
+ Constants.LOOP_TIME);
+ } else {
+ currentSetpoint =
+ setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ centerOfMassHeight,
+ currentSetpoint,
+ chassisSpeeds,
+ Constants.LOOP_TIME);
+ }
+
+ SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates();
+ setModuleStates(swerveModuleStates, isOpenLoop);
+ }
+
+ public void setDriveVoltages(Voltage voltage) {
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].setDriveVoltage(voltage);
+ }
+ }
+
+ public void setAngleMotors(Rotation2d[] angles) {
+ for (int i = 0; i < modules.length; i++) {
+ modules[i].setAngle(angles[i]);
+ }
+ }
+
+ /**
+ * Returns the angular rate from the pigeon.
+ *
+ * @param id 0 for x, 1 for y, 2 for z
+ * @return the rate in rads/s from the pigeon
+ */
+ public double getAngularRate(int id) {
+ // double speed = 0;
+ // switch(id){
+ // case 0:
+ // speed = gyroInputs..getAngularVelocityXWorld().getValueAsDouble();
+ // break;
+ // case 1:
+ // speed = pigeon.getAngularVelocityYWorld().getValueAsDouble();
+ // break;
+ // case 2:
+ // speed = pigeon.getAngularVelocityZWorld().getValueAsDouble();
+ // break;
+ // }
+ // outputs in deg/s, so convert to rad/s
+ return gyroInputs.yawVelocityRadPerSec;
+ }
+
+ /**
+ * Updates and returns the array of SwerveModulePositions, which store the distance travleled by
+ * the drive and the steer angle.
+ *
+ * @return An array of all swerve module positions
+ */
+ private SwerveModulePosition[] updateModulePositions() {
+ return modulePositions =
+ Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
+ }
+
+ /**
+ * Gets an array of SwerveModulePositions, which store the distance travleled by the drive and the
+ * steer angle.
+ *
+ * @return An array of all swerve module positions
+ */
+ public SwerveModulePosition[] getModulePositions() {
+ return modulePositions;
+ }
+
+ /**
+ * Enables or disables the state deadband for all swerve modules. The state deadband determines if
+ * the robot will stop drive and steer motors when inputted drive velocity is low. It should be
+ * enabled for all regular driving, to prevent releasing the controls from setting the angles.
+ */
+ public void setStateDeadband(boolean stateDeadBand) {
+ Arrays.stream(modules).forEach(module -> module.setStateDeadband(stateDeadBand));
+ }
+
+ public void setOptimized(boolean optimized) {
+ Arrays.stream(modules).forEach(module -> module.setOptimize(optimized));
+ }
+
+ public void setVisionEnabled(boolean enabled) {
+ visionEnabled = enabled;
+ }
+
+ public void setIsAlign(boolean isAlign) {
+ this.isAlign = isAlign;
+ }
+
+ public boolean getIsAlign() {
+ return isAlign;
+ }
+
+ /**
+ * Calculates chassis speed of drivetrain using the current SwerveModuleStates
+ *
+ * @return ChassisSpeeds object This is often used as an input for other methods
+ */
+ public ChassisSpeeds getChassisSpeeds() {
+ return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates());
+ }
+
+ /**
+ * Gets the state of each module
+ *
+ * @return An array of 4 SwerveModuleStates
+ */
+ public SwerveModuleState[] getModuleStates() {
+ return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new);
+ }
+
+ public SwerveSetpoint getCurrSetpoint() {
+ return currentSetpoint;
+ }
+
+ /**
+ * @return the yaw of the robot, aka heading, the direction it is facing
+ */
+ public Rotation2d getYaw() {
+ return getPose().getRotation();
+ }
+
+ /**
+ * @return an array of modules
+ */
+ public Module[] getModules() {
+ return modules;
+ }
+
+ /**
+ * gets gyro yaw at a specific timestamp with interpolation this is used for
+ * timestamp-synchronized gyro/vision comparison.
+ *
+ * @param timestampSeconds the timestamp to get the gyro yaw at
+ * @return the gyro yaw in radians, or Double.NaN if no valid data
+ */
+ private double getGyroYawAtTimestamp(double timestampSeconds) {
+ return getPose().getRotation().getRadians();
+ }
+
+ /**
+ * Resets the yaw of the robot.
+ *
+ * @param rotation the new yaw angle as Rotation2d
+ */
+ public void setYaw(Rotation2d rotation) {
+ resetOdometry(new Pose2d(getPose().getTranslation(), rotation));
+ }
+
+ /**
+ * Resets the odometry to the given pose.
+ *
+ * @param pose the pose to reset to.
+ */
+ public void resetOdometry(Pose2d pose) {
+ // NOTE: must use pigeon yaw for odometer!
+ currentHeading = pose.getRotation().getRadians();
+ poseEstimator.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose);
+ modulePoses.reset();
+ }
+
+ /**
+ * @return the pose of the robot as estimated by the odometry
+ */
+ @AutoLogOutput(key = "Odometry/Robot")
+ public Pose2d getPose() {
+ return poseEstimator.getEstimatedPosition();
+ }
+
+ /**
+ * Sets the angle to align to
+ *
+ * @param newAngle The new angle in radians, can be set to null
+ */
+ public void setAlignAngle(Double newAngle) {
+ alignAngle = newAngle;
+ }
+
+ /**
+ * Returns whether or not the robot is at the input align angle
+ *
+ * @return true if it within tolerance the align angle, false otherwise
+ */
+ public boolean atAlignAngle() {
+ if (alignAngle == null) {
+ return false;
+ }
+ double diff = Math.abs(alignAngle - getYaw().getRadians());
+ return diff < DriveConstants.HEADING_TOLERANCE
+ || diff > 2 * Math.PI - DriveConstants.HEADING_TOLERANCE;
+ }
+
+ /**
+ * Gets the angle to align to
+ *
+ * @return The angle in radians
+ */
+ public double getAlignAngle() {
+ if (alignAngle != null) {
+ return alignAngle;
+ }
+ return 0;
+ }
+
+ /**
+ * Sets vision to only use certain April tags
+ *
+ * @param ids An array of the tags to only use
+ */
+ public void onlyUseTags(int[] ids) {
+ if (vision != null) {
+ vision.onlyUse(ids);
+ }
+ }
+
+ /**
+ * Returns if vision has seen an April tag in the last frame
+ *
+ * @return true if vision saw a tag last frame or if vision is disabled
+ */
+ public boolean canSeeTag() {
+ // if no vision system, then return true
+ if (vision == null) return true;
+
+ return vision.canSeeTag() || !visionEnabled || !VisionConstants.ENABLED;
+ }
+
+ /**
+ * Gets the pose at a previous time
+ *
+ * @param timestamp The timestamp of the pose to get
+ * @return The pose, null if there are no poses yet, or the current pose if timestamp < 0
+ */
+ public Pose2d getPoseAt(double timestamp) {
+ if (timestamp < 0) {
+ return getPose();
+ }
+ Optional<Pose2d> pose = poseEstimator.sampleAt(timestamp);
+ if (pose.isPresent()) {
+ return pose.get();
+ } else {
+ return null;
+ }
+ }
+
+ /**
+ * Uses pigeon and rotational input to return a rotation that accounts for drift
+ *
+ * @return A rotation
+ */
+ public double headingControl(double rot, double xSpeed, double ySpeed) {
+ if ((!EqualsUtil.epsilonEquals(getAngularRate(0), 0, 0.0004)
+ && EqualsUtil.epsilonEquals(Math.hypot(xSpeed, ySpeed), 0, 0.1))
+ || !EqualsUtil.epsilonEquals(rot, 0, 0.0004)) {
+ drive_turning = true;
+ currentHeading = getYaw().getRadians();
+ } else {
+ drive_turning = false;
+ }
+ if (!drive_turning) {
+ rotationController.setSetpoint(currentHeading);
+ double output = rotationController.calculate(getYaw().getRadians());
+ rot = Math.abs(output) > Math.abs(rot) ? output : rot;
+ }
+ return rot;
+ }
+
+ /** Resets the swerve modules from the absolute encoders */
+ public void resetModulesToAbsolute() {
+ Arrays.stream(modules).forEach(Module::resetToAbsolute);
+ }
+
+ // getters for the PID Controllers
+ public PIDController getXController() {
+ return xController;
+ }
+
+ public PIDController getYController() {
+ return yController;
+ }
+
+ public PIDController getRotationController() {
+ return rotationController;
+ }
+
+ /**
+ * Set the desired pose to drive to This will enable driver assist to go to the pose
+ *
+ * @param supplier The supplier for the desired pose, use ()->null to not use a desired pose
+ */
+ public void setDesiredPose(Supplier<Pose2d> supplier) {
+ desiredPoSupplier = supplier;
+ }
+
+ /**
+ * Set the desired pose to drive to This will enable driver assist to go to the pose
+ *
+ * @param pose The Pose2d to drive to
+ */
+ public void setDesiredPose(Pose2d pose) {
+ setDesiredPose(() -> pose);
+ }
+
+ /**
+ * Gets the current desired pose, or null if there is no desired pose
+ *
+ * @return The Pose2d if it exists, null otherwise
+ */
+ public Pose2d getDesiredPose() {
+ return desiredPoSupplier.get();
+ }
+
+ public boolean atSetpoint() {
+ Pose2d pose = getDesiredPose();
+ return pose != null && getPose().getTranslation().getDistance(pose.getTranslation()) < 0.025;
+ }
+
+ public SwerveModulePose getSwerveModulePose() {
+ return modulePoses;
+ }
+
+ public double getAcceleration() {
+ double accelX = gyroInputs.accelerationX;
+ double accelY = gyroInputs.accelerationY;
+
+ double angularVelocity = getAngularRate(3);
+ double angularAccel = (angularVelocity - previousAngularVelocity) / Constants.LOOP_TIME;
+ previousAngularVelocity = angularVelocity;
+
+ double pigeonOffsetX = 0.082677;
+ double pigeonOffsetY = 0.030603444;
+
+ double totalX =
+ accelX + Math.pow(angularVelocity, 2) * pigeonOffsetX + angularAccel * pigeonOffsetY;
+ double totalY =
+ accelY + Math.pow(angularVelocity, 2) * pigeonOffsetY - angularAccel * pigeonOffsetX;
+
+ return Math.hypot(totalX, totalY);
+ }
+
+ @AutoLogOutput(key = "Drivetrain/AccelerationFaults")
+ public boolean accelerationOverMax() {
+ return getAcceleration() > DriveConstants.MAX_LINEAR_ACCEL;
+ }
+
+ public void setCenterOfMass(double height) {
+ centerOfMassHeight = height;
+ }
+
+ public void alignWheels() {
+ SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0));
+ setModuleStates(new SwerveModuleState[] {state, state, state, state}, false);
+ }
}
/** IO implementation for Pigeon 2. */
public class GyroIOPigeon2 implements GyroIO {
- private final Pigeon2 pigeon =
- new Pigeon2(
- IdConstants.PIGEON,
- DriveConstants.PIGEON_CAN);
+ private final Pigeon2 pigeon = new Pigeon2(IdConstants.PIGEON, DriveConstants.PIGEON_CAN);
private final StatusSignal<Angle> yaw = pigeon.getYaw();
private final StatusSignal<LinearAcceleration> accelrationx = pigeon.getAccelerationX();
private final StatusSignal<LinearAcceleration> accelrationy = pigeon.getAccelerationY();
private final Queue<Double> yawPositionQueue;
private final Queue<Double> yawTimestampQueue;
private final StatusSignal<AngularVelocity> yawVelocity = pigeon.getAngularVelocityZWorld();
- private final Pigeon2Configuration config = new Pigeon2Configuration();
+ private final Pigeon2Configuration config = new Pigeon2Configuration();
public GyroIOPigeon2() {
- config.MountPose.MountPoseRoll = DriveConstants.GYRO_MOUNT_POSE_ROLL;
+ config.MountPose.MountPoseRoll = DriveConstants.GYRO_MOUNT_POSE_ROLL;
pigeon.getConfigurator().apply(config);
pigeon.getConfigurator().setYaw(0.0);
yaw.setUpdateFrequency(250);
@Override
public void updateInputs(GyroIOInputs inputs) {
- inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity, accelrationx, accelrationy).equals(StatusCode.OK);
+ inputs.connected =
+ BaseStatusSignal.refreshAll(yaw, yawVelocity, accelrationx, accelrationy)
+ .equals(StatusCode.OK);
inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
inputs.accelerationX = accelrationx.getValueAsDouble();
public StatusSignal<Angle> getYawSignal() {
return yaw;
}
-
+
@Override
public void setYaw(Rotation2d rotation) {
pigeon.getConfigurator().setYaw(rotation.getDegrees());
}
-}
\ No newline at end of file
+}
import frc.robot.util.PhoenixOdometryThread;
import lib.CTREModuleState;
+public class Module implements ModuleIO {
+ private final ModuleType type;
-public class Module implements ModuleIO{
- private final ModuleType type;
-
- // Degrees
- private final double angleOffset;
-
- private final TalonFX angleMotor;
- private final TalonFX driveMotor;
- private final CANcoder CANcoder;
- private SwerveModuleState desiredState;
-
- protected boolean stateDeadband = true;
-
- private boolean optimizeStates = true;
-
- // Inputs from drive motor
- private final StatusSignal<Angle> drivePosition;
- private final StatusSignal<AngularVelocity> driveVelocity;
- private final StatusSignal<Voltage> driveAppliedVolts;
- private final StatusSignal<Current> driveCurrent;
-
- // Inputs from turn motor
- private final StatusSignal<Angle> turnAbsolutePosition;
- private final StatusSignal<Angle> turnPosition;
- private final StatusSignal<AngularVelocity> turnVelocity;
- private final StatusSignal<Voltage> turnAppliedVolts;
- private final StatusSignal<Current> turnCurrent;
-
- // Timestamp inputs from Phoenix thread
- protected final Queue<Double> timestampQueue;
- protected final Queue<Double> drivePositionQueue;
- protected final Queue<Double> turnPositionQueue;
-
- private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
-
- // Connection debouncers
- private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
- private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
- private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
-
- private final Alert driveDisconnectedAlert;
- private final Alert turnDisconnectedAlert;
- private final Alert turnEncoderDisconnectedAlert;
-
- protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
-
- private ModuleConstants moduleConstants;
- private final MotionMagicVelocityVoltage velocityRequest =
- new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
-
-
- public Module(ModuleConstants moduleConstants) {
- this.moduleConstants = moduleConstants;
-
- type = moduleConstants.getType();
- angleOffset = moduleConstants.getSteerOffset();
-
- /* Angle Encoder Config */
- CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
- /* Angle Motor Config */
- angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
- driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
- // Create drive status signals
- drivePosition = driveMotor.getPosition();
- driveVelocity = driveMotor.getVelocity();
- driveAppliedVolts = driveMotor.getMotorVoltage();
- driveCurrent = driveMotor.getStatorCurrent();
-
- // Create turn status signals
- turnAbsolutePosition = CANcoder.getAbsolutePosition();
- turnPosition = angleMotor.getPosition();
- turnVelocity = angleMotor.getVelocity();
- turnAppliedVolts = angleMotor.getMotorVoltage();
- turnCurrent = angleMotor.getStatorCurrent();
-
- // Create timestamp queue
- timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
- drivePositionQueue =
- PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
- turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
- updateInputs();
-
- configCANcoder();
- configAngleMotor();
- configDriveMotor();
-
- driveDisconnectedAlert =
- new Alert(
- "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
- AlertType.kError);
- turnDisconnectedAlert =
- new Alert(
- "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
- turnEncoderDisconnectedAlert =
- new Alert(
- "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
- AlertType.kError);
-
-
- // Configure periodic frames
- BaseStatusSignal.setUpdateFrequencyForAll(
- 250, drivePosition, turnPosition);
- BaseStatusSignal.setUpdateFrequencyForAll(
- 50.0,
- driveVelocity,
- driveAppliedVolts,
- driveCurrent,
- turnAbsolutePosition,
- turnVelocity,
- turnAppliedVolts,
- turnCurrent);
-
- setDesiredState(new SwerveModuleState(0, getAngle()), false);
- }
-
- public void close() {
- angleMotor.close();
- driveMotor.close();
- CANcoder.close();
- }
-
- @Override
- public void updateInputs() {
- // Refresh all signals
- var driveStatus =
- BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
- var turnStatus =
- BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
- var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
-
- // Update drive inputs
- inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
- inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
- inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
- inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
- inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
-
- // Update turn inputs
- inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
- inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
- inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
- inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
- inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
-
- // Update encoder inputs
- inputs.encoderOffset = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble());
-
- // Update odometry inputs
- inputs.odometryTimestamps =
- timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
- inputs.odometryDrivePositionsRad =
- drivePositionQueue.stream()
- .mapToDouble((Double value) -> Units.rotationsToRadians(value))
- .toArray();
- inputs.odometryTurnPositions =
- turnPositionQueue.stream()
- .map((Double value) -> Rotation2d.fromRotations(value))
- .toArray(Rotation2d[]::new);
- timestampQueue.clear();
- drivePositionQueue.clear();
- turnPositionQueue.clear();
-
- inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble();
- inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble();
- inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble();
- inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble();
- }
-
- public void periodic() {
- updateInputs();
- Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
-
- // Calculate positions for odometry
- int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
- odometryPositions = new SwerveModulePosition[sampleCount];
- for (int i = 0; i < sampleCount; i++) {
- double positionMeters = inputs.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS;
- Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
- }
- // Update alerts
- driveDisconnectedAlert.set(!inputs.driveConnected);
- turnDisconnectedAlert.set(!inputs.turnConnected);
- turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
- }
- }
-
- public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
- // Separate if here and in setAngle() to avoid warning
- if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
- /*
- * This is a custom optimize function, since default WPILib optimize assumes
- * continuous controller which CTRE and Rev onboard is not
- */
- desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
- }else{
- desiredState = wantedState;
- }
- setAngle();
- setSpeed(isOpenLoop);
- }
-
- public void setSpeed(boolean isOpenLoop) {
- if(desiredState == null){
- return;
- }
- if (isOpenLoop) {
- double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
- driveMotor.setControl(new DutyCycleOut(percentOutput));
- } else {
- double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO;
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
- }
-
- double feedforward = velocity * moduleConstants.getDriveV();
- driveMotor.setControl(
- velocityRequest
- .withVelocity(velocity)
- .withFeedForward(feedforward));
- }
- }
-
- private void setAngle() {
- if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
- // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement.
- if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
- stop();
- return;
- }
- }
- if(desiredState == null){
- return;
- }
- angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
- }
-
- public void setDriveVoltage(Voltage voltage){
- driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
- }
- public void setAngle(Rotation2d angle){
- angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
- }
-
- public void setOptimize(boolean enable) {
- optimizeStates = enable;
- }
-
- public byte getModuleIndex() {
- return type.id;
- }
-
- public Rotation2d getAngle() {
- return inputs.turnPosition;
- }
-
- public Rotation2d getCANcoder() {
- return inputs.turnAbsolutePosition;
- }
-
- public void resetToAbsolute() {
- // Sensor ticks
- double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
- angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- }
-
- private void configCANcoder() {
- CANcoder.getConfigurator().apply(new CANcoderConfiguration());
- CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).
- withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive));
- }
+ // Degrees
+ private final double angleOffset;
- private void configAngleMotor() {
- angleMotor.getConfigurator().apply(new TalonFXConfiguration());
-
- CurrentLimitsConfigs config = new CurrentLimitsConfigs();
- config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
- config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
- config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
- config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(config);
- angleMotor.getConfigurator().apply(new Slot0Configs()
- .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
- .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
- .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
- angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
- angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
- angleMotor.setPosition(0);
-
- // optimize bus utilization for angle motor
- angleMotor.optimizeBusUtilization();
-
- resetToAbsolute();
- }
+ private final TalonFX angleMotor;
+ private final TalonFX driveMotor;
+ private final CANcoder CANcoder;
+ private SwerveModuleState desiredState;
- /**
- * @return Speed in RPM
- */
- public double getDriveVelocity() {
- return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI;
- }
+ protected boolean stateDeadband = true;
- public double getDriveVoltage(){
- return inputs.driveAppliedVolts;
- }
+ private boolean optimizeStates = true;
- public double getDriveStatorCurrent(){
- return inputs.driveCurrentAmps;
- }
+ // Inputs from drive motor
+ private final StatusSignal<Angle> drivePosition;
+ private final StatusSignal<AngularVelocity> driveVelocity;
+ private final StatusSignal<Voltage> driveAppliedVolts;
+ private final StatusSignal<Current> driveCurrent;
- public double getModuleStatorCurrent() {
- return inputs.steerStator + inputs.driveStator;
- }
+ // Inputs from turn motor
+ private final StatusSignal<Angle> turnAbsolutePosition;
+ private final StatusSignal<Angle> turnPosition;
+ private final StatusSignal<AngularVelocity> turnVelocity;
+ private final StatusSignal<Voltage> turnAppliedVolts;
+ private final StatusSignal<Current> turnCurrent;
- public double getModuleSupplyCurrent() {
- return inputs.steerSupply + inputs.driveSupply;
- }
+ // Timestamp inputs from Phoenix thread
+ protected final Queue<Double> timestampQueue;
+ protected final Queue<Double> drivePositionQueue;
+ protected final Queue<Double> turnPositionQueue;
- // I took the config things straight from this file
- public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator, double currentDriveSupply) {
- CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
- // steer
- steerConfig.SupplyCurrentLimitEnable = true;
- steerConfig.StatorCurrentLimitEnable = true;
- steerConfig.StatorCurrentLimit = currentSteerSupply;
- steerConfig.SupplyCurrentLimit = currentSteerSupply;
- steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(steerConfig); // apply
-
- // drive
- CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
- driveConfig.SupplyCurrentLimitEnable = true;
- driveConfig.StatorCurrentLimitEnable = true;
- driveConfig.SupplyCurrentLimit = currentDriveSupply;
- driveConfig.StatorCurrentLimit = currentDriveStator;
- driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- driveMotor.getConfigurator().apply(driveConfig); // apply
- }
+ private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
- private void configDriveMotor() {
- var talonFXConfigs = new TalonFXConfiguration();
- // set Motion Magic settings
- var motionMagicConfigs = talonFXConfigs.MotionMagic;
- motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
- motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
- var slot0Configs = talonFXConfigs.Slot0;
- slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
- slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
- slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
- slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
- slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
- slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
- driveMotor.getConfigurator().apply(talonFXConfigs);
- CurrentLimitsConfigs config = new CurrentLimitsConfigs();
- config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
- config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
- config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
- config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveMotor.getConfigurator().apply(config);
- driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
- driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
- driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
- driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
-
- // optimize bus utilization for drive motor
- driveMotor.optimizeBusUtilization();
-
- }
+ // Connection debouncers
+ private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
- public SwerveModuleState getState() {
- return new SwerveModuleState(
- inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS,
- getAngle());
- }
+ private final Alert driveDisconnectedAlert;
+ private final Alert turnDisconnectedAlert;
+ private final Alert turnEncoderDisconnectedAlert;
- public SwerveModulePosition getPosition() {
- return new SwerveModulePosition(
- inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS,
- getAngle());
- }
+ protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
- public SwerveModuleState getDesiredState() {
- return desiredState;
- }
-
-
- public double getDriveVelocityError() {
- return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
- }
+ private ModuleConstants moduleConstants;
+ private final MotionMagicVelocityVoltage velocityRequest =
+ new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
- public void stop() {
- driveMotor.set(0);
- angleMotor.set(0);
- }
+ public Module(ModuleConstants moduleConstants) {
+ this.moduleConstants = moduleConstants;
+
+ type = moduleConstants.getType();
+ angleOffset = moduleConstants.getSteerOffset();
+
+ /* Angle Encoder Config */
+ CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
+ /* Angle Motor Config */
+ angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
+ driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
+ // Create drive status signals
+ drivePosition = driveMotor.getPosition();
+ driveVelocity = driveMotor.getVelocity();
+ driveAppliedVolts = driveMotor.getMotorVoltage();
+ driveCurrent = driveMotor.getStatorCurrent();
+
+ // Create turn status signals
+ turnAbsolutePosition = CANcoder.getAbsolutePosition();
+ turnPosition = angleMotor.getPosition();
+ turnVelocity = angleMotor.getVelocity();
+ turnAppliedVolts = angleMotor.getMotorVoltage();
+ turnCurrent = angleMotor.getStatorCurrent();
+
+ // Create timestamp queue
+ timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+ drivePositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
+ turnPositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
+ updateInputs();
- public ModuleType getModuleType(){
- return type;
- }
+ configCANcoder();
+ configAngleMotor();
+ configDriveMotor();
- public void setStateDeadband(boolean enabled) {
- stateDeadband = enabled;
- }
+ driveDisconnectedAlert =
+ new Alert(
+ "Disconnected drive motor on module "
+ + Integer.toString(moduleConstants.ordinal())
+ + ".",
+ AlertType.kError);
+ turnDisconnectedAlert =
+ new Alert(
+ "Disconnected turn motor on module "
+ + Integer.toString(moduleConstants.ordinal())
+ + ".",
+ AlertType.kError);
+ turnEncoderDisconnectedAlert =
+ new Alert(
+ "Disconnected turn encoder on module "
+ + Integer.toString(moduleConstants.ordinal())
+ + ".",
+ AlertType.kError);
- public double getDesiredVelocity() {
- return getDesiredState().speedMetersPerSecond;
+ // Configure periodic frames
+ BaseStatusSignal.setUpdateFrequencyForAll(250, drivePosition, turnPosition);
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent);
+
+ setDesiredState(new SwerveModuleState(0, getAngle()), false);
+ }
+
+ public void close() {
+ angleMotor.close();
+ driveMotor.close();
+ CANcoder.close();
+ }
+
+ @Override
+ public void updateInputs() {
+ // Refresh all signals
+ var driveStatus =
+ BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+ var turnStatus =
+ BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+ var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+
+ // Update drive inputs
+ inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
+ inputs.drivePositionRad =
+ Units.rotationsToRadians(
+ drivePosition.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveVelocityRadPerSec =
+ Units.rotationsToRadians(
+ driveVelocity.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
+
+ // Update turn inputs
+ inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
+ inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
+ inputs.turnPosition =
+ Rotation2d.fromRotations(
+ turnPosition.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnVelocityRadPerSec =
+ Units.rotationsToRadians(
+ turnVelocity.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+ inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
+
+ // Update encoder inputs
+ inputs.encoderOffset =
+ Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble());
+
+ // Update odometry inputs
+ inputs.odometryTimestamps =
+ timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRad =
+ drivePositionQueue.stream()
+ .mapToDouble((Double value) -> Units.rotationsToRadians(value))
+ .toArray();
+ inputs.odometryTurnPositions =
+ turnPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromRotations(value))
+ .toArray(Rotation2d[]::new);
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+
+ inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble();
+ inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble();
+ inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble();
+ inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble();
+ }
+
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+
+ // Calculate positions for odometry
+ int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ odometryPositions = new SwerveModulePosition[sampleCount];
+ for (int i = 0; i < sampleCount; i++) {
+ double positionMeters =
+ inputs.odometryDrivePositionsRad[i]
+ / DriveConstants.DRIVE_GEAR_RATIO
+ * DriveConstants.WHEEL_RADIUS;
+ Rotation2d angle =
+ inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+ }
+ // Update alerts
+ driveDisconnectedAlert.set(!inputs.driveConnected);
+ turnDisconnectedAlert.set(!inputs.turnConnected);
+ turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput(
+ "Angle " + moduleConstants.ordinal(),
+ MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+ }
+ }
+
+ public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
+ // Separate if here and in setAngle() to avoid warning
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ /*
+ * This is a custom optimize function, since default WPILib optimize assumes
+ * continuous controller which CTRE and Rev onboard is not
+ */
+ desiredState =
+ optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
+ } else {
+ desiredState = wantedState;
+ }
+ setAngle();
+ setSpeed(isOpenLoop);
+ }
+
+ public void setSpeed(boolean isOpenLoop) {
+ if (desiredState == null) {
+ return;
+ }
+ if (isOpenLoop) {
+ double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
+ driveMotor.setControl(new DutyCycleOut(percentOutput));
+ } else {
+ double velocity =
+ desiredState.speedMetersPerSecond
+ / DriveConstants.WHEEL_RADIUS
+ / 2
+ / Math.PI
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
}
-
- public Rotation2d getDesiredAngle() {
- return getDesiredState().angle;
- }
- /** Returns the module positions received this cycle. */
- public SwerveModulePosition[] getOdometryPositions() {
- return odometryPositions;
+ double feedforward = velocity * moduleConstants.getDriveV();
+ driveMotor.setControl(velocityRequest.withVelocity(velocity).withFeedForward(feedforward));
}
+ }
- /** Returns the timestamps of the samples received this cycle. */
- public double[] getOdometryTimestamps() {
- return inputs.odometryTimestamps;
- }
-
- /** returns the drive position status signal for time-synced odometry. */
- public StatusSignal<Angle> getDrivePositionSignal() {
- return drivePosition;
- }
-
- /** returns the turn position status signal for time-synced odometry. */
- public StatusSignal<Angle> getTurnPositionSignal() {
- return turnPosition;
- }
-
- /** returns the turn absolute position status signal for time-synced odometry. */
- public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
- return turnAbsolutePosition;
- }
-
- public TalonFX[] getMotors() {
- return new TalonFX[]{angleMotor, driveMotor};
+ private void setAngle() {
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement.
+ if (stateDeadband
+ && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
+ stop();
+ return;
+ }
}
+ if (desiredState == null) {
+ return;
+ }
+ angleMotor.setControl(
+ new PositionDutyCycle(
+ desiredState.angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setDriveVoltage(Voltage voltage) {
+ driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
+ }
+
+ public void setAngle(Rotation2d angle) {
+ angleMotor.setControl(
+ new PositionDutyCycle(
+ angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setOptimize(boolean enable) {
+ optimizeStates = enable;
+ }
+
+ public byte getModuleIndex() {
+ return type.id;
+ }
+
+ public Rotation2d getAngle() {
+ return inputs.turnPosition;
+ }
+
+ public Rotation2d getCANcoder() {
+ return inputs.turnAbsolutePosition;
+ }
+
+ public void resetToAbsolute() {
+ // Sensor ticks
+ double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
+ angleMotor.setPosition(absolutePosition * DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ }
+
+ private void configCANcoder() {
+ CANcoder.getConfigurator().apply(new CANcoderConfiguration());
+ CANcoder.getConfigurator()
+ .apply(
+ new MagnetSensorConfigs()
+ .withAbsoluteSensorDiscontinuityPoint(1)
+ .withSensorDirection(
+ DriveConstants.MODULE_CONSTANTS.canCoderInvert
+ ? SensorDirectionValue.Clockwise_Positive
+ : SensorDirectionValue.CounterClockwise_Positive));
+ }
+
+ private void configAngleMotor() {
+ angleMotor.getConfigurator().apply(new TalonFXConfiguration());
+
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(config);
+ angleMotor
+ .getConfigurator()
+ .apply(
+ new Slot0Configs()
+ .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
+ .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
+ .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
+ angleMotor
+ .getConfigurator()
+ .apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
+ angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
+ angleMotor.setPosition(0);
+
+ // optimize bus utilization for angle motor
+ angleMotor.optimizeBusUtilization();
+
+ resetToAbsolute();
+ }
+
+ /**
+ * @return Speed in RPM
+ */
+ public double getDriveVelocity() {
+ return inputs.driveVelocityRadPerSec
+ * 60
+ / DriveConstants.MODULE_CONSTANTS.driveGearRatio
+ / 2
+ / Math.PI;
+ }
+
+ public double getDriveVoltage() {
+ return inputs.driveAppliedVolts;
+ }
+
+ public double getDriveStatorCurrent() {
+ return inputs.driveCurrentAmps;
+ }
+
+ public double getModuleStatorCurrent() {
+ return inputs.steerStator + inputs.driveStator;
+ }
+
+ public double getModuleSupplyCurrent() {
+ return inputs.steerSupply + inputs.driveSupply;
+ }
+
+ // I took the config things straight from this file
+ public void setNewCurrentLimit(
+ double currentSteerStator,
+ double currentSteerSupply,
+ double currentDriveStator,
+ double currentDriveSupply) {
+ CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
+ // steer
+ steerConfig.SupplyCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimitEnable = true;
+ steerConfig.StatorCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLimit = currentSteerSupply;
+ steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(steerConfig); // apply
+
+ // drive
+ CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
+ driveConfig.SupplyCurrentLimitEnable = true;
+ driveConfig.StatorCurrentLimitEnable = true;
+ driveConfig.SupplyCurrentLimit = currentDriveSupply;
+ driveConfig.StatorCurrentLimit = currentDriveStator;
+ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ driveMotor.getConfigurator().apply(driveConfig); // apply
+ }
+
+ private void configDriveMotor() {
+ var talonFXConfigs = new TalonFXConfiguration();
+ // set Motion Magic settings
+ var motionMagicConfigs = talonFXConfigs.MotionMagic;
+ motionMagicConfigs.MotionMagicCruiseVelocity =
+ DriveConstants.MAX_SPEED
+ / DriveConstants.WHEEL_CIRCUMFERENCE
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ motionMagicConfigs.MotionMagicAcceleration =
+ DriveConstants.MAX_DRIVE_ACCEL
+ / DriveConstants.WHEEL_CIRCUMFERENCE
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ var slot0Configs = talonFXConfigs.Slot0;
+ slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
+ slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
+ slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
+ slot0Configs.kP =
+ moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
+ slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
+ slot0Configs.kD =
+ moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
+ driveMotor.getConfigurator().apply(talonFXConfigs);
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(config);
+ driveMotor
+ .getConfigurator()
+ .apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
+ driveMotor
+ .getConfigurator()
+ .apply(
+ new OpenLoopRampsConfigs()
+ .withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
+ driveMotor
+ .getConfigurator()
+ .apply(
+ new ClosedLoopRampsConfigs()
+ .withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
+ driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
+
+ // optimize bus utilization for drive motor
+ driveMotor.optimizeBusUtilization();
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(
+ inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS, getAngle());
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS, getAngle());
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+ public double getDriveVelocityError() {
+ return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
+ }
+
+ public void stop() {
+ driveMotor.set(0);
+ angleMotor.set(0);
+ }
+
+ public ModuleType getModuleType() {
+ return type;
+ }
+
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
+
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
+ }
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /** Returns the module positions received this cycle. */
+ public SwerveModulePosition[] getOdometryPositions() {
+ return odometryPositions;
+ }
+
+ /** Returns the timestamps of the samples received this cycle. */
+ public double[] getOdometryTimestamps() {
+ return inputs.odometryTimestamps;
+ }
+
+ /** returns the drive position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getDrivePositionSignal() {
+ return drivePosition;
+ }
+
+ /** returns the turn position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnPositionSignal() {
+ return turnPosition;
+ }
+
+ /** returns the turn absolute position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
+ return turnAbsolutePosition;
+ }
+
+ public TalonFX[] getMotors() {
+ return new TalonFX[] {angleMotor, driveMotor};
+ }
}
/** Updates the set of loggable inputs. */
public default void updateInputs() {}
-
-}
\ No newline at end of file
+}
import frc.robot.constants.swerve.ModuleConstants;
import lib.CTREModuleState;
-
/**
- * Swerve module for drivetrain to be used inside of simulation.
- * TODO: improve this simulation to be more realistic
+ * Swerve module for drivetrain to be used inside of simulation. TODO: improve this simulation to be
+ * more realistic
*/
public class ModuleSim extends Module {
- private double currentSteerPositionRad = 0;
- private double currentDrivePositionMeters = 0;
- private double currentSpeed = 0;
-
- private SwerveModuleState desiredState;
-
-
- protected boolean stateDeadband = true;
-
- public ModuleSim(ModuleConstants ignored) {
- super(ignored);
- }
-
- /**
- * Updates the simulation
- */
- @Override
- public void periodic() {
- currentDrivePositionMeters += currentSpeed * Constants.LOOP_TIME;
- super.periodic();
- }
-
- @Override
- public void updateInputs(){
- // Update drive inputs
- inputs.driveConnected = true;
- inputs.drivePositionRad = currentDrivePositionMeters / DriveConstants.WHEEL_RADIUS;
- inputs.driveVelocityRadPerSec = currentSpeed / DriveConstants.WHEEL_RADIUS;
- inputs.driveAppliedVolts = currentSpeed / DriveConstants.MAX_SPEED * Constants.ROBOT_VOLTAGE;
- inputs.driveCurrentAmps = 0; // This simulation currently isn't good enough to calculate this
-
- // Update turn inputs
- inputs.turnConnected = true;
- inputs.turnEncoderConnected = true;
- inputs.turnAbsolutePosition = new Rotation2d(currentSteerPositionRad);
- inputs.turnPosition = new Rotation2d(currentSteerPositionRad);
- inputs.turnVelocityRadPerSec = 0; // Simulated modules currently teleport
- inputs.turnAppliedVolts = 0;
- inputs.turnCurrentAmps = 0;
-
- // Update odometry inputs
- // Simulate as only getting one value per frame
- inputs.odometryTimestamps =
- new double[]{Timer.getFPGATimestamp()};
- inputs.odometryDrivePositionsRad =
- new double[]{inputs.drivePositionRad*DriveConstants.DRIVE_GEAR_RATIO};
- inputs.odometryTurnPositions =
- new Rotation2d[]{inputs.turnPosition};
- timestampQueue.clear();
- drivePositionQueue.clear();
- turnPositionQueue.clear();
- }
-
- /**
- * Sets the desired state for the module.
- *
- * @param desiredState Desired state with speed and angle.
- * @param isOpenLoop whether to use closed/open loop control for drive velocity
- */
- public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
- if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
- // If the module isn't moving, don't rotate it
- if (Math.abs(desiredState.speedMetersPerSecond) < 0.001) {
- currentSpeed = 0;
- return;
- }
- // Optimize the reference state to avoid spinning further than 90 degrees
- desiredState = CTREModuleState.optimize(desiredState, new Rotation2d(currentSteerPositionRad));
- }
-
- currentSpeed = desiredState.speedMetersPerSecond;
- currentSteerPositionRad = desiredState.angle.getRadians();
- }
-
- public void resetToAbsolute() {
- // does nothing when robot does not have a swerve drivetrain
- }
-
- public SwerveModuleState getDesiredState() {
- return desiredState;
- }
-
- public double getDesiredVelocity() {
- return getDesiredState().speedMetersPerSecond;
- }
-
- public Rotation2d getDesiredAngle() {
- return getDesiredState().angle;
- }
-
- /**
- * Sets current speed to zero
- */
- public void stop() {
+ private double currentSteerPositionRad = 0;
+ private double currentDrivePositionMeters = 0;
+ private double currentSpeed = 0;
+
+ private SwerveModuleState desiredState;
+
+ protected boolean stateDeadband = true;
+
+ public ModuleSim(ModuleConstants ignored) {
+ super(ignored);
+ }
+
+ /** Updates the simulation */
+ @Override
+ public void periodic() {
+ currentDrivePositionMeters += currentSpeed * Constants.LOOP_TIME;
+ super.periodic();
+ }
+
+ @Override
+ public void updateInputs() {
+ // Update drive inputs
+ inputs.driveConnected = true;
+ inputs.drivePositionRad = currentDrivePositionMeters / DriveConstants.WHEEL_RADIUS;
+ inputs.driveVelocityRadPerSec = currentSpeed / DriveConstants.WHEEL_RADIUS;
+ inputs.driveAppliedVolts = currentSpeed / DriveConstants.MAX_SPEED * Constants.ROBOT_VOLTAGE;
+ inputs.driveCurrentAmps = 0; // This simulation currently isn't good enough to calculate this
+
+ // Update turn inputs
+ inputs.turnConnected = true;
+ inputs.turnEncoderConnected = true;
+ inputs.turnAbsolutePosition = new Rotation2d(currentSteerPositionRad);
+ inputs.turnPosition = new Rotation2d(currentSteerPositionRad);
+ inputs.turnVelocityRadPerSec = 0; // Simulated modules currently teleport
+ inputs.turnAppliedVolts = 0;
+ inputs.turnCurrentAmps = 0;
+
+ // Update odometry inputs
+ // Simulate as only getting one value per frame
+ inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()};
+ inputs.odometryDrivePositionsRad =
+ new double[] {inputs.drivePositionRad * DriveConstants.DRIVE_GEAR_RATIO};
+ inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+ }
+
+ /**
+ * Sets the desired state for the module.
+ *
+ * @param desiredState Desired state with speed and angle.
+ * @param isOpenLoop whether to use closed/open loop control for drive velocity
+ */
+ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ // If the module isn't moving, don't rotate it
+ if (Math.abs(desiredState.speedMetersPerSecond) < 0.001) {
currentSpeed = 0;
+ return;
+ }
+ // Optimize the reference state to avoid spinning further than 90 degrees
+ desiredState =
+ CTREModuleState.optimize(desiredState, new Rotation2d(currentSteerPositionRad));
}
- public SwerveModuleState getState() {
- return new SwerveModuleState(
- currentSpeed,
- getAngle()
- );
- }
-
- public SwerveModulePosition getPosition() {
- return new SwerveModulePosition(
- currentDrivePositionMeters,
- new Rotation2d(currentSteerPositionRad)
- );
- }
-
- /**
- * Gets the simulated angle of the module.
- */
- public Rotation2d getAngle() {
- return new Rotation2d(currentSteerPositionRad);
- }
-
- /**
- * Sets state deadband
- */
- public void setStateDeadband(boolean enabled) {
- stateDeadband = enabled;
- }
-
- public TalonFX getDriveMotor(){
- return null;
- }
-
- public double getDriveVoltage(){
- return 0;
- }
-
- public double getDriveStatorCurrent(){
- return 0;
- }
-
- public double getSteerVelocity() {
- return 0;
- }
- public double getDriveVelocity() {
- return 0;
- }
-
-}
\ No newline at end of file
+ currentSpeed = desiredState.speedMetersPerSecond;
+ currentSteerPositionRad = desiredState.angle.getRadians();
+ }
+
+ public void resetToAbsolute() {
+ // does nothing when robot does not have a swerve drivetrain
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
+ }
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /** Sets current speed to zero */
+ public void stop() {
+ currentSpeed = 0;
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(currentSpeed, getAngle());
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ currentDrivePositionMeters, new Rotation2d(currentSteerPositionRad));
+ }
+
+ /** Gets the simulated angle of the module. */
+ public Rotation2d getAngle() {
+ return new Rotation2d(currentSteerPositionRad);
+ }
+
+ /** Sets state deadband */
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
+
+ public TalonFX getDriveMotor() {
+ return null;
+ }
+
+ public double getDriveVoltage() {
+ return 0;
+ }
+
+ public double getDriveStatorCurrent() {
+ return 0;
+ }
+
+ public double getSteerVelocity() {
+ return 0;
+ }
+
+ public double getDriveVelocity() {
+ return 0;
+ }
+}
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-/**
- * Exactly the same as ElevatorSim, except it can be angled and have a constant force spring
- */
+/** Exactly the same as ElevatorSim, except it can be angled and have a constant force spring */
public class AngledElevatorSim extends ElevatorSim {
- private double angle;
- private boolean simulateGravity;
- private double minHeight;
- private double maxHeight;
- private double springAccel;
+ private double angle;
+ private boolean simulateGravity;
+ private double minHeight;
+ private double maxHeight;
+ private double springAccel;
- /**
- * Creates a simulated angled elevator mechanism.
- *
- * @param gearbox The type of and number of motors in the elevator gearbox.
- * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
- * @param carriageMassKg The mass of the elevator carriage.
- * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
- * @param minHeightMeters The min allowable height of the elevator.
- * @param maxHeightMeters The max allowable height of the elevator.
- * @param simulateGravity Whether gravity should be simulated or not.
- * @param startingHeightMeters The starting height of the elevator.
- * @param angleRads The angle of the elevator from vertical in radians.
- * @param springForceNewtons The force of the constant force spring in Newtons. Up is positive.
- * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
- * noise is desired. If present must have 1 element for position.
- */
- public AngledElevatorSim(
+ /**
+ * Creates a simulated angled elevator mechanism.
+ *
+ * @param gearbox The type of and number of motors in the elevator gearbox.
+ * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+ * @param carriageMassKg The mass of the elevator carriage.
+ * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+ * @param minHeightMeters The min allowable height of the elevator.
+ * @param maxHeightMeters The max allowable height of the elevator.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingHeightMeters The starting height of the elevator.
+ * @param angleRads The angle of the elevator from vertical in radians.
+ * @param springForceNewtons The force of the constant force spring in Newtons. Up is positive.
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ public AngledElevatorSim(
DCMotor gearbox,
double gearing,
double carriageMassKg,
double startingHeightMeters,
double angleRads,
double springForceNewtons,
- double... measurementStdDevs) {
- super(gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, startingHeightMeters, measurementStdDevs);
- angle = angleRads;
- this.simulateGravity = simulateGravity;
- minHeight = minHeightMeters;
- maxHeight = maxHeightMeters;
- springAccel = springForceNewtons/carriageMassKg;
- }
+ double... measurementStdDevs) {
+ super(
+ gearbox,
+ gearing,
+ carriageMassKg,
+ drumRadiusMeters,
+ minHeightMeters,
+ maxHeightMeters,
+ simulateGravity,
+ startingHeightMeters,
+ measurementStdDevs);
+ angle = angleRads;
+ this.simulateGravity = simulateGravity;
+ minHeight = minHeightMeters;
+ maxHeight = maxHeightMeters;
+ springAccel = springForceNewtons / carriageMassKg;
+ }
- // Copied from ElevatorSim with one difference
- /**
- * Creates a simulated elevator mechanism.
- *
- * @param gearbox The type of and number of motors in the elevator gearbox.
- * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
- * @param carriageMassKg The mass of the elevator carriage.
- * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
- * @param minHeightMeters The min allowable height of the elevator.
- * @param maxHeightMeters The max allowable height of the elevator.
- * @param simulateGravity Whether gravity should be simulated or not.
- * @param startingHeightMeters The starting height of the elevator.
- * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
- * noise is desired. If present must have 1 element for position.
- */
- @Override
- protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
- // Calculate updated x-hat from Runge-Kutta.
- var updatedXhat =
- NumericalIntegration.rkdp(
- (x, _u) -> {
- Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
- if (simulateGravity) {
- // This is the only line that is different
- xdot = xdot.plus(VecBuilder.fill(0, springAccel-9.8*Math.cos(angle)));
- }
- return xdot;
- },
- currentXhat,
- u,
- dtSeconds);
+ // Copied from ElevatorSim with one difference
+ /**
+ * Creates a simulated elevator mechanism.
+ *
+ * @param gearbox The type of and number of motors in the elevator gearbox.
+ * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+ * @param carriageMassKg The mass of the elevator carriage.
+ * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+ * @param minHeightMeters The min allowable height of the elevator.
+ * @param maxHeightMeters The max allowable height of the elevator.
+ * @param simulateGravity Whether gravity should be simulated or not.
+ * @param startingHeightMeters The starting height of the elevator.
+ * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+ * noise is desired. If present must have 1 element for position.
+ */
+ @Override
+ protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
+ // Calculate updated x-hat from Runge-Kutta.
+ var updatedXhat =
+ NumericalIntegration.rkdp(
+ (x, _u) -> {
+ Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
+ if (simulateGravity) {
+ // This is the only line that is different
+ xdot = xdot.plus(VecBuilder.fill(0, springAccel - 9.8 * Math.cos(angle)));
+ }
+ return xdot;
+ },
+ currentXhat,
+ u,
+ dtSeconds);
- // We check for collisions after updating x-hat.
- if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
- return VecBuilder.fill(minHeight, 0);
- }
- if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
- return VecBuilder.fill(maxHeight, 0);
- }
- return updatedXhat;
+ // We check for collisions after updating x-hat.
+ if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(minHeight, 0);
+ }
+ if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
+ return VecBuilder.fill(maxHeight, 0);
}
+ return updatedXhat;
+ }
}
public final class ChineseRemainderTheorem {
- private ChineseRemainderTheorem() {}
+ private ChineseRemainderTheorem() {}
+
+ /**
+ * Computes x such that: x ≡ a (mod n1) x ≡ b (mod n2)
+ *
+ * <p>n1 and n2 MUST be coprime.
+ *
+ * <p>Returns x in range [0, n1*n2)
+ */
+ public static int solve(int a, int n1, int b, int n2) {
+ if (gcd(n1, n2) != 1) {
+ throw new IllegalArgumentException("Moduli must be coprime for CRT.");
+ }
- /**
- * Computes x such that:
- * x ≡ a (mod n1)
- * x ≡ b (mod n2)
- *
- * n1 and n2 MUST be coprime.
- *
- * Returns x in range [0, n1*n2)
- */
+ int N = n1 * n2;
- public static int solve(int a, int n1, int b, int n2) {
- if (gcd(n1, n2) != 1) {
- throw new IllegalArgumentException("Moduli must be coprime for CRT.");
- }
+ int invN1modN2 = modInverse(n1, n2);
+ int invN2modN1 = modInverse(n2, n1);
- int N = n1 * n2;
+ int result = (a * n2 * invN2modN1 + b * n1 * invN1modN2) % N;
- int invN1modN2 = modInverse(n1, n2);
- int invN2modN1 = modInverse(n2, n1);
+ return (result + N) % N;
+ }
- int result =
- (a * n2 * invN2modN1 +
- b * n1 * invN1modN2) % N;
+ private static int modInverse(int a, int mod) {
+ a = ((a % mod) + mod) % mod;
- return (result + N) % N;
+ for (int x = 1; x < mod; x++) {
+ if ((a * x) % mod == 1) {
+ return x;
+ }
}
-
- private static int modInverse(int a, int mod) {
- a = ((a % mod) + mod) % mod;
-
- for (int x = 1; x < mod; x++) {
- if ((a * x) % mod == 1) {
- return x;
- }
- }
- throw new IllegalStateException("No modular inverse exists.");
- }
-
- private static int gcd(int a, int b) {
- while (b != 0) {
- int t = b;
- b = a % b;
- a = t;
- }
- return Math.abs(a);
+ throw new IllegalStateException("No modular inverse exists.");
+ }
+
+ private static int gcd(int a, int b) {
+ while (b != 0) {
+ int t = b;
+ b = a % b;
+ a = t;
}
-}
\ No newline at end of file
+ return Math.abs(a);
+ }
+}
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
/**
- * Similar to SingleJointedArmSim, except it simulates an upward normal force on the end of the arm equal to the weight of the robot
- * Use setIsClimbing() to change whether this normal force should be simulated
+ * Similar to SingleJointedArmSim, except it simulates an upward normal force on the end of the arm
+ * equal to the weight of the robot Use setIsClimbing() to change whether this normal force should
+ * be simulated
*/
public class ClimbArmSim extends SingleJointedArmSim {
private boolean simulateGravity;
double robotMasKilograms,
double armMassKilograms,
double... measurementStdDevs) {
- super(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, simulateGravity, startingAngleRads, measurementStdDevs);
+ super(
+ plant,
+ gearbox,
+ gearing,
+ armLengthMeters,
+ minAngleRads,
+ maxAngleRads,
+ simulateGravity,
+ startingAngleRads,
+ measurementStdDevs);
armLenMeters = armLengthMeters;
minAngle = minAngleRads;
maxAngle = maxAngleRads;
this.simulateGravity = simulateGravity;
mass = robotMasKilograms;
- momentOfInertia = 1.0/3.0 * armMassKilograms * armLengthMeters * armLengthMeters;
+ momentOfInertia = 1.0 / 3.0 * armMassKilograms * armLengthMeters * armLengthMeters;
isClimbing = false;
}
momentOfInertia = jKgMetersSquared;
}
- public void setIsClimbing(boolean climbing){
+ public void setIsClimbing(boolean climbing) {
isClimbing = climbing;
}
-
+
/**
* Updates the state of the arm.
*
(Matrix<N2, N1> x, Matrix<N1, N1> _u) -> {
Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
if (simulateGravity) {
- double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / armLenMeters + (isClimbing ? mass * 9.8 * Math.cos(x.get(0, 0)) * armLenMeters / momentOfInertia : 0);
+ double alphaGrav =
+ 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / armLenMeters
+ + (isClimbing
+ ? mass * 9.8 * Math.cos(x.get(0, 0)) * armLenMeters / momentOfInertia
+ : 0);
xdot = xdot.plus(VecBuilder.fill(0, alphaGrav));
}
return xdot;
}
return updatedXhat;
}
-
}
public class ConversionUtils {
- /**
- * @param positionCounts CANCoder Position Counts
- * @param gearRatio Gear Ratio between CANCoder and Mechanism
- * @return Degrees of Rotation of Mechanism
- */
- public static double CANcoderToDegrees(double positionCounts, double gearRatio) {
- return positionCounts * (360.0 / (gearRatio * 4096.0));
- }
-
- /**
- * @param degrees Degrees of rotation of Mechanism
- * @param gearRatio Gear Ratio between CANCoder and Mechanism
- * @return CANCoder Position Counts
- */
- public static double degreesToCANcoder(double degrees, double gearRatio) {
- return degrees / (360.0 / (gearRatio * 4096.0));
- }
-
- /**
- * @param positionCounts CANCoder Position Counts
- * @param gearRatio Gear Ratio between CANCoder and Mechanism
- * @return Radians of Rotation of Mechanism
- */
- public static double CANcoderToRadians(double positionCounts, double gearRatio) {
- return Math.toRadians(CANcoderToDegrees(positionCounts, gearRatio));
- }
-
- /**
- * @param radians Radians of rotation of Mechanism
- * @param gearRatio Gear Ratio between CANCoder and Mechanism
- * @return CANCoder Position Counts
- */
- public static double radiansToCANcoder(double radians, double gearRatio) {
- return degreesToCANcoder(Math.toDegrees(radians), gearRatio);
- }
-
- /**
- * @param positionCounts Falcon Position Counts
- * @param gearRatio Gear Ratio between Falcon and Mechanism
- * @return Degrees of Rotation of Mechanism
- */
- public static double falconToDegrees(double positionCounts, double gearRatio) {
- return positionCounts * (360.0 / (gearRatio * 2048.0));
- }
-
- /**
- * @param degrees Degrees of rotation of Mechanism
- * @param gearRatio Gear Ratio between Falcon and Mechanism
- * @return Falcon Position Counts
- */
- public static double degreesToFalcon(double degrees, double gearRatio) {
- return degrees / (360.0 / (gearRatio * 2048.0));
- }
-
- /**
- * @param velocityCounts Falcon Velocity Counts
- * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
- * @return RPM of Mechanism
- */
- public static double falconToRPM(double velocityCounts, double gearRatio) {
- double motorRPM = velocityCounts * (600.0 / 2048.0);
- return motorRPM / gearRatio;
- }
-
- /**
- * @param RPM RPM of mechanism
- * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
- * @return RPM of Mechanism
- */
- public static double RPMToFalcon(double RPM, double gearRatio) {
- double motorRPM = RPM * gearRatio;
- return motorRPM * (2048.0 / 600.0);
- }
-
- /**
- * @param velocitycounts Falcon Velocity Counts
- * @param circumference Circumference of Wheel
- * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
- * @return Falcon Velocity Counts
- */
- public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) {
- double wheelRPM = falconToRPM(velocitycounts, gearRatio);
- return (wheelRPM * circumference) / 60;
- }
-
- /**
- * @param velocity Velocity MPS
- * @param circumference Circumference of Wheel
- * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
- * @return Falcon Velocity Counts
- */
- public static double MPSToFalcon(double velocity, double circumference, double gearRatio) {
- double wheelRPM = ((velocity * 60) / circumference);
- return RPMToFalcon(wheelRPM, gearRatio);
- }
-
- /**
- * @param positionCounts Falcon Position Counts
- * @param circumference Circumference of Wheel
- * @param gearRatio Gear Ratio between Falcon and Wheel
- * @return Meters
- */
- public static double falconToMeters(double positionCounts, double circumference, double gearRatio) {
- return positionCounts * (circumference / (gearRatio * 2048.0));
- }
-
- /**
- * @param meters Meters
- * @param circumference Circumference of Wheel
- * @param gearRatio Gear Ratio between Falcon and Wheel
- * @return Falcon Position Counts
- */
- public static double MetersToFalcon(double meters, double circumference, double gearRatio) {
- return meters / (circumference / (gearRatio * 2048.0));
- }
-
-
- /**
- * Converts between an absolute coordinate system and the pathplanner coordinate system.
- * <p>
- * Absolute coordinate system always has the origin right of the blue driver station from blue driver perspective,
- * bottom left if looking down at the field. Positive X goes toward red alliance (forward from blue driver perspective)
- * and positive Y toward red loading zone (left from blue driver perspective). The Pathplanner coordinate system has the coordinate
- * system rotated such that the origin starts right of the current driver station.
- * <p> The transformation is self-inverse, so there is no second function to convert back.
- *
- * @param pose pose to convert
- * @param alliance alliance PathPlanner is using for their origin
- * @return converted pose
- */
- public static Pose2d absolutePoseToPathPlannerPose(Pose2d pose, Alliance alliance) {
- if (alliance == Alliance.Red) {
- return pose.relativeTo(new Pose2d(FieldConstants.field.getFieldLength(), FieldConstants.field.getFieldWidth(), new Rotation2d(Math.PI)));
- }
- return new Pose2d(pose.getX(), pose.getY(), pose.getRotation());
+ /**
+ * @param positionCounts CANCoder Position Counts
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return Degrees of Rotation of Mechanism
+ */
+ public static double CANcoderToDegrees(double positionCounts, double gearRatio) {
+ return positionCounts * (360.0 / (gearRatio * 4096.0));
+ }
+
+ /**
+ * @param degrees Degrees of rotation of Mechanism
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return CANCoder Position Counts
+ */
+ public static double degreesToCANcoder(double degrees, double gearRatio) {
+ return degrees / (360.0 / (gearRatio * 4096.0));
+ }
+
+ /**
+ * @param positionCounts CANCoder Position Counts
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return Radians of Rotation of Mechanism
+ */
+ public static double CANcoderToRadians(double positionCounts, double gearRatio) {
+ return Math.toRadians(CANcoderToDegrees(positionCounts, gearRatio));
+ }
+
+ /**
+ * @param radians Radians of rotation of Mechanism
+ * @param gearRatio Gear Ratio between CANCoder and Mechanism
+ * @return CANCoder Position Counts
+ */
+ public static double radiansToCANcoder(double radians, double gearRatio) {
+ return degreesToCANcoder(Math.toDegrees(radians), gearRatio);
+ }
+
+ /**
+ * @param positionCounts Falcon Position Counts
+ * @param gearRatio Gear Ratio between Falcon and Mechanism
+ * @return Degrees of Rotation of Mechanism
+ */
+ public static double falconToDegrees(double positionCounts, double gearRatio) {
+ return positionCounts * (360.0 / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param degrees Degrees of rotation of Mechanism
+ * @param gearRatio Gear Ratio between Falcon and Mechanism
+ * @return Falcon Position Counts
+ */
+ public static double degreesToFalcon(double degrees, double gearRatio) {
+ return degrees / (360.0 / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param velocityCounts Falcon Velocity Counts
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+ * @return RPM of Mechanism
+ */
+ public static double falconToRPM(double velocityCounts, double gearRatio) {
+ double motorRPM = velocityCounts * (600.0 / 2048.0);
+ return motorRPM / gearRatio;
+ }
+
+ /**
+ * @param RPM RPM of mechanism
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+ * @return RPM of Mechanism
+ */
+ public static double RPMToFalcon(double RPM, double gearRatio) {
+ double motorRPM = RPM * gearRatio;
+ return motorRPM * (2048.0 / 600.0);
+ }
+
+ /**
+ * @param velocitycounts Falcon Velocity Counts
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
+ * @return Falcon Velocity Counts
+ */
+ public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) {
+ double wheelRPM = falconToRPM(velocitycounts, gearRatio);
+ return (wheelRPM * circumference) / 60;
+ }
+
+ /**
+ * @param velocity Velocity MPS
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
+ * @return Falcon Velocity Counts
+ */
+ public static double MPSToFalcon(double velocity, double circumference, double gearRatio) {
+ double wheelRPM = ((velocity * 60) / circumference);
+ return RPMToFalcon(wheelRPM, gearRatio);
+ }
+
+ /**
+ * @param positionCounts Falcon Position Counts
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Wheel
+ * @return Meters
+ */
+ public static double falconToMeters(
+ double positionCounts, double circumference, double gearRatio) {
+ return positionCounts * (circumference / (gearRatio * 2048.0));
+ }
+
+ /**
+ * @param meters Meters
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Wheel
+ * @return Falcon Position Counts
+ */
+ public static double MetersToFalcon(double meters, double circumference, double gearRatio) {
+ return meters / (circumference / (gearRatio * 2048.0));
+ }
+
+ /**
+ * Converts between an absolute coordinate system and the pathplanner coordinate system.
+ *
+ * <p>Absolute coordinate system always has the origin right of the blue driver station from blue
+ * driver perspective, bottom left if looking down at the field. Positive X goes toward red
+ * alliance (forward from blue driver perspective) and positive Y toward red loading zone (left
+ * from blue driver perspective). The Pathplanner coordinate system has the coordinate system
+ * rotated such that the origin starts right of the current driver station.
+ *
+ * <p>The transformation is self-inverse, so there is no second function to convert back.
+ *
+ * @param pose pose to convert
+ * @param alliance alliance PathPlanner is using for their origin
+ * @return converted pose
+ */
+ public static Pose2d absolutePoseToPathPlannerPose(Pose2d pose, Alliance alliance) {
+ if (alliance == Alliance.Red) {
+ return pose.relativeTo(
+ new Pose2d(
+ FieldConstants.field.getFieldLength(),
+ FieldConstants.field.getFieldWidth(),
+ new Rotation2d(Math.PI)));
}
+ return new Pose2d(pose.getX(), pose.getY(), pose.getRotation());
+ }
}
* A class that limits the rate of change of an input value. Useful for implementing voltage,
* setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
* controlled is a velocity or a voltage; when controlling a position, consider using a {@link
- * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
- * Edited by 972 to be "dynamic", that is, the slew rate can be modified on the fly.
- * Additionally, it can be set to be continuous on a range, useful for angles.
+ * edu.wpi.first.math.trajectory.TrapezoidProfile} instead. Edited by 972 to be "dynamic", that is,
+ * the slew rate can be modified on the fly. Additionally, it can be set to be continuous on a
+ * range, useful for angles.
*/
public class DynamicSlewRateLimiter {
- private double positiveRateLimit;
- private double negativeRateLimit;
- private double prevVal;
- private double prevTime;
-
- private boolean continuous = false;
- private double lowerContinuousLimit = -1;
- private double upperContinuousLimit = 1;
-
- /**
- * Creates a new DynamicSlewRateLimiter with the given positive and negative rate limits and initial
- * value.
- *
- * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
- * second. This is expected to be positive.
- * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
- * second. This is expected to be negative.
- * @param initialValue The initial value of the input.
- */
- public DynamicSlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) {
- this.positiveRateLimit = positiveRateLimit;
- this.negativeRateLimit = negativeRateLimit;
- prevVal = initialValue;
- prevTime = WPIUtilJNI.now() * 1e-6;
+ private double positiveRateLimit;
+ private double negativeRateLimit;
+ private double prevVal;
+ private double prevTime;
+
+ private boolean continuous = false;
+ private double lowerContinuousLimit = -1;
+ private double upperContinuousLimit = 1;
+
+ /**
+ * Creates a new DynamicSlewRateLimiter with the given positive and negative rate limits and
+ * initial value.
+ *
+ * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+ * second. This is expected to be positive.
+ * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+ * second. This is expected to be negative.
+ * @param initialValue The initial value of the input.
+ */
+ public DynamicSlewRateLimiter(
+ double positiveRateLimit, double negativeRateLimit, double initialValue) {
+ this.positiveRateLimit = positiveRateLimit;
+ this.negativeRateLimit = negativeRateLimit;
+ prevVal = initialValue;
+ prevTime = WPIUtilJNI.now() * 1e-6;
+ }
+
+ /**
+ * Creates a new DynamicSlewRateLimiter with the given positive rate limit and negative rate limit
+ * of -rateLimit and initial value.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ * @param initialValue The initial value of the input.
+ */
+ @Deprecated(since = "2023", forRemoval = true)
+ public DynamicSlewRateLimiter(double rateLimit, double initialValue) {
+ this(rateLimit, -rateLimit, initialValue);
+ }
+
+ /**
+ * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
+ * -rateLimit.
+ *
+ * @param rateLimit The rate-of-change limit, in units per second.
+ */
+ public DynamicSlewRateLimiter(double rateLimit) {
+ this(rateLimit, -rateLimit, 0);
+ }
+
+ /**
+ * Filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input) {
+ double currentTime = WPIUtilJNI.now() * 1e-6;
+ double elapsedTime = currentTime - prevTime;
+ prevTime = currentTime;
+
+ double change =
+ MathUtil.clamp(
+ input - prevVal, negativeRateLimit * elapsedTime, positiveRateLimit * elapsedTime);
+
+ if (continuous) {
+ change =
+ MathUtil.clamp(
+ MathUtil.inputModulus(input - prevVal, lowerContinuousLimit, upperContinuousLimit),
+ negativeRateLimit * elapsedTime,
+ positiveRateLimit * elapsedTime);
+
+ prevVal += change;
+
+ // Extra check to make sure it is within the limits, probably unnecessary
+ prevVal = MathUtil.inputModulus(prevVal, lowerContinuousLimit, upperContinuousLimit);
+ } else {
+ prevVal += change;
}
- /**
- * Creates a new DynamicSlewRateLimiter with the given positive rate limit and negative rate limit of
- * -rateLimit and initial value.
- *
- * @param rateLimit The rate-of-change limit, in units per second.
- * @param initialValue The initial value of the input.
- */
- @Deprecated(since = "2023", forRemoval = true)
- public DynamicSlewRateLimiter(double rateLimit, double initialValue) {
- this(rateLimit, -rateLimit, initialValue);
- }
-
- /**
- * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
- * -rateLimit.
- *
- * @param rateLimit The rate-of-change limit, in units per second.
- */
- public DynamicSlewRateLimiter(double rateLimit) {
- this(rateLimit, -rateLimit, 0);
- }
-
- /**
- * Filters the input to limit its slew rate.
- *
- * @param input The input value whose slew rate is to be limited.
- * @return The filtered value, which will not change faster than the slew rate.
- */
- public double calculate(double input) {
- double currentTime = WPIUtilJNI.now() * 1e-6;
- double elapsedTime = currentTime - prevTime;
- prevTime = currentTime;
-
- double change = MathUtil.clamp(
- input - prevVal,
- negativeRateLimit * elapsedTime,
- positiveRateLimit * elapsedTime);
-
- if (continuous) {
- change = MathUtil.clamp(
- MathUtil.inputModulus(input - prevVal, lowerContinuousLimit, upperContinuousLimit),
- negativeRateLimit * elapsedTime,
- positiveRateLimit * elapsedTime);
-
- prevVal += change;
-
- //Extra check to make sure it is within the limits, probably unnecessary
- prevVal = MathUtil.inputModulus(prevVal, lowerContinuousLimit, upperContinuousLimit);
- } else {
- prevVal += change;
- }
-
- return prevVal;
- }
-
- /**
- * Sets a new slewrate and filters the input to limit its slew rate.
- *
- * @param input The input value whose slew rate is to be limited.
- * @param rateLimit The new rate-of-change limit, in units per second.
- * @return The filtered value, which will not change faster than the slew rate.
- */
- public double calculate(double input, double rateLimit) {
- setRateLimit(rateLimit);
- return calculate(input);
- }
-
- /**
- * Sets new slew rates and filters the input to limit its slew rate.
- *
- * @param input The input value whose slew rate is to be limited.
- * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
- * second. This is expected to be positive.
- * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
- * second. This is expected to be negative.
- * @return The filtered value, which will not change faster than the slew rate.
- */
- public double calculate(double input, double positiveRateLimit, double negativeRateLimit) {
- setRateLimit(positiveRateLimit, negativeRateLimit);
- return calculate(input);
- }
-
-
- /**
- * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
- *
- * @param value The value to reset to.
- */
- public void reset(double value) {
- prevVal = value;
- prevTime = WPIUtilJNI.now() * 1e-6;
- }
-
- /**
- * set positive rate limit
- *
- * @param positiveRateLimit new positive rate limit
- */
- public void setPositiveRateLimit(double positiveRateLimit) {
- this.positiveRateLimit = positiveRateLimit;
- }
-
- /**
- * set negative rate limit
- *
- * @param negativeRateLimit new negative rate limit
- */
- public void setNegativeRateLimit(double negativeRateLimit) {
- this.negativeRateLimit = negativeRateLimit;
- }
-
- /**
- * Sets positive and negative rate limits
- *
- * @param rateLimit new rate limits
- */
- public void setRateLimit(double rateLimit) {
- positiveRateLimit = rateLimit;
- negativeRateLimit = -rateLimit;
- }
-
- /**
- * Sets positive and negative rate limits
- *
- * @param positiveRateLimit new positive rate limit
- * @param negativeRateLimit new negative rate limit
- */
- public void setRateLimit(double positiveRateLimit, double negativeRateLimit) {
- this.positiveRateLimit = positiveRateLimit;
- this.negativeRateLimit = negativeRateLimit;
- }
-
- /**
- * Sets Continuous Limits
- *
- * @param lowerContinuousLimit Lower Continuous Limit
- * @param upperContinuousLimit Upper Continuous Limit
- */
- public void setContinuousLimits(double lowerContinuousLimit, double upperContinuousLimit) {
- this.lowerContinuousLimit = lowerContinuousLimit;
- this.upperContinuousLimit = upperContinuousLimit;
- }
-
- /**
- * Enables or disables continuous
- * WARNING: Continuous doesn't work properly with non-symmetrical rate limits
- *
- * @param continuous is continuous enabled
- */
- public void enableContinuous(boolean continuous) {
- this.continuous = continuous;
- }
-}
\ No newline at end of file
+ return prevVal;
+ }
+
+ /**
+ * Sets a new slewrate and filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @param rateLimit The new rate-of-change limit, in units per second.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input, double rateLimit) {
+ setRateLimit(rateLimit);
+ return calculate(input);
+ }
+
+ /**
+ * Sets new slew rates and filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
+ * second. This is expected to be positive.
+ * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
+ * second. This is expected to be negative.
+ * @return The filtered value, which will not change faster than the slew rate.
+ */
+ public double calculate(double input, double positiveRateLimit, double negativeRateLimit) {
+ setRateLimit(positiveRateLimit, negativeRateLimit);
+ return calculate(input);
+ }
+
+ /**
+ * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+ *
+ * @param value The value to reset to.
+ */
+ public void reset(double value) {
+ prevVal = value;
+ prevTime = WPIUtilJNI.now() * 1e-6;
+ }
+
+ /**
+ * set positive rate limit
+ *
+ * @param positiveRateLimit new positive rate limit
+ */
+ public void setPositiveRateLimit(double positiveRateLimit) {
+ this.positiveRateLimit = positiveRateLimit;
+ }
+
+ /**
+ * set negative rate limit
+ *
+ * @param negativeRateLimit new negative rate limit
+ */
+ public void setNegativeRateLimit(double negativeRateLimit) {
+ this.negativeRateLimit = negativeRateLimit;
+ }
+
+ /**
+ * Sets positive and negative rate limits
+ *
+ * @param rateLimit new rate limits
+ */
+ public void setRateLimit(double rateLimit) {
+ positiveRateLimit = rateLimit;
+ negativeRateLimit = -rateLimit;
+ }
+
+ /**
+ * Sets positive and negative rate limits
+ *
+ * @param positiveRateLimit new positive rate limit
+ * @param negativeRateLimit new negative rate limit
+ */
+ public void setRateLimit(double positiveRateLimit, double negativeRateLimit) {
+ this.positiveRateLimit = positiveRateLimit;
+ this.negativeRateLimit = negativeRateLimit;
+ }
+
+ /**
+ * Sets Continuous Limits
+ *
+ * @param lowerContinuousLimit Lower Continuous Limit
+ * @param upperContinuousLimit Upper Continuous Limit
+ */
+ public void setContinuousLimits(double lowerContinuousLimit, double upperContinuousLimit) {
+ this.lowerContinuousLimit = lowerContinuousLimit;
+ this.upperContinuousLimit = upperContinuousLimit;
+ }
+
+ /**
+ * Enables or disables continuous WARNING: Continuous doesn't work properly with non-symmetrical
+ * rate limits
+ *
+ * @param continuous is continuous enabled
+ */
+ public void enableContinuous(boolean continuous) {
+ this.continuous = continuous;
+ }
+}
import java.util.List;
/**
- * A class for storing and processing feedforward characterization data. Used in automatic feedforward characterization.
+ * A class for storing and processing feedforward characterization data. Used in automatic
+ * feedforward characterization.
*/
public class FeedForwardCharacterizationData {
- private PolynomialRegression regression;
- private final List<Double> velocityData = new LinkedList<>();
- private final List<Double> voltageData = new LinkedList<>();
+ private PolynomialRegression regression;
+ private final List<Double> velocityData = new LinkedList<>();
+ private final List<Double> voltageData = new LinkedList<>();
- /**
- * Adds a data point to the data set.
- *
- * @param velocity the velocity of the motor
- * @param voltage the voltage applied to the motor
- */
- public void add(double velocity, double voltage) {
- if (Math.abs(velocity) > 1E-4) {
- velocityData.add(Math.abs(velocity));
- voltageData.add(Math.abs(voltage));
- }
+ /**
+ * Adds a data point to the data set.
+ *
+ * @param velocity the velocity of the motor
+ * @param voltage the voltage applied to the motor
+ */
+ public void add(double velocity, double voltage) {
+ if (Math.abs(velocity) > 1E-4) {
+ velocityData.add(Math.abs(velocity));
+ voltageData.add(Math.abs(voltage));
}
+ }
- /**
- * Processes the data set using {@link PolynomialRegression}
- *
- * @see PolynomialRegression
- */
- public void process() {
- // creates a new process polynomial regression to get calculated values
- regression = new PolynomialRegression(
- velocityData.stream().mapToDouble(Double::doubleValue).toArray(),
- voltageData.stream().mapToDouble(Double::doubleValue).toArray(),
- 1
- );
- }
+ /**
+ * Processes the data set using {@link PolynomialRegression}
+ *
+ * @see PolynomialRegression
+ */
+ public void process() {
+ // creates a new process polynomial regression to get calculated values
+ regression =
+ new PolynomialRegression(
+ velocityData.stream().mapToDouble(Double::doubleValue).toArray(),
+ voltageData.stream().mapToDouble(Double::doubleValue).toArray(),
+ 1);
+ }
- /**
- * Gets the static voltage of the motor.
- *
- * @return the static voltage of the motor
- */
- public double getStatic() {
- // gets y-intercept
- return regression.beta(0);
- }
+ /**
+ * Gets the static voltage of the motor.
+ *
+ * @return the static voltage of the motor
+ */
+ public double getStatic() {
+ // gets y-intercept
+ return regression.beta(0);
+ }
- /**
- * Gets the velocity of the motor.
- *
- * @return the velocity of the motor
- */
- public double getVelocity() {
- // gets a slope of regression line
- return regression.beta(1);
- }
+ /**
+ * Gets the velocity of the motor.
+ *
+ * @return the velocity of the motor
+ */
+ public double getVelocity() {
+ // gets a slope of regression line
+ return regression.beta(1);
+ }
- /**
- * Gets the variance of the data set.
- *
- * @return the variance of the data set
- */
- public double getVariance() {
- // gets variance of data set
- return regression.R2();
- }
+ /**
+ * Gets the variance of the data set.
+ *
+ * @return the variance of the data set
+ */
+ public double getVariance() {
+ // gets variance of data set
+ return regression.R2();
+ }
}
-
import edu.wpi.first.math.MathUtil;
import frc.robot.constants.Constants;
-/**
- * Utility class for useful functions.
- */
+/** Utility class for useful functions. */
public class MathUtils {
- /**
- * Deadbands an input to [-1, -deadband], [deadband, 1], rescaling inputs to be linear from
- * (deadband, 0) to (1,1)
- *
- * @param input The input value to rescale
- * @param deadband The deadband
- * @return the input rescaled and to fit [-1, -deadband], [deadband, 1]
- */
- public static double deadband(double input, double deadband) {
- if (Math.abs(input) <= deadband) {
- return 0;
- } else if (Math.abs(input) == 1) {
- return input;
- } else {
- return (1 / (1 - deadband) * (input + Math.signum(-input) * deadband));
- }
+ /**
+ * Deadbands an input to [-1, -deadband], [deadband, 1], rescaling inputs to be linear from
+ * (deadband, 0) to (1,1)
+ *
+ * @param input The input value to rescale
+ * @param deadband The deadband
+ * @return the input rescaled and to fit [-1, -deadband], [deadband, 1]
+ */
+ public static double deadband(double input, double deadband) {
+ if (Math.abs(input) <= deadband) {
+ return 0;
+ } else if (Math.abs(input) == 1) {
+ return input;
+ } else {
+ return (1 / (1 - deadband) * (input + Math.signum(-input) * deadband));
}
-
- /**
- * Deadbands an input to [-1, -OIConstants.DEADBAND], [OIConstants.DEADBAND, 1], rescaling inputs to be linear from
- * (OIConstants.DEADBAND, 0) to (1,1)
- *
- * @param input The input value to rescale
- * @return the input rescaled and to fit [-1, -DEADBAND], [DEADBAND, 1]
- */
- public static double deadband(double input) {
- return deadband(input, Constants.DEFAULT_DEADBAND);
- }
-
- /**
- * An exponential function that maintains positive or negative sign.
- *
- * @param exponent the power to raise the base to
- * @param base the base which will be raised to the power
- * @return base to the power of exponent, maintaining sign of base
- */
- public static double expoMS(double base, double exponent) {
- // weird stuff will happen if you don't put a number > 0 for controller inputs
- double finVal = Math.pow(Math.abs(base), exponent);
- if (base < 0) {
- finVal *= -1;
- }
- return finVal;
- }
-
- /**
- * Calculates Midpoint of two numbers on modulus number line
- *
- * @param num1 first number
- * @param num2 second number
- * @param lowerBound lower bound of modulus number line
- * @param upperBound upper bound of modulus number line
- * @return midpoint of 2 numbers on modulus number line
- */
- public static double modulusMidpoint(double num1, double num2, double lowerBound, double upperBound) {
- num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
- num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
- if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
- return MathUtil.inputModulus((num1 + num2) / 2 + (upperBound - lowerBound) / 2, lowerBound, upperBound);
- }
- return (num1 + num2) / 2;
+ }
+
+ /**
+ * Deadbands an input to [-1, -OIConstants.DEADBAND], [OIConstants.DEADBAND, 1], rescaling inputs
+ * to be linear from (OIConstants.DEADBAND, 0) to (1,1)
+ *
+ * @param input The input value to rescale
+ * @return the input rescaled and to fit [-1, -DEADBAND], [DEADBAND, 1]
+ */
+ public static double deadband(double input) {
+ return deadband(input, Constants.DEFAULT_DEADBAND);
+ }
+
+ /**
+ * An exponential function that maintains positive or negative sign.
+ *
+ * @param exponent the power to raise the base to
+ * @param base the base which will be raised to the power
+ * @return base to the power of exponent, maintaining sign of base
+ */
+ public static double expoMS(double base, double exponent) {
+ // weird stuff will happen if you don't put a number > 0 for controller inputs
+ double finVal = Math.pow(Math.abs(base), exponent);
+ if (base < 0) {
+ finVal *= -1;
}
-
- /**
- * Interpolates between two numbers on modulus number line
- *
- * @param num1 first number
- * @param num2 second number
- * @param amount the amount to interpolate, 0 = first number, 1 = second number
- * @param lowerBound lower bound of modulus number line
- * @param upperBound upper bound of modulus number line
- * @return interpolated value between 2 numbers on modulus number line
- */
- public static double modulusInterpolate(double num1, double num2, double amount, double lowerBound, double upperBound) {
- num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
- num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
- if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
- if(num1 < num2){
- num1 += upperBound-lowerBound;
- }else{
- num2 += upperBound-lowerBound;
- }
- }
- return MathUtil.inputModulus((1-amount)*num1 + amount*num2, lowerBound, upperBound);
+ return finVal;
+ }
+
+ /**
+ * Calculates Midpoint of two numbers on modulus number line
+ *
+ * @param num1 first number
+ * @param num2 second number
+ * @param lowerBound lower bound of modulus number line
+ * @param upperBound upper bound of modulus number line
+ * @return midpoint of 2 numbers on modulus number line
+ */
+ public static double modulusMidpoint(
+ double num1, double num2, double lowerBound, double upperBound) {
+ num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
+ num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
+ if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
+ return MathUtil.inputModulus(
+ (num1 + num2) / 2 + (upperBound - lowerBound) / 2, lowerBound, upperBound);
}
-
- /**
- * Calls {@link #mean(double...)}.
- *
- * @param data the list of data to find the mean of
- * @return the mean of the data
- */
- public static double mean(List<Double> data) {
- return mean(doubleListToArray(data));
+ return (num1 + num2) / 2;
+ }
+
+ /**
+ * Interpolates between two numbers on modulus number line
+ *
+ * @param num1 first number
+ * @param num2 second number
+ * @param amount the amount to interpolate, 0 = first number, 1 = second number
+ * @param lowerBound lower bound of modulus number line
+ * @param upperBound upper bound of modulus number line
+ * @return interpolated value between 2 numbers on modulus number line
+ */
+ public static double modulusInterpolate(
+ double num1, double num2, double amount, double lowerBound, double upperBound) {
+ num1 = MathUtil.inputModulus(num1, lowerBound, upperBound);
+ num2 = MathUtil.inputModulus(num2, lowerBound, upperBound);
+ if (Math.abs(num1 - num2) > (upperBound - lowerBound) / 2) {
+ if (num1 < num2) {
+ num1 += upperBound - lowerBound;
+ } else {
+ num2 += upperBound - lowerBound;
+ }
}
-
- /**
- * Finds the mean of the provided array of doubles
- *
- * @param data an array of doubles
- * @return the mean of the data
- */
- public static double mean(double... data) {
- double mean = 0;
- for (double datum : data) {
- mean += datum;
- }
- mean /= data.length;
- return mean;
+ return MathUtil.inputModulus((1 - amount) * num1 + amount * num2, lowerBound, upperBound);
+ }
+
+ /**
+ * Calls {@link #mean(double...)}.
+ *
+ * @param data the list of data to find the mean of
+ * @return the mean of the data
+ */
+ public static double mean(List<Double> data) {
+ return mean(doubleListToArray(data));
+ }
+
+ /**
+ * Finds the mean of the provided array of doubles
+ *
+ * @param data an array of doubles
+ * @return the mean of the data
+ */
+ public static double mean(double... data) {
+ double mean = 0;
+ for (double datum : data) {
+ mean += datum;
}
-
- /**
- * Calls {@link #stdDev(double...)}.
- *
- * @param data the list of data to find the standard deviation of
- * @return the standard deviation of the data
- */
- public static double stdDev(List<Double> data) {
- return stdDev(doubleListToArray(data));
- }
-
- /**
- * Finds the standard deviation of the provided array of doubles
- *
- * @param data an array of doubles
- * @return the standard deviation of the data
- */
- public static double stdDev(double... data) {
- if (data.length == 0 || data.length == 1) return 0;
-
- double mean = mean(data);
-
- double total = 0;
- for (double datum : data) {
- total += Math.pow(datum - mean, 2);
- }
- return Math.sqrt(total / (data.length - 1));
- }
-
- private static double[] doubleListToArray(List<Double> arrayList) {
- return arrayList.stream().mapToDouble(Double::doubleValue).toArray();
+ mean /= data.length;
+ return mean;
+ }
+
+ /**
+ * Calls {@link #stdDev(double...)}.
+ *
+ * @param data the list of data to find the standard deviation of
+ * @return the standard deviation of the data
+ */
+ public static double stdDev(List<Double> data) {
+ return stdDev(doubleListToArray(data));
+ }
+
+ /**
+ * Finds the standard deviation of the provided array of doubles
+ *
+ * @param data an array of doubles
+ * @return the standard deviation of the data
+ */
+ public static double stdDev(double... data) {
+ if (data.length == 0 || data.length == 1) return 0;
+
+ double mean = mean(data);
+
+ double total = 0;
+ for (double datum : data) {
+ total += Math.pow(datum - mean, 2);
}
+ return Math.sqrt(total / (data.length - 1));
+ }
+ private static double[] doubleListToArray(List<Double> arrayList) {
+ return arrayList.stream().mapToDouble(Double::doubleValue).toArray();
+ }
}
package frc.robot.util;
public class ModifiedCRT {
- private int gearOne;
- private int gearTwo;
- private int turretGear;
+ private int gearOne;
+ private int gearTwo;
+ private int turretGear;
- public ModifiedCRT(int gearOne, int gearTwo, int turretGear) {
- this.gearOne = gearOne;
- this.gearTwo = gearTwo;
- this.turretGear = turretGear;
- }
+ public ModifiedCRT(int gearOne, int gearTwo, int turretGear) {
+ this.gearOne = gearOne;
+ this.gearTwo = gearTwo;
+ this.turretGear = turretGear;
+ }
- public double bruteForce(double encoderLeftRot, double encoderRightRot) {
- double[] encoderLeft = new double[gearOne];
- double[] encoderRight = new double[gearTwo];
+ public double bruteForce(double encoderLeftRot, double encoderRightRot) {
+ double[] encoderLeft = new double[gearOne];
+ double[] encoderRight = new double[gearTwo];
- // Adds all possible positons for encoder left
- for (int n=0; n < gearOne; n++) {
- encoderLeft[n] = (n+encoderLeftRot) * (gearOne/turretGear);
- }
- // Gets all possible encoder two positions
- for (int n=0; n < gearTwo; n++) {
- encoderRight[n] = (n+encoderRightRot) * (gearTwo/turretGear);
- }
-
- for (double a: encoderLeft) {
- for (double b: encoderRight) {
- if (a==b) {
- return a;
- }
- }
- }
- return 0.0;
+ // Adds all possible positons for encoder left
+ for (int n = 0; n < gearOne; n++) {
+ encoderLeft[n] = (n + encoderLeftRot) * (gearOne / turretGear);
+ }
+ // Gets all possible encoder two positions
+ for (int n = 0; n < gearTwo; n++) {
+ encoderRight[n] = (n + encoderRightRot) * (gearTwo / turretGear);
}
- private long modInverse(long a, long m) {
- long m0 = m, t, q;
- long x0 = 0, x1 = 1;
- if (m==1) return 0;
- while (a > 1) {
- q = a / m;
- t = m;
- m = a % m;
- a = t;
-
- t = x0;
- x0 = x1 - q * x0;
- x1 = t;
+ for (double a : encoderLeft) {
+ for (double b : encoderRight) {
+ if (a == b) {
+ return a;
}
+ }
+ }
+ return 0.0;
+ }
- if (x1 < 0) {
- x1 =+ m0;
- }
- return x1;
+ private long modInverse(long a, long m) {
+ long m0 = m, t, q;
+ long x0 = 0, x1 = 1;
+ if (m == 1) return 0;
+ while (a > 1) {
+ q = a / m;
+ t = m;
+ m = a % m;
+ a = t;
+
+ t = x0;
+ x0 = x1 - q * x0;
+ x1 = t;
}
- public double solve(double encoderLeftRot, double encoderRightRot) {
- double r1 = encoderLeftRot * gearOne;
- double r2 = encoderRightRot * gearTwo;
+ if (x1 < 0) {
+ x1 = +m0;
+ }
+ return x1;
+ }
- long m1 = gearOne;
- long m2 = gearTwo;
+ public double solve(double encoderLeftRot, double encoderRightRot) {
+ double r1 = encoderLeftRot * gearOne;
+ double r2 = encoderRightRot * gearTwo;
- long inv = modInverse(m1 % m2, m2);
+ long m1 = gearOne;
+ long m2 = gearTwo;
- double x = r1 + m1 * (((r2-r1) * inv) % m2);
- double combined = x % (m1 * m2);
+ long inv = modInverse(m1 % m2, m2);
- return combined / turretGear;
- }
+ double x = r1 + m1 * (((r2 - r1) * inv) % m2);
+ double combined = x % (m1 * m2);
+ return combined / turretGear;
+ }
}
import frc.robot.constants.Constants;
-/**
- * Utility class for easy creation of motor controllers.
- */
+/** Utility class for easy creation of motor controllers. */
public class MotorFactory {
- private static final int SPARK_MAX_DEFAULT_CURRENT_LIMIT = 60;
-
- ///////////////////////////////////////////////////////////////////////////////////////////////
- // SPARK MAX
- ///////////////////////////////////////////////////////////////////////////////////////////////
-
- /**
- * Create a SparkMax with current limiting enabled
- *
- * @param id the ID of the Spark MAX
- * @param motortype the type of motor the Spark MAX is connected to
- * @param stallLimit the current limit to set at stall
- * @return a fully configured CANSparkMAX
- */
- public static SparkMax createSparkMAX(int id, MotorType motortype, int stallLimit) {
- SparkMax sparkMAX = new SparkMax(id, motortype);
-
- sparkMAX.configure(new SparkMaxConfig()
+ private static final int SPARK_MAX_DEFAULT_CURRENT_LIMIT = 60;
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+ // SPARK MAX
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+
+ /**
+ * Create a SparkMax with current limiting enabled
+ *
+ * @param id the ID of the Spark MAX
+ * @param motortype the type of motor the Spark MAX is connected to
+ * @param stallLimit the current limit to set at stall
+ * @return a fully configured CANSparkMAX
+ */
+ public static SparkMax createSparkMAX(int id, MotorType motortype, int stallLimit) {
+ SparkMax sparkMAX = new SparkMax(id, motortype);
+
+ sparkMAX.configure(
+ new SparkMaxConfig()
.voltageCompensation(Constants.ROBOT_VOLTAGE)
.smartCurrentLimit(stallLimit)
.idleMode(IdleMode.kBrake),
- ResetMode.kResetSafeParameters,
- PersistMode.kNoPersistParameters
- );
- return sparkMAX;
- }
-
- /**
- * Create a SparkMax with default current limiting enabled
- *
- * @param id the ID of the Spark MAX
- * @param motortype the type of motor the Spark MAX is connected to
- * @return a fully configured CANSparkMAX
- */
- public static SparkMax createSparkMAXDefault(int id, MotorType motortype) {
- return createSparkMAX(id, motortype, SPARK_MAX_DEFAULT_CURRENT_LIMIT);
- }
-
- ///////////////////////////////////////////////////////////////////////////////////////////////
- // TALON FX (Falcon 500 and Kraken X60)
- ///////////////////////////////////////////////////////////////////////////////////////////////
-
- /**
- * Creates a TalonFX with all current limit options. If you would like to use
- * defaults it is recommended to use the other createTalonFX.. methods.
- *
- * @param id the CAN ID of the TalonFX
- * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
- * @param StatorLimitEnable whether to enable stator limiting
- * @param StatorCurrentLimit the current, in amps, to return to after the
- * stator limit is triggered
- * @param StatorTriggerThreshold the threshold current to trigger the stator
- * limit
- * @param StatorTriggerDuration the duration, in seconds, the current is above
- * the threshold before triggering
- * @param SupplyLimitEnable whether to enable supply limiting
- * @param SupplyCurrentLimit the current, in amps, to return to after the
- * supply limit is triggered
- * @param SupplyTriggerThreshold the threshold current to trigger the supply
- * limit
- * @param SupplyTriggerDuration the duration, in seconds, the current is above
- * the threshold before triggering
- * @return A fully configured TalonFX
- */
- public static TalonFX createTalonFXFull(int id, CANBus CANBus, boolean StatorLimitEnable,
- double StatorCurrentLimit,
- double StatorTriggerThreshold, double StatorTriggerDuration, boolean SupplyLimitEnable, double SupplyCurrentLimit,
- double SupplyTriggerThreshold, double SupplyTriggerDuration) {
-
- if (id == -1) {
- return null;
- }
-
- TalonFX talon = new TalonFX(id, CANBus);
-
- TalonFXConfiguration config = new TalonFXConfiguration();
-
- // See explanations for Supply and Stator limiting in FalconConstants.java
- config.CurrentLimits = new CurrentLimitsConfigs().withStatorCurrentLimitEnable(StatorLimitEnable).withStatorCurrentLimit(StatorCurrentLimit).
- withSupplyCurrentLimitEnable(SupplyLimitEnable).withSupplyCurrentLimit(SupplyCurrentLimit).
- withSupplyCurrentLowerLimit(SupplyTriggerThreshold).withSupplyCurrentLowerTime(SupplyTriggerDuration);
-
- config.Voltage = new VoltageConfigs().withPeakForwardVoltage(Constants.ROBOT_VOLTAGE);
-
- talon.getConfigurator().apply(config);
- talon.setNeutralMode(NeutralModeValue.Brake);
-
-
- return talon;
- }
-
- /**
- * Creates a TalonFX with all the default settings.
- *
- * @param id the id of the motor
- * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
- */
- public static TalonFX createTalonFX(int id, CANBus CANBus) {
- return createTalonFXFull(id, CANBus, Constants.TALONFX_STATOR_LIMIT_ENABLE, Constants.TALONFX_STATOR_CURRENT_LIMIT,
- Constants.TALONFX_STATOR_TRIGGER_THRESHOLD, Constants.TALONFX_STATOR_TRIGGER_DURATION,
- Constants.TALONFX_SUPPLY_LIMIT_ENABLE, Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
- Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD, Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
+ ResetMode.kResetSafeParameters,
+ PersistMode.kNoPersistParameters);
+ return sparkMAX;
+ }
+
+ /**
+ * Create a SparkMax with default current limiting enabled
+ *
+ * @param id the ID of the Spark MAX
+ * @param motortype the type of motor the Spark MAX is connected to
+ * @return a fully configured CANSparkMAX
+ */
+ public static SparkMax createSparkMAXDefault(int id, MotorType motortype) {
+ return createSparkMAX(id, motortype, SPARK_MAX_DEFAULT_CURRENT_LIMIT);
+ }
+
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+ // TALON FX (Falcon 500 and Kraken X60)
+ ///////////////////////////////////////////////////////////////////////////////////////////////
+
+ /**
+ * Creates a TalonFX with all current limit options. If you would like to use defaults it is
+ * recommended to use the other createTalonFX.. methods.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param StatorLimitEnable whether to enable stator limiting
+ * @param StatorCurrentLimit the current, in amps, to return to after the stator limit is
+ * triggered
+ * @param StatorTriggerThreshold the threshold current to trigger the stator limit
+ * @param StatorTriggerDuration the duration, in seconds, the current is above the threshold
+ * before triggering
+ * @param SupplyLimitEnable whether to enable supply limiting
+ * @param SupplyCurrentLimit the current, in amps, to return to after the supply limit is
+ * triggered
+ * @param SupplyTriggerThreshold the threshold current to trigger the supply limit
+ * @param SupplyTriggerDuration the duration, in seconds, the current is above the threshold
+ * before triggering
+ * @return A fully configured TalonFX
+ */
+ public static TalonFX createTalonFXFull(
+ int id,
+ CANBus CANBus,
+ boolean StatorLimitEnable,
+ double StatorCurrentLimit,
+ double StatorTriggerThreshold,
+ double StatorTriggerDuration,
+ boolean SupplyLimitEnable,
+ double SupplyCurrentLimit,
+ double SupplyTriggerThreshold,
+ double SupplyTriggerDuration) {
+
+ if (id == -1) {
+ return null;
}
- /**
- * Creates a TalonFX with supply current limit options.
- * <p>
- * Supply current is current that's being drawn at the input bus voltage.
- * Supply limiting is useful for preventing breakers from tripping in the PDP.
- *
- * @param id the CAN ID of the TalonFX
- * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
- * @param currentLimit the current, in amps, to return to after the supply limit is triggered
- * @param triggerThreshold the threshold current to trigger the supply limit
- * @param triggerDuration the duration, in seconds, the current is above the threshold before triggering
- */
- public static TalonFX createTalonFXSupplyLimit(int id, CANBus CANBus, double currentLimit,
- double triggerThreshold, double triggerDuration) {
- return createTalonFXFull(id, CANBus, Constants.TALONFX_STATOR_LIMIT_ENABLE, Constants.TALONFX_STATOR_CURRENT_LIMIT,
- Constants.TALONFX_STATOR_TRIGGER_THRESHOLD, Constants.TALONFX_STATOR_TRIGGER_DURATION, true, currentLimit,
- triggerThreshold, triggerDuration);
- }
-
- /**
- * Creates a TalonFX with stator current limit options.
- * <p>
- * Stator current is current that’s being drawn by the motor.
- * Stator limiting is useful for limiting acceleration/heat.
- *
- * @param id the CAN ID of the TalonFX
- * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
- * @param currentLimit the current, in amps, to return to after the stator limit is triggered
- * @param triggerThreshold the threshold current to trigger the stator limit
- * @param triggerDuration the duration, in seconds, the current is above the threshold before triggering
- */
- public static TalonFX createTalonFXStatorLimit(int id, CANBus CANBus, double currentLimit,
- double triggerThreshold, double triggerDuration) {
- return createTalonFXFull(id, CANBus, true, currentLimit, triggerThreshold, triggerDuration,
- Constants.TALONFX_SUPPLY_LIMIT_ENABLE, Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
- Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD, Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
- }
+ TalonFX talon = new TalonFX(id, CANBus);
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+
+ // See explanations for Supply and Stator limiting in FalconConstants.java
+ config.CurrentLimits =
+ new CurrentLimitsConfigs()
+ .withStatorCurrentLimitEnable(StatorLimitEnable)
+ .withStatorCurrentLimit(StatorCurrentLimit)
+ .withSupplyCurrentLimitEnable(SupplyLimitEnable)
+ .withSupplyCurrentLimit(SupplyCurrentLimit)
+ .withSupplyCurrentLowerLimit(SupplyTriggerThreshold)
+ .withSupplyCurrentLowerTime(SupplyTriggerDuration);
+
+ config.Voltage = new VoltageConfigs().withPeakForwardVoltage(Constants.ROBOT_VOLTAGE);
+
+ talon.getConfigurator().apply(config);
+ talon.setNeutralMode(NeutralModeValue.Brake);
+
+ return talon;
+ }
+
+ /**
+ * Creates a TalonFX with all the default settings.
+ *
+ * @param id the id of the motor
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ */
+ public static TalonFX createTalonFX(int id, CANBus CANBus) {
+ return createTalonFXFull(
+ id,
+ CANBus,
+ Constants.TALONFX_STATOR_LIMIT_ENABLE,
+ Constants.TALONFX_STATOR_CURRENT_LIMIT,
+ Constants.TALONFX_STATOR_TRIGGER_THRESHOLD,
+ Constants.TALONFX_STATOR_TRIGGER_DURATION,
+ Constants.TALONFX_SUPPLY_LIMIT_ENABLE,
+ Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
+ Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD,
+ Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
+ }
+
+ /**
+ * Creates a TalonFX with supply current limit options.
+ *
+ * <p>Supply current is current that's being drawn at the input bus voltage. Supply limiting is
+ * useful for preventing breakers from tripping in the PDP.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param currentLimit the current, in amps, to return to after the supply limit is triggered
+ * @param triggerThreshold the threshold current to trigger the supply limit
+ * @param triggerDuration the duration, in seconds, the current is above the threshold before
+ * triggering
+ */
+ public static TalonFX createTalonFXSupplyLimit(
+ int id, CANBus CANBus, double currentLimit, double triggerThreshold, double triggerDuration) {
+ return createTalonFXFull(
+ id,
+ CANBus,
+ Constants.TALONFX_STATOR_LIMIT_ENABLE,
+ Constants.TALONFX_STATOR_CURRENT_LIMIT,
+ Constants.TALONFX_STATOR_TRIGGER_THRESHOLD,
+ Constants.TALONFX_STATOR_TRIGGER_DURATION,
+ true,
+ currentLimit,
+ triggerThreshold,
+ triggerDuration);
+ }
+
+ /**
+ * Creates a TalonFX with stator current limit options.
+ *
+ * <p>Stator current is current that’s being drawn by the motor. Stator limiting is useful for
+ * limiting acceleration/heat.
+ *
+ * @param id the CAN ID of the TalonFX
+ * @param CANBus the CAN bus the TalonFX is on. If connected to the rio it is "rio".
+ * @param currentLimit the current, in amps, to return to after the stator limit is triggered
+ * @param triggerThreshold the threshold current to trigger the stator limit
+ * @param triggerDuration the duration, in seconds, the current is above the threshold before
+ * triggering
+ */
+ public static TalonFX createTalonFXStatorLimit(
+ int id, CANBus CANBus, double currentLimit, double triggerThreshold, double triggerDuration) {
+ return createTalonFXFull(
+ id,
+ CANBus,
+ true,
+ currentLimit,
+ triggerThreshold,
+ triggerDuration,
+ Constants.TALONFX_SUPPLY_LIMIT_ENABLE,
+ Constants.TALONFX_SUPPLY_CURRENT_LIMIT,
+ Constants.TALONFX_SUPPLY_TRIGGER_THRESHOLD,
+ Constants.TALONFX_SUPPLY_TRIGGER_DURATION);
+ }
}
import java.util.HashMap;
import com.pathplanner.lib.path.PathPlannerPath;
-/**
- * Utility class for loading paths using pathplanner.
- */
+/** Utility class for loading paths using pathplanner. */
public class PathGroupLoader {
-
// private static final HashMap<String, List<PathPlannerTrajectory>> pathGroups = new HashMap<>();
- private static final HashMap<String, PathPlannerPath> pathGroups = new HashMap<>();
+ // private static final HashMap<String, List<PathPlannerTrajectory>> pathGroups = new HashMap<>();
+ private static final HashMap<String, PathPlannerPath> pathGroups = new HashMap<>();
+ /**
+ * Loads all the paths in the trajectory directory (specified in the constants). These paths are
+ * loaded and stored so that they do not take time while the robot is running and can be accessed
+ * with {@link #getPathGroup(String)}
+ */
+ public static void loadPathGroups() {
+ double totalTime = 0;
+ File[] directoryListing =
+ Filesystem.getDeployDirectory()
+ .toPath()
+ .resolve(AutoConstants.TRAJECTORY_DIRECTORY)
+ .toFile()
+ .listFiles();
- /**
- * Loads all the paths in the trajectory directory (specified in the constants).
- * These paths are loaded and stored so that they do not take time while the robot is running
- * and can be accessed with {@link #getPathGroup(String)}
- */
- public static void loadPathGroups() {
- double totalTime = 0;
- File[] directoryListing = Filesystem.getDeployDirectory().toPath().resolve(AutoConstants.TRAJECTORY_DIRECTORY).toFile().listFiles();
-
- if (directoryListing != null) {
- for (File file : directoryListing) {
- if (file.isFile() && file.getName().contains(".")) {
- try {
- long startTime = System.nanoTime();
- String name = file.getName().substring(0, file.getName().lastIndexOf("."));
- // pathGroups.put(name, PathPlannerAuto.getPathGroupFromAutoFile(name));
- pathGroups.put(name, PathPlannerPath.fromPathFile(name));
- double time = (System.nanoTime() - startTime) / 1000000.0;
- totalTime += time;
- System.out.println("Processed file: " + file.getName() + ", took " + time + " milliseconds.");
- } catch (Exception e) {
- DriverStation.reportError(e.getMessage(), true);
- }
- }
- }
- } else {
- System.out.println("Error processing file");
- DriverStation.reportWarning(
- "Issue with finding path files. Paths will not be loaded.",
- true
- );
+ if (directoryListing != null) {
+ for (File file : directoryListing) {
+ if (file.isFile() && file.getName().contains(".")) {
+ try {
+ long startTime = System.nanoTime();
+ String name = file.getName().substring(0, file.getName().lastIndexOf("."));
+ // pathGroups.put(name, PathPlannerAuto.getPathGroupFromAutoFile(name));
+ pathGroups.put(name, PathPlannerPath.fromPathFile(name));
+ double time = (System.nanoTime() - startTime) / 1000000.0;
+ totalTime += time;
+ System.out.println(
+ "Processed file: " + file.getName() + ", took " + time + " milliseconds.");
+ } catch (Exception e) {
+ DriverStation.reportError(e.getMessage(), true);
+ }
}
- System.out.println("File processing took a total of " + totalTime + " milliseconds");
+ }
+ } else {
+ System.out.println("Error processing file");
+ DriverStation.reportWarning("Issue with finding path files. Paths will not be loaded.", true);
}
+ System.out.println("File processing took a total of " + totalTime + " milliseconds");
+ }
- /**
- * Gets a path that has already been loaded with {@link #loadPathGroups()}. The path group is a list
- * of trajectories that path planner can run.
- *
- * @param pathGroupName the name of the file, without any extensions. This should be the same exact name that is displayed in pathplanner
- * @return a list of trajectories that path planner can run.
- */
- public static PathPlannerPath getPathGroup(String pathGroupName) {
- if (pathGroups.get(pathGroupName) == null) {
- System.out.println("Error retrieving " + pathGroupName + " path!");
- }
- return pathGroups.get(pathGroupName);
+ /**
+ * Gets a path that has already been loaded with {@link #loadPathGroups()}. The path group is a
+ * list of trajectories that path planner can run.
+ *
+ * @param pathGroupName the name of the file, without any extensions. This should be the same
+ * exact name that is displayed in pathplanner
+ * @return a list of trajectories that path planner can run.
+ */
+ public static PathPlannerPath getPathGroup(String pathGroupName) {
+ if (pathGroups.get(pathGroupName) == null) {
+ System.out.println("Error retrieving " + pathGroupName + " path!");
}
-
-
-}
\ No newline at end of file
+ return pathGroups.get(pathGroupName);
+ }
+}
while (true) {
// Wait for updates from all signals
signalsLock.lock();
- try {
- BaseStatusSignal.waitForAll(2.0 / 250, phoenixSignals);}
- finally {
+ try {
+ BaseStatusSignal.waitForAll(2.0 / 250, phoenixSignals);
+ } finally {
signalsLock.unlock();
}
}
}
}
-}
\ No newline at end of file
+}
package frc.robot.util;
+
import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusCode;
import java.util.function.Supplier;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import frc.robot.subsystems.drivetrain.Drivetrain;
-/**
- * Stores and updates the position of each module
- */
+/** Stores and updates the position of each module */
public class SwerveModulePose {
- private double[] dist = {0,0,0,0};
- private Translation2d[] moduleTranslations;
- private Pose2d[] modulePositions;
- private double[] angles;
- private Drivetrain drive;
- private double prevRotation;
- private Pose2d[] displayPoses;
+ private double[] dist = {0, 0, 0, 0};
+ private Translation2d[] moduleTranslations;
+ private Pose2d[] modulePositions;
+ private double[] angles;
+ private Drivetrain drive;
+ private double prevRotation;
+ private Pose2d[] displayPoses;
- /**
- * Creates a new SwerveModulePose object to store and update the positions of each module
- * @param drive The drivetrain
- * @param modulePositions The translations of the modules relative to the center of the robot
- */
- public SwerveModulePose(Drivetrain drive, Translation2d... modulePositions){
- this.drive = drive;
- this.moduleTranslations = modulePositions;
- this.modulePositions = new Pose2d[4];
- angles = new double[4];
- reset();
- update();
- reset();
- }
+ /**
+ * Creates a new SwerveModulePose object to store and update the positions of each module
+ *
+ * @param drive The drivetrain
+ * @param modulePositions The translations of the modules relative to the center of the robot
+ */
+ public SwerveModulePose(Drivetrain drive, Translation2d... modulePositions) {
+ this.drive = drive;
+ this.moduleTranslations = modulePositions;
+ this.modulePositions = new Pose2d[4];
+ angles = new double[4];
+ reset();
+ update();
+ reset();
+ }
- /**
- * Updates the module positions
- */
- public void update(){
- SwerveModuleState[] states = drive.getModuleStates();
- double currentRotation = drive.getYaw().getRadians();
- double chassisRotation = currentRotation - prevRotation;
+ /** Updates the module positions */
+ public void update() {
+ SwerveModuleState[] states = drive.getModuleStates();
+ double currentRotation = drive.getYaw().getRadians();
+ double chassisRotation = currentRotation - prevRotation;
- for(int i = 0; i<4; i++){
- double position = drive.getModules()[i].getPosition().distanceMeters;
- double distance = position - dist[i];
- dist[i] = position;
-
- Twist2d twist = new Twist2d(distance, 0, MathUtil.angleModulus(states[i].angle.getRadians()-angles[i] + chassisRotation));
- angles[i] = states[i].angle.getRadians();
- modulePositions[i] = modulePositions[i].exp(twist);
+ for (int i = 0; i < 4; i++) {
+ double position = drive.getModules()[i].getPosition().distanceMeters;
+ double distance = position - dist[i];
+ dist[i] = position;
- displayPoses[i] = new Pose2d(
- modulePositions[i].getTranslation(),
- EqualsUtil.epsilonEquals(states[i].speedMetersPerSecond, 0, 0.01) ? displayPoses[i].getRotation() :
- states[i].speedMetersPerSecond < 0 ? modulePositions[i].getRotation().plus(new Rotation2d(Math.PI)) :
- modulePositions[i].getRotation()
- );
- }
- prevRotation = currentRotation;
- }
+ Twist2d twist =
+ new Twist2d(
+ distance,
+ 0,
+ MathUtil.angleModulus(states[i].angle.getRadians() - angles[i] + chassisRotation));
+ angles[i] = states[i].angle.getRadians();
+ modulePositions[i] = modulePositions[i].exp(twist);
- /**
- * Gets the positions of the modules
- * @return The module poses as an array of Pose2ds
- */
- public Pose2d[] getModulePoses(){
- return displayPoses;
+ displayPoses[i] =
+ new Pose2d(
+ modulePositions[i].getTranslation(),
+ EqualsUtil.epsilonEquals(states[i].speedMetersPerSecond, 0, 0.01)
+ ? displayPoses[i].getRotation()
+ : states[i].speedMetersPerSecond < 0
+ ? modulePositions[i].getRotation().plus(new Rotation2d(Math.PI))
+ : modulePositions[i].getRotation());
}
+ prevRotation = currentRotation;
+ }
+
+ /**
+ * Gets the positions of the modules
+ *
+ * @return The module poses as an array of Pose2ds
+ */
+ public Pose2d[] getModulePoses() {
+ return displayPoses;
+ }
- /**
- * Resets the modules to the correct positions relative to the robot
- */
- public void reset(){
- Pose2d chassisPose2d = drive.getPose();
- SwerveModuleState[] states = drive.getModuleStates();
- for (int i = 0; i<4; i++){
- angles[i] = states[i].angle.getRadians();
- this.modulePositions[i] = new Pose2d(moduleTranslations[i].rotateBy(chassisPose2d.getRotation()).plus(chassisPose2d.getTranslation()), new Rotation2d(angles[i]).plus(chassisPose2d.getRotation()));
- }
- prevRotation = drive.getYaw().getRadians();
- displayPoses = Arrays.copyOf(modulePositions, 4);
+ /** Resets the modules to the correct positions relative to the robot */
+ public void reset() {
+ Pose2d chassisPose2d = drive.getPose();
+ SwerveModuleState[] states = drive.getModuleStates();
+ for (int i = 0; i < 4; i++) {
+ angles[i] = states[i].angle.getRadians();
+ this.modulePositions[i] =
+ new Pose2d(
+ moduleTranslations[i]
+ .rotateBy(chassisPose2d.getRotation())
+ .plus(chassisPose2d.getTranslation()),
+ new Rotation2d(angles[i]).plus(chassisPose2d.getRotation()));
}
+ prevRotation = drive.getYaw().getRadians();
+ displayPoses = Arrays.copyOf(modulePositions, 4);
+ }
- /**
- * Gets whehter or not the modules have slipped
- * A module has slipped if it has moved 0.3m (about 1ft) from its correct position relative to the other modules
- * @return True if any of the modules have slipped, false otherwise
- */
- public boolean slipped(){
- Translation2d total = new Translation2d();
- for(Pose2d pose : modulePositions){
- total = total.plus(pose.getTranslation());
- }
- Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
- for(int i = 0; i < 4; i++){
- double dist = modulePositions[i].relativeTo(drivePose).getTranslation().getDistance(moduleTranslations[i]);
- if(dist > 0.3){
- return true;
- }
- }
- return false;
+ /**
+ * Gets whehter or not the modules have slipped A module has slipped if it has moved 0.3m (about
+ * 1ft) from its correct position relative to the other modules
+ *
+ * @return True if any of the modules have slipped, false otherwise
+ */
+ public boolean slipped() {
+ Translation2d total = new Translation2d();
+ for (Pose2d pose : modulePositions) {
+ total = total.plus(pose.getTranslation());
}
+ Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
+ for (int i = 0; i < 4; i++) {
+ double dist =
+ modulePositions[i]
+ .relativeTo(drivePose)
+ .getTranslation()
+ .getDistance(moduleTranslations[i]);
+ if (dist > 0.3) {
+ return true;
+ }
+ }
+ return false;
+ }
- /**
- * Gets the average slip distance
- * @return The average distance between each module and its correct position
- */
- public double getAverageSlip(){
- Translation2d total = new Translation2d();
- for(Pose2d pose : modulePositions){
- total = total.plus(pose.getTranslation());
- }
- Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
- double slip = 0;
- for(int i = 0; i < 4; i++){
- slip += modulePositions[i].relativeTo(drivePose).getTranslation().getDistance(moduleTranslations[i]);
- }
- return slip/4;
+ /**
+ * Gets the average slip distance
+ *
+ * @return The average distance between each module and its correct position
+ */
+ public double getAverageSlip() {
+ Translation2d total = new Translation2d();
+ for (Pose2d pose : modulePositions) {
+ total = total.plus(pose.getTranslation());
+ }
+ Pose2d drivePose = new Pose2d(total.div(4), drive.getYaw());
+ double slip = 0;
+ for (int i = 0; i < 4; i++) {
+ slip +=
+ modulePositions[i]
+ .relativeTo(drivePose)
+ .getTranslation()
+ .getDistance(moduleTranslations[i]);
}
-}
\ No newline at end of file
+ return slip / 4;
+ }
+}
package frc.robot.util.SwerveStuff;
public record ModuleLimits(
- double maxDriveVelocity, double maxDriveAcceleration, double staticFriction, double maxSteeringVelocity) {}
+ double maxDriveVelocity,
+ double maxDriveAcceleration,
+ double staticFriction,
+ double maxSteeringVelocity) {}
* robot will converge to the desired setpoint quickly while avoiding any intermediate state that is
* kinematically infeasible (and can result in wheel slip or robot heading drift as a result).
*/
-
public class SwerveSetpointGenerator {
- private final SwerveDriveKinematics kinematics= DriveConstants.KINEMATICS;
+ private final SwerveDriveKinematics kinematics = DriveConstants.KINEMATICS;
private final Translation2d[] moduleLocations = DriveConstants.MODULE_LOCATIONS;
/**
}
/**
- * Limits the acceleration in all directions.
- * This is different from findDriveMaxS because it includes the acceleration perpendicular to the wheel as it rotates.
- * Given the same velocity step, this will return a lower S value than findDriveMaxS.
+ * Limits the acceleration in all directions. This is different from findDriveMaxS because it
+ * includes the acceleration perpendicular to the wheel as it rotates. Given the same velocity
+ * step, this will return a lower S value than findDriveMaxS.
+ *
* @param x_0 The initial x velocity
* @param y_0 The initial y velocity
* @param x_1 The final x velocity
* @return The maximum interpolation value
*/
protected double findAccelerationMaxS(
- double x_0,
- double y_0,
- double x_1,
- double y_1,
- double max_vel_step,
- int max_iterations) {
- double dist = Math.hypot(x_1-x_0, y_1-y_0);
- if(dist <= max_vel_step){
+ double x_0, double y_0, double x_1, double y_1, double max_vel_step, int max_iterations) {
+ double dist = Math.hypot(x_1 - x_0, y_1 - y_0);
+ if (dist <= max_vel_step) {
return 1;
}
return Math.max(0.0, Math.min(1.0, max_vel_step / dist));
*
* @param limits The kinematic limits to respect for this setpoint.
* @param centerOfMassHeight The height of the robot's center of mass, in meters, off the ground.
- * This assumes that the center of mass is in the center of the robot in the x and y directions.
- * If tipping is not a potential problem this year, set this to 0.
+ * This assumes that the center of mass is in the center of the robot in the x and y
+ * directions. If tipping is not a potential problem this year, set this to 0.
* @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous
* iteration setpoint instead of the actual measured/estimated kinematic state.
* @param desiredState The desired state of motion, such as from the driver sticks or a path
// Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so
// just use the previous angle.
boolean need_to_steer = true;
- if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) {
+ if (EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState), new Twist2d())) {
need_to_steer = false;
for (int i = 0; i < modules.length; ++i) {
desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle;
}
}
if (all_modules_should_flip
- && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()),new Twist2d())
- && !EqualsUtil.GeomExtensions.epsilonEquals(GeomUtil.toTwist2d(desiredState),new Twist2d())) {
+ && !EqualsUtil.GeomExtensions.epsilonEquals(
+ GeomUtil.toTwist2d(prevSetpoint.chassisSpeeds()), new Twist2d())
+ && !EqualsUtil.GeomExtensions.epsilonEquals(
+ GeomUtil.toTwist2d(desiredState), new Twist2d())) {
// It will (likely) be faster to stop the robot, rotate the modules in place to the complement
// of the desired
// angle, and accelerate again.
// already know we can't go faster
// than that.
final int kMaxIterations = 10;
- double s = min_s*findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step);
-
+ double s = min_s * findDriveMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step);
+
// Limit the overall acceleration of this wheel
- double s2 = min_s*findAccelerationMaxS(prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations);
+ double s2 =
+ min_s
+ * findAccelerationMaxS(
+ prev_vx[i], prev_vy[i], vx_min_s, vy_min_s, max_vel_step_2, kMaxIterations);
min_s = Math.min(Math.min(min_s, s), s2);
}
- if(centerOfMassHeight > 0.02){
+ if (centerOfMassHeight > 0.02) {
// Limit the acceleration in the x and y directions separately based on the center of mass.
- // To make the torque on the robot 0, we can assume all of the mass is on the back wheel, where the front is the direction the robot is accelerating toward
+ // To make the torque on the robot 0, we can assume all of the mass is on the back wheel,
+ // where the front is the direction the robot is accelerating toward
// Torque is equal to the force times the component of the radius perpendicular to the force
- // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = distance from center to wheel
+ // T = torque, m = mass, a = acceleration, g = gravity acceleration, x = distance from center
+ // to wheel
// T = mgx - mah = 0
// a = gx/h
- double maxAccel = Constants.GRAVITY_ACCELERATION*(DriveConstants.TRACK_WIDTH/2)/centerOfMassHeight;
+ double maxAccel =
+ Constants.GRAVITY_ACCELERATION * (DriveConstants.TRACK_WIDTH / 2) / centerOfMassHeight;
// Limit based on this calculated value
- // x and y are limited separately because, when tipping in a diagonal direction, the distance is longer
- double xAccel = Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond) / dt;
- double yAccel = Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond) / dt;
- if(!epsilonEquals(xAccel, 0)){
+ // x and y are limited separately because, when tipping in a diagonal direction, the distance
+ // is longer
+ double xAccel =
+ Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond)
+ / dt;
+ double yAccel =
+ Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond)
+ / dt;
+ if (!epsilonEquals(xAccel, 0)) {
double s = maxAccel / xAccel;
min_s = Math.min(min_s, s);
}
- if(!epsilonEquals(yAccel, 0)){
+ if (!epsilonEquals(yAccel, 0)) {
double s = maxAccel / yAccel;
min_s = Math.min(min_s, s);
}
retStates[i].speedMetersPerSecond *= -1.0;
}
retStates[i].angle = override;
- }
+ }
final var deltaRotation =
prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle);
if (flipHeading(deltaRotation)) {
// the WPILib BSD license file in the root directory of this project.
package frc.robot.util;
+
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
import edu.wpi.first.wpilibj2.command.Command;
import java.util.function.Consumer;
-
-/**
- * Util class for creating SysId routines
-*/
+/** Util class for creating SysId routines */
public class SysId {
- private SysIdRoutine sysIdRoutine;
-
- public SysId(String name, Consumer<Voltage> driveConsumer, Consumer<SysIdRoutineLog> logConsumer, Subsystem subsystem, Config config){
- sysIdRoutine = new SysIdRoutine(
- config,
- new Mechanism(
- driveConsumer,
- logConsumer,
- subsystem,
- name
- )
- );
- }
- public SysId(String name, Consumer<Voltage> driveConsumer, Subsystem subsystem, Config config){
- this(name,driveConsumer,null,subsystem,config);
- }
-
- public Command runQuasisStatic(Direction direction){
- return sysIdRoutine.quasistatic(direction);
- }
- public Command runDynamic(Direction direction){
- return sysIdRoutine.dynamic(direction);
- }
-}
\ No newline at end of file
+ private SysIdRoutine sysIdRoutine;
+
+ public SysId(
+ String name,
+ Consumer<Voltage> driveConsumer,
+ Consumer<SysIdRoutineLog> logConsumer,
+ Subsystem subsystem,
+ Config config) {
+ sysIdRoutine =
+ new SysIdRoutine(config, new Mechanism(driveConsumer, logConsumer, subsystem, name));
+ }
+
+ public SysId(String name, Consumer<Voltage> driveConsumer, Subsystem subsystem, Config config) {
+ this(name, driveConsumer, null, subsystem, config);
+ }
+
+ public Command runQuasisStatic(Direction direction) {
+ return sysIdRoutine.quasistatic(direction);
+ }
+
+ public Command runDynamic(Direction direction) {
+ return sysIdRoutine.dynamic(direction);
+ }
+}
*/
public class TimeAccuracyTest {
- private final BooleanSupplier accuracyTest;
- private final double setpointUpdateTime;
- private final double errorMargin;
- private final double timeMargin;
- private boolean lastUseableResult = false;
+ private final BooleanSupplier accuracyTest;
+ private final double setpointUpdateTime;
+ private final double errorMargin;
+ private final double timeMargin;
+ private boolean lastUseableResult = false;
- /**
- * @param actual DoubleSupplier that returns the actual value
- * @param setpoint DoubleSupplier that returns the setpoint
- * @param errorMargin margin of error for the test to be accurate
- * @param timeMargin time in seconds that the setpoint must be held for the test to be accurate
- */
- public TimeAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint, double errorMargin, double timeMargin) {
- this.errorMargin = errorMargin;
- this.timeMargin = timeMargin;
- setpointUpdateTime = WPIUtilJNI.now() * 1e-6;
- accuracyTest = () -> getDoubleAccuracyTest(actual, setpoint);
- }
+ /**
+ * @param actual DoubleSupplier that returns the actual value
+ * @param setpoint DoubleSupplier that returns the setpoint
+ * @param errorMargin margin of error for the test to be accurate
+ * @param timeMargin time in seconds that the setpoint must be held for the test to be accurate
+ */
+ public TimeAccuracyTest(
+ DoubleSupplier actual, DoubleSupplier setpoint, double errorMargin, double timeMargin) {
+ this.errorMargin = errorMargin;
+ this.timeMargin = timeMargin;
+ setpointUpdateTime = WPIUtilJNI.now() * 1e-6;
+ accuracyTest = () -> getDoubleAccuracyTest(actual, setpoint);
+ }
- /**
- * Determines if the test is successful.
- *
- * @return true if the test is successful, false if not
- */
- public boolean calculate() {
- if (setpointUpdateTime + timeMargin <= WPIUtilJNI.now() * 1e-6)
- lastUseableResult = accuracyTest.getAsBoolean();
- return lastUseableResult;
- }
+ /**
+ * Determines if the test is successful.
+ *
+ * @return true if the test is successful, false if not
+ */
+ public boolean calculate() {
+ if (setpointUpdateTime + timeMargin <= WPIUtilJNI.now() * 1e-6)
+ lastUseableResult = accuracyTest.getAsBoolean();
+ return lastUseableResult;
+ }
- /**
- * Determines if the actual value is within the error margin of the setpoint.
- *
- * @return true if the actual value is within the error margin of the setpoint, false if not
- */
- private boolean getDoubleAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint) {
- return Math.abs(actual.getAsDouble() - setpoint.getAsDouble()) <= errorMargin;
- }
+ /**
+ * Determines if the actual value is within the error margin of the setpoint.
+ *
+ * @return true if the actual value is within the error margin of the setpoint, false if not
+ */
+ private boolean getDoubleAccuracyTest(DoubleSupplier actual, DoubleSupplier setpoint) {
+ return Math.abs(actual.getAsDouble() - setpoint.getAsDouble()) <= errorMargin;
+ }
}
import frc.robot.Robot;
import frc.robot.subsystems.drivetrain.Drivetrain;
-/**
- * Stores information about an object detected by vision
- */
+/** Stores information about an object detected by vision */
public class DetectedObject {
- private static Drivetrain drive;
- public final Pose3d pose;
- public final ObjectType type;
+ private static Drivetrain drive;
+ public final Pose3d pose;
+ public final ObjectType type;
- public enum ObjectType{
- CORAL(Units.inchesToMeters(4.5/2)),
- ALGAE(Units.inchesToMeters(16.25/2)),
- RED_ROBOT(0),
- BLUE_ROBOT(0),
- NONE(0);
+ public enum ObjectType {
+ CORAL(Units.inchesToMeters(4.5 / 2)),
+ ALGAE(Units.inchesToMeters(16.25 / 2)),
+ RED_ROBOT(0),
+ BLUE_ROBOT(0),
+ NONE(0);
- public final double height;
+ public final double height;
- private ObjectType(double h){
- height = h;
- }
- };
-
- /**
- * Sets the drivetrain to use for pose calculations
- * @param drive The drivetrain
- */
- public static void setDrive(Drivetrain drive){
- DetectedObject.drive = drive;
+ private ObjectType(double h) {
+ height = h;
}
+ };
- /**
- * Creates a default DetectedObject with default attributes
- */
- public DetectedObject(){
- pose = new Pose3d();
- type = ObjectType.NONE;
- }
+ /**
+ * Sets the drivetrain to use for pose calculations
+ *
+ * @param drive The drivetrain
+ */
+ public static void setDrive(Drivetrain drive) {
+ DetectedObject.drive = drive;
+ }
- /**
- * Creates a new DetectedObject
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param distance The distance from the camera to the object in meters
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- */
- public DetectedObject(double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera){
- this(xOffset, yOffset, distance, type, robotToCamera, -1);
- }
+ /** Creates a default DetectedObject with default attributes */
+ public DetectedObject() {
+ pose = new Pose3d();
+ type = ObjectType.NONE;
+ }
+
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, distance, type, robotToCamera, -1);
+ }
- /**
- * Creates a new DetectedObject
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param distance The distance from the camera to the object in meters
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- * @param timestamp The timestamp of the picture in seconds
- */
- public DetectedObject(double xOffset, double yOffset, double distance, ObjectType type, Transform3d robotToCamera, double timestamp){
- this.type = type;
- // Get the position relative to the camera
- Translation3d translation = new Translation3d(distance, new Rotation3d(0, -yOffset, -xOffset))
- // Rotate and translate it to get the position relative to the robot
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset,
+ double yOffset,
+ double distance,
+ ObjectType type,
+ Transform3d robotToCamera,
+ double timestamp) {
+ this.type = type;
+ // Get the position relative to the camera
+ Translation3d translation =
+ new Translation3d(distance, new Rotation3d(0, -yOffset, -xOffset))
+ // Rotate and translate it to get the position relative to the robot
.rotateBy(robotToCamera.getRotation())
.plus(robotToCamera.getTranslation());
- // If the drivetrain exists, rotate and translate it to get the field relative position
- if(drive != null){
- Pose2d drivePose = drive.getPoseAt(timestamp);
- translation = translation.rotateBy(new Rotation3d(
- 0,
- 0,
- drivePose.getRotation().getRadians()
- )).plus(new Translation3d(
- drivePose.getX(),
- drivePose.getY(),
- 0
- ));
- }
- pose = new Pose3d(translation, new Rotation3d());
- }
-
- /**
- * Creates a new DetectedObject
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param distance The distance from the camera to the object in meters
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- * @param timestamp The timestamp of the picture in seconds
- */
- public DetectedObject(double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera, double timestamp){
- this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+ // If the drivetrain exists, rotate and translate it to get the field relative position
+ if (drive != null) {
+ Pose2d drivePose = drive.getPoseAt(timestamp);
+ translation =
+ translation
+ .rotateBy(new Rotation3d(0, 0, drivePose.getRotation().getRadians()))
+ .plus(new Translation3d(drivePose.getX(), drivePose.getY(), 0));
}
+ pose = new Pose3d(translation, new Rotation3d());
+ }
- /**
- * Creates a new DetectedObject
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param distance The distance from the camera to the object in meters
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- */
- public DetectedObject(double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera){
- this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
- }
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset,
+ double yOffset,
+ double distance,
+ int type,
+ Transform3d robotToCamera,
+ double timestamp) {
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+ }
- /**
- * Creates a new DetectedObject
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param distance The distance from the camera to the object in meters
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- * @param timestamp The timestamp of the picture in seconds
- */
- public DetectedObject(double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera, double timestamp){
- this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
- }
-
- /**
- * Creates a new DetectedObject
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param distance The distance from the camera to the object in meters
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- */
- public DetectedObject(double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera){
- this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
- }
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, double distance, int type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
+ }
- /**
- * Creates a new DetectedObject, assuming the object is on the ground
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- */
- public DetectedObject(double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera){
- this(xOffset, yOffset, type, robotToCamera, -1);
- }
-
- /**
- * Creates a new DetectedObject, assuming the object is on the ground
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- * @param timestamp The timestamp of the picture in seconds
- */
- public DetectedObject(double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera, double timestamp){
- this.type = type;
- // Get the position relative to the camera
- Translation3d translation = new Translation3d(1, new Rotation3d(0, -yOffset, -xOffset))
- // Rotate it to get the position relative to the rotated camera
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset,
+ double yOffset,
+ double distance,
+ String type,
+ Transform3d robotToCamera,
+ double timestamp) {
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, timestamp);
+ }
+
+ /**
+ * Creates a new DetectedObject
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param distance The distance from the camera to the object in meters
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, double distance, String type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, distance, getType(type), robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, ObjectType type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, type, robotToCamera, -1);
+ }
+
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset,
+ double yOffset,
+ ObjectType type,
+ Transform3d robotToCamera,
+ double timestamp) {
+ this.type = type;
+ // Get the position relative to the camera
+ Translation3d translation =
+ new Translation3d(1, new Rotation3d(0, -yOffset, -xOffset))
+ // Rotate it to get the position relative to the rotated camera
.rotateBy(robotToCamera.getRotation());
- // Scale it so that the object will be on the ground (- because translation's z will be negative)
- if(!isRobot()){
- translation = translation.times(-(robotToCamera.getZ()-type.height)/translation.getZ());
- }else{
- // Assume all robots are ~3m from the camera
- translation = translation.times(3);
- }
- // Translate it to make it relative to the robot
- translation = translation.plus(robotToCamera.getTranslation());
- // If the drivetrain exists, rotate and translate it to be field relative
- if(drive != null){
- Pose2d drivePose = drive.getPoseAt(timestamp);
- translation = translation.rotateBy(new Rotation3d(
- 0,
- 0,
- drivePose.getRotation().getRadians()
- )).plus(new Translation3d(
- drivePose.getX(),
- drivePose.getY(),
- 0
- ));
- }
- pose = new Pose3d(translation, new Rotation3d());
+ // Scale it so that the object will be on the ground (- because translation's z will be
+ // negative)
+ if (!isRobot()) {
+ translation = translation.times(-(robotToCamera.getZ() - type.height) / translation.getZ());
+ } else {
+ // Assume all robots are ~3m from the camera
+ translation = translation.times(3);
}
-
- /**
- * Creates a new DetectedObject, assuming the object is on the ground
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- * @param timestamp The timestamp of the picture in seconds
- */
- public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera, double timestamp){
- this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+ // Translate it to make it relative to the robot
+ translation = translation.plus(robotToCamera.getTranslation());
+ // If the drivetrain exists, rotate and translate it to be field relative
+ if (drive != null) {
+ Pose2d drivePose = drive.getPoseAt(timestamp);
+ translation =
+ translation
+ .rotateBy(new Rotation3d(0, 0, drivePose.getRotation().getRadians()))
+ .plus(new Translation3d(drivePose.getX(), drivePose.getY(), 0));
}
+ pose = new Pose3d(translation, new Rotation3d());
+ }
- /**
- * Creates a new DetectedObject, assuming the object is on the ground
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- */
- public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera){
- this(xOffset, yOffset, getType(type), robotToCamera, -1);
- }
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, int type, Transform3d robotToCamera, double timestamp) {
+ this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+ }
- /**
- * Creates a new DetectedObject, assuming the object is on the ground
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- * @param timestamp The timestamp of the picture in seconds
- */
- public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera, double timestamp){
- this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
- }
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, int type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, getType(type), robotToCamera, -1);
+ }
- /**
- * Creates a new DetectedObject, assuming the object is on the ground
- * @param xOffset The x offset from the camera to the object in radians
- * @param yOffset The y offset form the camera to the object in radians
- * @param type What type of object it is
- * @param robotToCamera The transformation form the robot to the camera
- */
- public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera){
- this(xOffset, yOffset, getType(type), robotToCamera, -1);
- }
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ * @param timestamp The timestamp of the picture in seconds
+ */
+ public DetectedObject(
+ double xOffset, double yOffset, String type, Transform3d robotToCamera, double timestamp) {
+ this(xOffset, yOffset, getType(type), robotToCamera, timestamp);
+ }
- /**
- * Converts an int to an ObjectType
- * @param type The type as an int, between 0 and the number of object types - 1
- * @return The type as an ObjectType
- */
- public static ObjectType getType(int type){
- ObjectType[] values = ObjectType.values();
- if(type < 0 || type >= values.length){
- return ObjectType.NONE;
- }
- return values[type];
- }
+ /**
+ * Creates a new DetectedObject, assuming the object is on the ground
+ *
+ * @param xOffset The x offset from the camera to the object in radians
+ * @param yOffset The y offset form the camera to the object in radians
+ * @param type What type of object it is
+ * @param robotToCamera The transformation form the robot to the camera
+ */
+ public DetectedObject(double xOffset, double yOffset, String type, Transform3d robotToCamera) {
+ this(xOffset, yOffset, getType(type), robotToCamera, -1);
+ }
- /**
- * Converts a String to an ObjectType
- * @param type The type as a String
- * @return The type as an ObjectType
- */
- public static ObjectType getType(String type){
- ObjectType result = ObjectType.valueOf(type.toUpperCase());
- return result==null ? ObjectType.NONE : result;
+ /**
+ * Converts an int to an ObjectType
+ *
+ * @param type The type as an int, between 0 and the number of object types - 1
+ * @return The type as an ObjectType
+ */
+ public static ObjectType getType(int type) {
+ ObjectType[] values = ObjectType.values();
+ if (type < 0 || type >= values.length) {
+ return ObjectType.NONE;
}
+ return values[type];
+ }
- /**
- * Returns if the object is a game piece
- * @return True if the object is a game piece, false otherwise
- */
- public boolean isGamePiece(){
- return type==ObjectType.CORAL || type==ObjectType.ALGAE;
- }
- /**
- * Returns if the object is a robot
- * @return True if the object is a red or blue robot, false otherwise
- */
- public boolean isRobot(){
- return type==ObjectType.RED_ROBOT || type==ObjectType.BLUE_ROBOT;
- }
- /**
- * Returns if the object is a robot on the same alliance
- * @return If the object is a robot on the same alliance
- */
- public boolean isSameAllianceRobot(){
- return type == (Robot.getAlliance()==Alliance.Red?ObjectType.RED_ROBOT:ObjectType.BLUE_ROBOT);
- }
- /**
- * Returns if the object is a robot on the other alliance
- * @return If the object is a robot on the other alliance
- */
- public boolean isOtherAllianceRobot(){
- return type == (Robot.getAlliance()==Alliance.Red?ObjectType.BLUE_ROBOT:ObjectType.RED_ROBOT);
- }
+ /**
+ * Converts a String to an ObjectType
+ *
+ * @param type The type as a String
+ * @return The type as an ObjectType
+ */
+ public static ObjectType getType(String type) {
+ ObjectType result = ObjectType.valueOf(type.toUpperCase());
+ return result == null ? ObjectType.NONE : result;
+ }
- /**
- * Gets the distance from the center of the robot to the object
- * @return The distance in meters
- */
- public double getDistance(){
- return drive.getPose().getTranslation().getDistance(pose.getTranslation().toTranslation2d());
- }
+ /**
+ * Returns if the object is a game piece
+ *
+ * @return True if the object is a game piece, false otherwise
+ */
+ public boolean isGamePiece() {
+ return type == ObjectType.CORAL || type == ObjectType.ALGAE;
+ }
- /**
- * Gets the field relative angle from the robot to the object
- * @return The angle in radians
- */
- public double getAngle(){
- Pose2d drivePose = drive.getPose();
- return Math.atan2(pose.getY()-drivePose.getY(), pose.getX()-drivePose.getX());
- }
+ /**
+ * Returns if the object is a robot
+ *
+ * @return True if the object is a red or blue robot, false otherwise
+ */
+ public boolean isRobot() {
+ return type == ObjectType.RED_ROBOT || type == ObjectType.BLUE_ROBOT;
+ }
- /**
- * Gets the angle relative to the front of the robot (0 is in front, positive counterclockwise)
- * @return The relative angle in radians
- */
- public double getRelativeAngle(){
- double angle = getAngle()-drive.getYaw().getRadians();
- return MathUtil.angleModulus(angle);
- }
+ /**
+ * Returns if the object is a robot on the same alliance
+ *
+ * @return If the object is a robot on the same alliance
+ */
+ public boolean isSameAllianceRobot() {
+ return type
+ == (Robot.getAlliance() == Alliance.Red ? ObjectType.RED_ROBOT : ObjectType.BLUE_ROBOT);
+ }
- /**
- * Gets the angle of the object relative to the robot's velocity (0 is in front, positive counterclockwise)
- * @return The relative angle in radians
- */
- public double getVelocityRelativeAngle(){
- ChassisSpeeds speeds = drive.getChassisSpeeds();
- double angle = getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond);
- return MathUtil.angleModulus(angle);
- }
+ /**
+ * Returns if the object is a robot on the other alliance
+ *
+ * @return If the object is a robot on the other alliance
+ */
+ public boolean isOtherAllianceRobot() {
+ return type
+ == (Robot.getAlliance() == Alliance.Red ? ObjectType.BLUE_ROBOT : ObjectType.RED_ROBOT);
+ }
- public String toString(){
- return type+" at ("+pose.getX()+", "+pose.getY()+", "+pose.getZ()+")";
- }
+ /**
+ * Gets the distance from the center of the robot to the object
+ *
+ * @return The distance in meters
+ */
+ public double getDistance() {
+ return drive.getPose().getTranslation().getDistance(pose.getTranslation().toTranslation2d());
+ }
+
+ /**
+ * Gets the field relative angle from the robot to the object
+ *
+ * @return The angle in radians
+ */
+ public double getAngle() {
+ Pose2d drivePose = drive.getPose();
+ return Math.atan2(pose.getY() - drivePose.getY(), pose.getX() - drivePose.getX());
+ }
+
+ /**
+ * Gets the angle relative to the front of the robot (0 is in front, positive counterclockwise)
+ *
+ * @return The relative angle in radians
+ */
+ public double getRelativeAngle() {
+ double angle = getAngle() - drive.getYaw().getRadians();
+ return MathUtil.angleModulus(angle);
+ }
+
+ /**
+ * Gets the angle of the object relative to the robot's velocity (0 is in front, positive
+ * counterclockwise)
+ *
+ * @return The relative angle in radians
+ */
+ public double getVelocityRelativeAngle() {
+ ChassisSpeeds speeds = drive.getChassisSpeeds();
+ double angle =
+ getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond);
+ return MathUtil.angleModulus(angle);
+ }
+
+ public String toString() {
+ return type + " at (" + pose.getX() + ", " + pose.getY() + ", " + pose.getZ() + ")";
+ }
}
import frc.robot.util.SwerveStuff.SwerveSetpoint;
import frc.robot.util.SwerveStuff.SwerveSetpointGenerator;
-/**
- * A util class to assist the driver drive to a pose
- */
+/** A util class to assist the driver drive to a pose */
public class DriverAssist {
- // The amount to correct the driver's inpus by
- // 0 = return unchanged driver inputs, 1 = return a value much closer to the calculated speed, sometimes equal to it
- // This can be greater than 1 to fully correct more of the time, like from farther away
- private static final double CORRECTION_FACTOR = 1;
-
-
- // Variables used for first method
- // The setpoint generator, which limits the acceleration
- private static final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
- private static final TrapezoidProfile xProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
- private static final TrapezoidProfile yProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
- private static final TrapezoidProfile angleProfile = new TrapezoidProfile(new Constraints(DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL));
-
- /**
- * Combines the driver input with a speed calculated using a trapezoidal profile <p>
- * Called when VisionConstants.DRIVER_ASSIST_MODE is 2
- * @param drive The drivetrain
- * @param driverInput The driver input speed
- * @param desiredPose The pose to drive to
- * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
- * @return The new speed
- */
- private static ChassisSpeeds calculate2(Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
- // Do nothing if there is no pose
- if(desiredPose == null){
- return driverInput;
- }
-
- // Store current states
- Pose2d currentPose = drive.getPose();
- Rotation2d yaw = drive.getYaw();
- ChassisSpeeds driveSpeeds = drive.getChassisSpeeds();
- driveSpeeds = ChassisSpeeds. fromFieldRelativeSpeeds(driveSpeeds,yaw); // Changing this does not cause problems because getChassisSpeeds() creates a new object
- State xState = new State(currentPose.getX(), driveSpeeds.vxMetersPerSecond);
- State yState = new State(currentPose.getY(), driveSpeeds.vyMetersPerSecond);
- State angleState = new State(currentPose.getRotation().getRadians(), driveSpeeds.omegaRadiansPerSecond);
-
- // Store goal states
- State xGoal = new State(desiredPose.getX(), 0);
- State yGoal = new State(desiredPose.getY(), 0);
- Translation2d difference = desiredPose.getTranslation().minus(currentPose.getTranslation());
- double rotation = keepAngle ? desiredPose.getRotation().getRadians() : difference.getAngle().getRadians();
- if(rotation - currentPose.getRotation().getRadians() > Math.PI){
- rotation -= 2*Math.PI;
- }else if(rotation - currentPose.getRotation().getRadians() < -Math.PI){
- rotation += 2*Math.PI;
- }
- State angleGoal = new State(rotation, 0);
-
- // Calculate ideal speeds for next frame
- ChassisSpeeds goal = new ChassisSpeeds(
+ // The amount to correct the driver's inpus by
+ // 0 = return unchanged driver inputs, 1 = return a value much closer to the calculated speed,
+ // sometimes equal to it
+ // This can be greater than 1 to fully correct more of the time, like from farther away
+ private static final double CORRECTION_FACTOR = 1;
+
+ // Variables used for first method
+ // The setpoint generator, which limits the acceleration
+ private static final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator();
+ private static final TrapezoidProfile xProfile =
+ new TrapezoidProfile(
+ new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
+ private static final TrapezoidProfile yProfile =
+ new TrapezoidProfile(
+ new Constraints(DriveConstants.MAX_SPEED, DriveConstants.MAX_LINEAR_ACCEL));
+ private static final TrapezoidProfile angleProfile =
+ new TrapezoidProfile(
+ new Constraints(DriveConstants.MAX_ANGULAR_SPEED, DriveConstants.MAX_ANGULAR_ACCEL));
+
+ /**
+ * Combines the driver input with a speed calculated using a trapezoidal profile
+ *
+ * <p>Called when VisionConstants.DRIVER_ASSIST_MODE is 2
+ *
+ * @param drive The drivetrain
+ * @param driverInput The driver input speed
+ * @param desiredPose The pose to drive to
+ * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
+ * @return The new speed
+ */
+ private static ChassisSpeeds calculate2(
+ Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+ // Do nothing if there is no pose
+ if (desiredPose == null) {
+ return driverInput;
+ }
+
+ // Store current states
+ Pose2d currentPose = drive.getPose();
+ Rotation2d yaw = drive.getYaw();
+ ChassisSpeeds driveSpeeds = drive.getChassisSpeeds();
+ driveSpeeds =
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ driveSpeeds,
+ yaw); // Changing this does not cause problems because getChassisSpeeds() creates a new
+ // object
+ State xState = new State(currentPose.getX(), driveSpeeds.vxMetersPerSecond);
+ State yState = new State(currentPose.getY(), driveSpeeds.vyMetersPerSecond);
+ State angleState =
+ new State(currentPose.getRotation().getRadians(), driveSpeeds.omegaRadiansPerSecond);
+
+ // Store goal states
+ State xGoal = new State(desiredPose.getX(), 0);
+ State yGoal = new State(desiredPose.getY(), 0);
+ Translation2d difference = desiredPose.getTranslation().minus(currentPose.getTranslation());
+ double rotation =
+ keepAngle ? desiredPose.getRotation().getRadians() : difference.getAngle().getRadians();
+ if (rotation - currentPose.getRotation().getRadians() > Math.PI) {
+ rotation -= 2 * Math.PI;
+ } else if (rotation - currentPose.getRotation().getRadians() < -Math.PI) {
+ rotation += 2 * Math.PI;
+ }
+ State angleGoal = new State(rotation, 0);
+
+ // Calculate ideal speeds for next frame
+ ChassisSpeeds goal =
+ new ChassisSpeeds(
xProfile.calculate(Constants.LOOP_TIME, xState, xGoal).velocity,
yProfile.calculate(Constants.LOOP_TIME, yState, yGoal).velocity,
- angleProfile.calculate(Constants.LOOP_TIME, angleState, angleGoal).velocity
- );
- // Robot-relataive goal
- ChassisSpeeds goalRobot = goal.times(1);
- goalRobot = ChassisSpeeds. fromRobotRelativeSpeeds(goalRobot, yaw);
-
- // This calculates the actual acceleration we can get
- // This is the only thing that needs to be robot relative
- SwerveSetpoint nextSetpoint = setpointGenerator.generateSetpoint(
- DriveConstants.MODULE_LIMITS,
- 0,
- drive.getCurrSetpoint(), goalRobot,
- Constants.LOOP_TIME);
- ChassisSpeeds nextChassisSpeed = nextSetpoint.chassisSpeeds();
- nextChassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds(nextChassisSpeed, yaw);
-
- // Robot relative driver inputs
- ChassisSpeeds driverInputRobot = driverInput.times(1); // Copy so original doesn't change
- driverInputRobot = ChassisSpeeds.fromFieldRelativeSpeeds(driverInputRobot, yaw);
- // This is the speed the driver will be able to get next frame
- // Both speeds need to be obtainable in 1 frame or the driver speed will always be farther away
- SwerveSetpoint driverSetpoint = setpointGenerator.generateSetpoint(
- DriveConstants.MODULE_LIMITS,
- 0,
- drive.getCurrSetpoint(), driverInputRobot,
- Constants.LOOP_TIME);
- ChassisSpeeds driverSpeeds = driverSetpoint.chassisSpeeds();
- driverSpeeds= ChassisSpeeds.fromRobotRelativeSpeeds(driverSpeeds,yaw);
-
- // The difference between the 2 speeds
- ChassisSpeeds error = nextChassisSpeed.minus(driverSpeeds);
-
- // 1.2*1.2^-distance decreases the amount it correct by as distance increases
- double distanceFactor = 1.2*Math.pow(1.2, -currentPose.getTranslation().getDistance(desiredPose.getTranslation()));
-
- // Driver input speed
- double driverInputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
-
- // The amount to correct by
- ChassisSpeeds correction = error.times(Math.min(CORRECTION_FACTOR * distanceFactor * driverInputSpeed / DriveConstants.MAX_SPEED, 1));
-
- return driverSpeeds.plus(correction);
- // return nextChassisSpeed.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR));
- }
+ angleProfile.calculate(Constants.LOOP_TIME, angleState, angleGoal).velocity);
+ // Robot-relataive goal
+ ChassisSpeeds goalRobot = goal.times(1);
+ goalRobot = ChassisSpeeds.fromRobotRelativeSpeeds(goalRobot, yaw);
+
+ // This calculates the actual acceleration we can get
+ // This is the only thing that needs to be robot relative
+ SwerveSetpoint nextSetpoint =
+ setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ 0,
+ drive.getCurrSetpoint(),
+ goalRobot,
+ Constants.LOOP_TIME);
+ ChassisSpeeds nextChassisSpeed = nextSetpoint.chassisSpeeds();
+ nextChassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds(nextChassisSpeed, yaw);
+
+ // Robot relative driver inputs
+ ChassisSpeeds driverInputRobot = driverInput.times(1); // Copy so original doesn't change
+ driverInputRobot = ChassisSpeeds.fromFieldRelativeSpeeds(driverInputRobot, yaw);
+ // This is the speed the driver will be able to get next frame
+ // Both speeds need to be obtainable in 1 frame or the driver speed will always be farther away
+ SwerveSetpoint driverSetpoint =
+ setpointGenerator.generateSetpoint(
+ DriveConstants.MODULE_LIMITS,
+ 0,
+ drive.getCurrSetpoint(),
+ driverInputRobot,
+ Constants.LOOP_TIME);
+ ChassisSpeeds driverSpeeds = driverSetpoint.chassisSpeeds();
+ driverSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(driverSpeeds, yaw);
- // Constants used for second method
- public static final double MAX_VELOCITY_ANGLE_ERROR = Math.PI/4;
- public static final double MAX_DISTANCE_ERROR = 2;
- public static final double ROTATION_CORRECTION_FACTOR = 0.1;
- public static final double MAX_ROTATION_ERROR = Math.PI/3;
-
- /**
- * Combines the driver input with a calculated correction speed
- * @param drive The drivetrain
- * @param driverInput The driver input speed
- * @param desiredPose The pose to drive to
- * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
- * @return The new speed
- */
- @SuppressWarnings("unused") // Needed because some code might not run for some values of DRIVER_ASSIST_MODE
- public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
- if(VisionConstants.DRIVER_ASSIST_MODE < 2 || desiredPose == null){
- return driverInput;
- }else if(VisionConstants.DRIVER_ASSIST_MODE == 2){
- return calculate2(drive, driverInput, desiredPose, keepAngle);
- }
- // Combines the driver input with a speed perpendicular to the input
- Pose2d drivePose = drive.getPose();
- Translation2d difference = desiredPose.getTranslation().minus(drivePose.getTranslation());
- double distance = difference.getNorm();
- double velocityAngle = difference.getAngle().getRadians();
- double targetAngle = keepAngle ? desiredPose.getRotation().getRadians() : MathUtil.angleModulus(velocityAngle + Math.PI/2);
- double inputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
- double driverAngle = Math.atan2(driverInput.vyMetersPerSecond, driverInput.vxMetersPerSecond);
- double velocityAngleError = MathUtil.angleModulus(velocityAngle - driverAngle);
- if(Math.abs(velocityAngleError) > MAX_VELOCITY_ANGLE_ERROR){
- return driverInput;
- }
- double perpendicularDist = Math.abs(distance * Math.sin(velocityAngleError));
- if(perpendicularDist > MAX_DISTANCE_ERROR){
- return driverInput;
- }
- double perpendicularAngle = MathUtil.angleModulus(driverAngle + Math.PI/2*Math.signum(velocityAngleError));
- // Different options for calculation.
- double correctionSpeed = 0;
- switch(VisionConstants.DRIVER_ASSIST_MODE){
- case 3:
- correctionSpeed = Math.min(CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist), Math.abs(Math.tan(velocityAngleError)*inputSpeed));
- break;
- case 4:
- correctionSpeed = Math.min(CORRECTION_FACTOR * Math.pow(1.5, -perpendicularDist), 1) * Math.abs(Math.tan(velocityAngleError)*inputSpeed);
- break;
- case 5:
- correctionSpeed = Math.min(CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist) * Math.pow(1.2, -distance+1), Math.abs(Math.tan(velocityAngleError)*inputSpeed));
- break;
- }
- double rotationError = MathUtil.angleModulus(targetAngle-drivePose.getRotation().getRadians());
- if(Math.abs(rotationError) > MAX_ROTATION_ERROR){
- return driverInput;
- }
- // We want to set the current angular velocity so that we can decelerate to 0rad/s at the setpoint
- // Since 0=v0^2+2ax, v0=√(2ax)
- // High correction factors will also ignore the driver's input more
- double rotationalSpeed = ROTATION_CORRECTION_FACTOR * Math.signum(rotationError)*Math.sqrt(2*DriveConstants.MAX_ANGULAR_ACCEL*Math.abs(rotationError)) - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond;
- return driverInput.plus(new ChassisSpeeds(correctionSpeed*Math.cos(perpendicularAngle), correctionSpeed*Math.sin(perpendicularAngle), rotationalSpeed));
+ // The difference between the 2 speeds
+ ChassisSpeeds error = nextChassisSpeed.minus(driverSpeeds);
+
+ // 1.2*1.2^-distance decreases the amount it correct by as distance increases
+ double distanceFactor =
+ 1.2
+ * Math.pow(
+ 1.2, -currentPose.getTranslation().getDistance(desiredPose.getTranslation()));
+
+ // Driver input speed
+ double driverInputSpeed =
+ Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+
+ // The amount to correct by
+ ChassisSpeeds correction =
+ error.times(
+ Math.min(
+ CORRECTION_FACTOR * distanceFactor * driverInputSpeed / DriveConstants.MAX_SPEED,
+ 1));
+
+ return driverSpeeds.plus(correction);
+ // return
+ // nextChassisSpeed.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR));
+ }
+
+ // Constants used for second method
+ public static final double MAX_VELOCITY_ANGLE_ERROR = Math.PI / 4;
+ public static final double MAX_DISTANCE_ERROR = 2;
+ public static final double ROTATION_CORRECTION_FACTOR = 0.1;
+ public static final double MAX_ROTATION_ERROR = Math.PI / 3;
+
+ /**
+ * Combines the driver input with a calculated correction speed
+ *
+ * @param drive The drivetrain
+ * @param driverInput The driver input speed
+ * @param desiredPose The pose to drive to
+ * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose
+ * @return The new speed
+ */
+ @SuppressWarnings(
+ "unused") // Needed because some code might not run for some values of DRIVER_ASSIST_MODE
+ public static ChassisSpeeds calculate(
+ Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) {
+ if (VisionConstants.DRIVER_ASSIST_MODE < 2 || desiredPose == null) {
+ return driverInput;
+ } else if (VisionConstants.DRIVER_ASSIST_MODE == 2) {
+ return calculate2(drive, driverInput, desiredPose, keepAngle);
+ }
+ // Combines the driver input with a speed perpendicular to the input
+ Pose2d drivePose = drive.getPose();
+ Translation2d difference = desiredPose.getTranslation().minus(drivePose.getTranslation());
+ double distance = difference.getNorm();
+ double velocityAngle = difference.getAngle().getRadians();
+ double targetAngle =
+ keepAngle
+ ? desiredPose.getRotation().getRadians()
+ : MathUtil.angleModulus(velocityAngle + Math.PI / 2);
+ double inputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond);
+ double driverAngle = Math.atan2(driverInput.vyMetersPerSecond, driverInput.vxMetersPerSecond);
+ double velocityAngleError = MathUtil.angleModulus(velocityAngle - driverAngle);
+ if (Math.abs(velocityAngleError) > MAX_VELOCITY_ANGLE_ERROR) {
+ return driverInput;
+ }
+ double perpendicularDist = Math.abs(distance * Math.sin(velocityAngleError));
+ if (perpendicularDist > MAX_DISTANCE_ERROR) {
+ return driverInput;
+ }
+ double perpendicularAngle =
+ MathUtil.angleModulus(driverAngle + Math.PI / 2 * Math.signum(velocityAngleError));
+ // Different options for calculation.
+ double correctionSpeed = 0;
+ switch (VisionConstants.DRIVER_ASSIST_MODE) {
+ case 3:
+ correctionSpeed =
+ Math.min(
+ CORRECTION_FACTOR * inputSpeed * Math.pow(2, -perpendicularDist),
+ Math.abs(Math.tan(velocityAngleError) * inputSpeed));
+ break;
+ case 4:
+ correctionSpeed =
+ Math.min(CORRECTION_FACTOR * Math.pow(1.5, -perpendicularDist), 1)
+ * Math.abs(Math.tan(velocityAngleError) * inputSpeed);
+ break;
+ case 5:
+ correctionSpeed =
+ Math.min(
+ CORRECTION_FACTOR
+ * inputSpeed
+ * Math.pow(2, -perpendicularDist)
+ * Math.pow(1.2, -distance + 1),
+ Math.abs(Math.tan(velocityAngleError) * inputSpeed));
+ break;
+ }
+ double rotationError =
+ MathUtil.angleModulus(targetAngle - drivePose.getRotation().getRadians());
+ if (Math.abs(rotationError) > MAX_ROTATION_ERROR) {
+ return driverInput;
}
+ // We want to set the current angular velocity so that we can decelerate to 0rad/s at the
+ // setpoint
+ // Since 0=v0^2+2ax, v0=√(2ax)
+ // High correction factors will also ignore the driver's input more
+ double rotationalSpeed =
+ ROTATION_CORRECTION_FACTOR
+ * Math.signum(rotationError)
+ * Math.sqrt(2 * DriveConstants.MAX_ANGULAR_ACCEL * Math.abs(rotationError))
+ - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond;
+ return driverInput.plus(
+ new ChassisSpeeds(
+ correctionSpeed * Math.cos(perpendicularAngle),
+ correctionSpeed * Math.sin(perpendicularAngle),
+ rotationalSpeed));
+ }
}
/**
* estimates gyro bias by comparing vision-derived yaw to gyro yaw.
*
- * when the robot observes April tags, PhotonVision calculates what the robot heading
- * SHOULD be based on known tag positions vs observed angles. This can be compared to
- * the gyro reading to detect and correct drift.
- *
+ * <p>when the robot observes April tags, PhotonVision calculates what the robot heading SHOULD be
+ * based on known tag positions vs observed angles. This can be compared to the gyro reading to
+ * detect and correct drift.
*/
public class GyroBiasEstimator {
- private double weightedBiasSum = 0.0;
- private double totalWeight = 0.0;
- private int sampleCount = 0;
-
- // exponential moving average
- private double emaBias = 0.0;
- private boolean emaInitialized = false;
-
- /**
- * process a new observation comparing vision pose to gyro reading.
- *
- * @param visionPose the pose estimated by vision (from PhotonVision)
- * @param gyroYaw current gyro reading in radians
- * @param visionWeight weight for observation (0.0 to 1.0, higher is more trusted)
- * @return true if bias should be applied (has enough samples)
- */
- public boolean addObservation(Pose3d visionPose, double gyroYaw, double visionWeight) {
- if (visionPose == null) {
- return false;
- }
-
- // get yaw from vision
- double visionYaw = visionPose.getRotation().getZ();
-
- return addObservation(visionYaw, gyroYaw, visionWeight);
+ private double weightedBiasSum = 0.0;
+ private double totalWeight = 0.0;
+ private int sampleCount = 0;
+
+ // exponential moving average
+ private double emaBias = 0.0;
+ private boolean emaInitialized = false;
+
+ /**
+ * process a new observation comparing vision pose to gyro reading.
+ *
+ * @param visionPose the pose estimated by vision (from PhotonVision)
+ * @param gyroYaw current gyro reading in radians
+ * @param visionWeight weight for observation (0.0 to 1.0, higher is more trusted)
+ * @return true if bias should be applied (has enough samples)
+ */
+ public boolean addObservation(Pose3d visionPose, double gyroYaw, double visionWeight) {
+ if (visionPose == null) {
+ return false;
}
- /**
- * process a new observation with just yaw values.
- *
- * @param visionYaw yaw from vision pose in radians
- * @param gyroYaw current gyro reading in radians
- * @param visionWeight weight for this observation (0.0 to 1.0, higher is more trusted)
- * @return true if bias should be applied
- */
- public boolean addObservation(double visionYaw, double gyroYaw, double visionWeight) {
- // normalize to [-PI, PI]
- double diff = normalizeAngle(visionYaw - gyroYaw);
-
- // reject outliers
- if (Math.abs(diff) > GyroBiasConstants.MAX_ANGLE_DIFF_RAD) {
- return false;
- }
-
- // clamp weight
- double weight = Math.max(0.0, Math.min(1.0, visionWeight));
-
- // accumulate weighted bias
- weightedBiasSum += diff * weight;
- totalWeight += weight;
- sampleCount++;
-
- // update exponential moving average
- if (!emaInitialized) {
- emaBias = diff;
- emaInitialized = true;
- } else {
- emaBias = emaBias * (1.0 - GyroBiasConstants.EMA_ALPHA) + diff * GyroBiasConstants.EMA_ALPHA;
- }
-
- return sampleCount >= GyroBiasConstants.MIN_SAMPLES;
+ // get yaw from vision
+ double visionYaw = visionPose.getRotation().getZ();
+
+ return addObservation(visionYaw, gyroYaw, visionWeight);
+ }
+
+ /**
+ * process a new observation with just yaw values.
+ *
+ * @param visionYaw yaw from vision pose in radians
+ * @param gyroYaw current gyro reading in radians
+ * @param visionWeight weight for this observation (0.0 to 1.0, higher is more trusted)
+ * @return true if bias should be applied
+ */
+ public boolean addObservation(double visionYaw, double gyroYaw, double visionWeight) {
+ // normalize to [-PI, PI]
+ double diff = normalizeAngle(visionYaw - gyroYaw);
+
+ // reject outliers
+ if (Math.abs(diff) > GyroBiasConstants.MAX_ANGLE_DIFF_RAD) {
+ return false;
}
- /**
- * process new observation with default weight of 1.0.
- * maintains backward compatibility.
- */
- public boolean addObservation(Pose3d visionPose, double gyroYaw) {
- return addObservation(visionPose, gyroYaw, 1.0);
- }
+ // clamp weight
+ double weight = Math.max(0.0, Math.min(1.0, visionWeight));
- /**
- * process new observation with default weight of 1.0.
- * maintains backward compatibility
- */
- public boolean addObservation(double visionYaw, double gyroYaw) {
- return addObservation(visionYaw, gyroYaw, 1.0);
- }
+ // accumulate weighted bias
+ weightedBiasSum += diff * weight;
+ totalWeight += weight;
+ sampleCount++;
- /**
- * get average bias and reset
- *
- * @return average bias in radians to apply, or 0 if not enough samples
- */
- public double getAndResetBias() {
- if (sampleCount < GyroBiasConstants.MIN_SAMPLES) {
- return 0.0;
- }
-
- // use weighted average
- double avgBias = weightedBiasSum / totalWeight;
-
- // reset
- weightedBiasSum = 0.0;
- totalWeight = 0.0;
- sampleCount = 0;
- emaInitialized = false;
-
- return avgBias;
+ // update exponential moving average
+ if (!emaInitialized) {
+ emaBias = diff;
+ emaInitialized = true;
+ } else {
+ emaBias = emaBias * (1.0 - GyroBiasConstants.EMA_ALPHA) + diff * GyroBiasConstants.EMA_ALPHA;
}
- /**
- * apply partial correction to avoid sudden jumps.
- *
- * @param fullBias the full calculated bias
- * @return partial correction to apply
- */
- public double applyPartialCorrection(double fullBias) {
- double clampedBias = fullBias;
- if (clampedBias > GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) {
- clampedBias = GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD;
- } else if (clampedBias < -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) {
- clampedBias = -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD;
- }
-
- return clampedBias * GyroBiasConstants.CORRECTION_FRACTION;
+ return sampleCount >= GyroBiasConstants.MIN_SAMPLES;
+ }
+
+ /** process new observation with default weight of 1.0. maintains backward compatibility. */
+ public boolean addObservation(Pose3d visionPose, double gyroYaw) {
+ return addObservation(visionPose, gyroYaw, 1.0);
+ }
+
+ /** process new observation with default weight of 1.0. maintains backward compatibility */
+ public boolean addObservation(double visionYaw, double gyroYaw) {
+ return addObservation(visionYaw, gyroYaw, 1.0);
+ }
+
+ /**
+ * get average bias and reset
+ *
+ * @return average bias in radians to apply, or 0 if not enough samples
+ */
+ public double getAndResetBias() {
+ if (sampleCount < GyroBiasConstants.MIN_SAMPLES) {
+ return 0.0;
}
- /**
- * normalize angle to [-PI, PI]
- */
- private double normalizeAngle(double angle) {
- return MathUtil.angleModulus(angle);
+ // use weighted average
+ double avgBias = weightedBiasSum / totalWeight;
+
+ // reset
+ weightedBiasSum = 0.0;
+ totalWeight = 0.0;
+ sampleCount = 0;
+ emaInitialized = false;
+
+ return avgBias;
+ }
+
+ /**
+ * apply partial correction to avoid sudden jumps.
+ *
+ * @param fullBias the full calculated bias
+ * @return partial correction to apply
+ */
+ public double applyPartialCorrection(double fullBias) {
+ double clampedBias = fullBias;
+ if (clampedBias > GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) {
+ clampedBias = GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD;
+ } else if (clampedBias < -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) {
+ clampedBias = -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD;
}
- /**
- * get sample count for debugging
- */
- public int getSampleCount() {
- return sampleCount;
- }
+ return clampedBias * GyroBiasConstants.CORRECTION_FRACTION;
+ }
- /**
- * get current accumulated bias without resetting
- */
- public double getCurrentBias() {
- if (sampleCount == 0) {
- return 0.0;
- }
- if (totalWeight > 0) {
- return weightedBiasSum / totalWeight;
- }
- return emaBias;
- }
+ /** normalize angle to [-PI, PI] */
+ private double normalizeAngle(double angle) {
+ return MathUtil.angleModulus(angle);
+ }
- /**
- * get current total weight for debugging
- */
- public double getTotalWeight() {
- return totalWeight;
- }
+ /** get sample count for debugging */
+ public int getSampleCount() {
+ return sampleCount;
+ }
- /**
- * reset accumulated state
- */
- public void reset() {
- weightedBiasSum = 0.0;
- totalWeight = 0.0;
- sampleCount = 0;
- emaInitialized = false;
+ /** get current accumulated bias without resetting */
+ public double getCurrentBias() {
+ if (sampleCount == 0) {
+ return 0.0;
+ }
+ if (totalWeight > 0) {
+ return weightedBiasSum / totalWeight;
}
+ return emaBias;
+ }
+
+ /** get current total weight for debugging */
+ public double getTotalWeight() {
+ return totalWeight;
+ }
+
+ /** reset accumulated state */
+ public void reset() {
+ weightedBiasSum = 0.0;
+ totalWeight = 0.0;
+ sampleCount = 0;
+ emaInitialized = false;
+ }
}
private NetworkTableEntry objectDistance;
private NetworkTableEntry objectClass;
private NetworkTableEntry cameraIndex;
-
+
// A list of the cameras on the robot.
private ArrayList<VisionCamera> cameras = new ArrayList<>();
// Array of tags to use, null or empty array to use all tags
private int[] onlyUse = null;
- /**
- * Creates a new instance of Vision and sets up the cameras and field layout
- */
+ /** Creates a new instance of Vision and sets up the cameras and field layout */
public Vision(ArrayList<Pair<String, Transform3d>> camList) {
// Initialize object_detection NetworkTable
objectDetectionTable = NetworkTableInstance.getDefault().getTable("object_detection");
// Sets the origin to the right side of the blue alliance wall
FieldConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
- if(VisionConstants.ENABLED){
+ if (VisionConstants.ENABLED) {
// Puts the cameras in an array list
for (int i = 0; i < camList.size(); i++) {
cameras.add(new VisionCamera(camList.get(i).getFirst(), camList.get(i).getSecond()));
}
- if(RobotBase.isSimulation()){
+ if (RobotBase.isSimulation()) {
visionSim = new VisionSystemSim("Vision");
visionSim.addAprilTags(FieldConstants.field);
- for(VisionCamera c : cameras){
+ for (VisionCamera c : cameras) {
PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera);
cameraSim.enableDrawWireframe(true);
cameraSim.prop.setAvgLatencyMs(30);
Pose3d[] tags = new Pose3d[FieldConstants.field.getTags().size()];
for (int i = 0; i < FieldConstants.field.getTags().size(); i++) {
- tags[i] = (FieldConstants.field.getTagPose(i+1).get());
+ tags[i] = (FieldConstants.field.getTagPose(i + 1).get());
}
if (!Constants.DISABLE_LOGGING) {
Logger.recordOutput("AprilTags", tags);
}
}
-
/**
* Get the horizontal offsets from the crosshair to the targets
+ *
* @return An array of offsets in degrees
*/
- public double[] getHorizontalOffset(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ public double[] getHorizontalOffset() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
return new double[0];
}
return xOffset.getDoubleArray(new double[0]);
/**
* Get the vertical offsets from the crosshair to the targets
+ *
* @return An array of offsets in degrees
*/
- public double[] getVerticalOffset(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ public double[] getVerticalOffset() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
return new double[0];
}
return yOffset.getDoubleArray(new double[0]);
/**
* Get the target distances
+ *
* @return Distance in meters
*/
@SuppressWarnings("unused")
- public double[] getDistance(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ public double[] getDistance() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
return new double[0];
}
return objectDistance.getDoubleArray(new double[0]);
/**
* Returns whether or not a valid object is detected
+ *
* @return true or false
*/
- public boolean validObjectDetected(){
+ public boolean validObjectDetected() {
return getHorizontalOffset().length > 0;
}
/**
* Returns what types of object are detected
+ *
* @return The object types as a String array
*/
@SuppressWarnings("unused")
- public String[] getDetectedObjectClass(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ public String[] getDetectedObjectClass() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
return new String[0];
}
return objectClass.getStringArray(new String[0]);
/**
* Gets the camera indices (which camera sees the object)
+ *
* @return The indices as a long array (method returns long array instead of int array)
*/
@SuppressWarnings("unused")
- public long[] getCameraIndex(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED || true){
+ public long[] getCameraIndex() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED || true) {
return new long[0];
}
return cameraIndex.getIntegerArray(new long[0]);
/**
* Stores all of the detected objects in an array
+ *
* @return The array of DetectedObjects
*/
- public DetectedObject[] getDetectedObjects(){
- if(!VisionConstants.OBJECT_DETECTION_ENABLED){
+ public DetectedObject[] getDetectedObjects() {
+ if (!VisionConstants.OBJECT_DETECTION_ENABLED) {
return new DetectedObject[0];
}
double[] xOffset = getHorizontalOffset();
String[] objectClass = getDetectedObjectClass();
// long[] cameraIndex = getCameraIndex();
DetectedObject[] objects = new DetectedObject[Math.min(xOffset.length, yOffset.length)];
- for(int i = 0; i < objects.length; i++){
- objects[i] = new DetectedObject(
- Units.degreesToRadians(xOffset[i]),
- -Units.degreesToRadians(yOffset[i]),
- // distance[i],
- objectClass[i],
- // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond()
- VisionConstants.OBJECT_DETECTION_CAMERAS.get(0)
- );
+ for (int i = 0; i < objects.length; i++) {
+ objects[i] =
+ new DetectedObject(
+ Units.degreesToRadians(xOffset[i]),
+ -Units.degreesToRadians(yOffset[i]),
+ // distance[i],
+ objectClass[i],
+ // VisionConstants.OBJECT_DETECTION_CAMERAS.get((int)cameraIndex[i]).getSecond()
+ VisionConstants.OBJECT_DETECTION_CAMERAS.get(0));
}
return objects;
}
/**
* Returns the closest game piece in front of the robot
- * @param maxAngle The maximum angle between the angle to the object and the robot's heading or rotation to use, in radians
- * @param relativeToVelocity Whether to compare the angle to the robot's heading or rotation, true for heading
+ *
+ * @param maxAngle The maximum angle between the angle to the object and the robot's heading or
+ * rotation to use, in radians
+ * @param relativeToVelocity Whether to compare the angle to the robot's heading or rotation, true
+ * for heading
* @return The best DetectedObject
*/
- public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity){
+ public DetectedObject getBestGamePiece(double maxAngle, boolean relativeToVelocity) {
DetectedObject[] objects = getDetectedObjects();
DetectedObject best = null;
double closest = Double.POSITIVE_INFINITY;
- for(DetectedObject object : objects){
+ for (DetectedObject object : objects) {
double dist = object.getDistance();
- if(object.isGamePiece() && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle()) < maxAngle && dist < closest){
+ if (object.isGamePiece()
+ && Math.abs(relativeToVelocity ? object.getVelocityRelativeAngle() : object.getAngle())
+ < maxAngle
+ && dist < closest) {
closest = dist;
best = object;
}
/**
* Gets the pose as a Pose2d using PhotonVision
+ *
* @param referencePoses The reference poses in order of preference, null poses will be skipped
* @return The pose of the robot, or null if it can't see april tags
*/
- public Pose2d getPose2d(Pose2d... referencePoses){
+ public Pose2d getPose2d(Pose2d... referencePoses) {
Pose2d referencePose = new Pose2d();
- for (Pose2d checkReferencePose:referencePoses){
+ for (Pose2d checkReferencePose : referencePoses) {
if (checkReferencePose != null) {
referencePose = checkReferencePose;
break;
}
}
ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(referencePose);
-
+
if (estimatedPoses.size() == 0) return null;
if (estimatedPoses.size() == 1) return estimatedPoses.get(0).estimatedPose.toPose2d();
-
+
if (estimatedPoses.size() == 2) {
return new Pose2d(
- estimatedPoses.get(0).estimatedPose.toPose2d().getTranslation()
- .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation())
- .div(2),
-
- new Rotation2d(MathUtils.modulusMidpoint(
- estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(),
- estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(),
- -Math.PI, Math.PI)
- )
- );
+ estimatedPoses
+ .get(0)
+ .estimatedPose
+ .toPose2d()
+ .getTranslation()
+ .plus(estimatedPoses.get(1).estimatedPose.toPose2d().getTranslation())
+ .div(2),
+ new Rotation2d(
+ MathUtils.modulusMidpoint(
+ estimatedPoses.get(0).estimatedPose.toPose2d().getRotation().getRadians(),
+ estimatedPoses.get(1).estimatedPose.toPose2d().getRotation().getRadians(),
+ -Math.PI,
+ Math.PI)));
}
-
+
// The average translation is just the average of all of the translations (sum divided by total)
- // Average angle is similar, except every step needs to use a modulus, since -π is the same angle as π
- // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + nextValue) / newNumberOfValues
+ // Average angle is similar, except every step needs to use a modulus, since -Ï€ is the same
+ // angle as π
+ // This calculation is essentially newAverage = (oldAverage * valuesInOldAverage + nextValue) /
+ // newNumberOfValues
Translation2d translation = new Translation2d();
double angle = 0;
- for(int i = 0; i < estimatedPoses.size(); i ++){
- translation = translation.plus(estimatedPoses.get(i).estimatedPose.toPose2d().getTranslation());
- angle = MathUtils.modulusInterpolate(angle, estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(), 1.0/(i+1), -Math.PI, Math.PI);
+ for (int i = 0; i < estimatedPoses.size(); i++) {
+ translation =
+ translation.plus(estimatedPoses.get(i).estimatedPose.toPose2d().getTranslation());
+ angle =
+ MathUtils.modulusInterpolate(
+ angle,
+ estimatedPoses.get(i).estimatedPose.toPose2d().getRotation().getRadians(),
+ 1.0 / (i + 1),
+ -Math.PI,
+ Math.PI);
}
return new Pose2d(translation.div(estimatedPoses.size()), new Rotation2d(angle));
}
- public AprilTagFieldLayout getAprilTagFieldLayout(){
+ public AprilTagFieldLayout getAprilTagFieldLayout() {
return FieldConstants.field;
}
/**
* Gets the pose of an april tag
+ *
* @param id AprilTag id (1-8)
* @return Pose3d of the AprilTag
*/
- public Pose3d getTagPose(int id){
- if(id < 1 || id > getAprilTagFieldLayout().getTags().size()){
- System.out.println("Tried to find the pose of april tag "+id);
+ public Pose3d getTagPose(int id) {
+ if (id < 1 || id > getAprilTagFieldLayout().getTags().size()) {
+ System.out.println("Tried to find the pose of april tag " + id);
return null;
}
- return getAprilTagFieldLayout().getTags().get(id-1).pose;
+ return getAprilTagFieldLayout().getTags().get(id - 1).pose;
}
/**
* Returns where it thinks the robot is
+ *
* @param referencePose The pose to use as a reference, usually the previous robot pose
* @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
* @return An array list of estimated poses, one for each camera that can see an april tag
*/
public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose) {
- return getEstimatedPoses(referencePose, ignoree->referencePose.getRotation().getRadians());
+ return getEstimatedPoses(referencePose, ignoree -> referencePose.getRotation().getRadians());
}
/**
* Returns where it thinks the robot is
+ *
* @param referencePose The pose to use as a reference, usually the previous robot pose
* @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
* @return An array list of estimated poses, one for each camera that can see an april tag
*/
- public ArrayList<EstimatedRobotPose> getEstimatedPoses(Pose2d referencePose, DoubleUnaryOperator yawFunction) {
+ public ArrayList<EstimatedRobotPose> getEstimatedPoses(
+ Pose2d referencePose, DoubleUnaryOperator yawFunction) {
ArrayList<EstimatedRobotPose> estimatedPoses = new ArrayList<>();
for (int i = 0; i < cameras.size(); i++) {
- if(VisionConstants.USE_MANUAL_CALCULATIONS){
- for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)){
- if(pose != null){
+ if (VisionConstants.USE_MANUAL_CALCULATIONS) {
+ for (EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(yawFunction)) {
+ if (pose != null) {
estimatedPoses.add(pose);
}
}
- }else{
- for(EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)){
+ } else {
+ for (EstimatedRobotPose pose : cameras.get(i).getEstimatedPose(referencePose)) {
// If the camera can see an april tag that exists, add it to the array list
- // April tags that don't exist might return a result that is present but doesn't have a pose
+ // April tags that don't exist might return a result that is present but doesn't have a
+ // pose
if (pose.estimatedPose != null) {
estimatedPoses.add(pose);
-
}
}
}
}
- if(estimatedPoses.size() > 1){
+ if (estimatedPoses.size() > 1) {
Translation2d average = new Translation2d();
- for(EstimatedRobotPose pose : estimatedPoses){
+ for (EstimatedRobotPose pose : estimatedPoses) {
average = average.plus(pose.estimatedPose.getTranslation().toTranslation2d());
}
average = average.div(estimatedPoses.size());
- for(int i = estimatedPoses.size()-1; i>=0; i--){
- if(estimatedPoses.get(i).estimatedPose.getTranslation().toTranslation2d().getDistance(average) > VisionConstants.MAX_POSE_DIFFERENCE/2){
+ for (int i = estimatedPoses.size() - 1; i >= 0; i--) {
+ if (estimatedPoses
+ .get(i)
+ .estimatedPose
+ .getTranslation()
+ .toTranslation2d()
+ .getDistance(average)
+ > VisionConstants.MAX_POSE_DIFFERENCE / 2) {
estimatedPoses.remove(i);
}
}
}
- return estimatedPoses;
+ return estimatedPoses;
}
/**
* Updates the robot's odometry with vision
+ *
* @param poseEstimator The pose estimator to update
* @param yawFunction A function that returns the yaw as a double given the timestamp
* @param slipped True if the wheels have slipped, false otherwise
* @return The list of estimated robot poses from vision
*/
- public ArrayList<EstimatedRobotPose> updateOdometry(SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped){
+ public ArrayList<EstimatedRobotPose> updateOdometry(
+ SwerveDrivePoseEstimator poseEstimator, DoubleUnaryOperator yawFunction, boolean slipped) {
// Simulate vision
// 2 ifs to avoid warning
- if(VisionConstants.ENABLED_SIM){
- if(RobotBase.isSimulation()){
+ if (VisionConstants.ENABLED_SIM) {
+ if (RobotBase.isSimulation()) {
visionSim.update(poseEstimator.getEstimatedPosition());
}
}
sawTag = false;
// An array list of poses returned by different cameras
- ArrayList<EstimatedRobotPose> estimatedPoses = getEstimatedPoses(poseEstimator.getEstimatedPosition(), yawFunction);
+ ArrayList<EstimatedRobotPose> estimatedPoses =
+ getEstimatedPoses(poseEstimator.getEstimatedPosition(), yawFunction);
for (EstimatedRobotPose estimatedPose : estimatedPoses) {
// Continue if this pose doesn't exist
- if(estimatedPose.timestampSeconds < 0 || !onField(estimatedPose.estimatedPose.toPose2d()) || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1){
+ if (estimatedPose.timestampSeconds < 0
+ || !onField(estimatedPose.estimatedPose.toPose2d())
+ || Timer.getFPGATimestamp() < estimatedPose.timestampSeconds
+ || Timer.getFPGATimestamp() > estimatedPose.timestampSeconds + 1) {
continue;
}
poseEstimator.addVisionMeasurement(
- estimatedPose.estimatedPose.toPose2d(),
- estimatedPose.timestampSeconds,
- slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS
- );
+ estimatedPose.estimatedPose.toPose2d(),
+ estimatedPose.timestampSeconds,
+ slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS);
sawTag = true;
}
return estimatedPoses;
}
- /**
- * Updates each camera's inputs for logging
- */
- public void updateInputs(){
- for(VisionCamera c : cameras){
+ /** Updates each camera's inputs for logging */
+ public void updateInputs() {
+ for (VisionCamera c : cameras) {
c.updateInputs();
}
}
/**
* If vision saw any April tags last frame
+ *
* @return If vision saw an April tag last frame
*/
- public boolean canSeeTag(){
+ public boolean canSeeTag() {
return sawTag;
}
/**
* Enable or disable a single camera
+ *
* @param index The camera index
* @param enabled If it should be enabled or disabled
*/
- public void enableCamera(int index, boolean enabled){
- try{
+ public void enableCamera(int index, boolean enabled) {
+ try {
cameras.get(index).enable(enabled);
- }catch(IndexOutOfBoundsException e){
- DriverStation.reportWarning("Camera index "+index+" is out of bounds", false);
+ } catch (IndexOutOfBoundsException e) {
+ DriverStation.reportWarning("Camera index " + index + " is out of bounds", false);
}
}
+
/**
* Sets the cameras to only use April tag in the specified array
+ *
* @param ids The ids of the tags to use, null or empty array to use all
*/
- public void onlyUse(int[] ids){
+ public void onlyUse(int[] ids) {
onlyUse = ids;
}
/**
* Checks if one or more cameras are disconnected
+ *
* @return true if at least one camera is disconnected, false otherwise
*/
- public boolean oneCameraDisconnected(){
- for(VisionCamera camera : cameras){
- if(!camera.inputs.connected){
+ public boolean oneCameraDisconnected() {
+ for (VisionCamera camera : cameras) {
+ if (!camera.inputs.connected) {
return true;
}
}
/**
* Checks if a pose is on the field
+ *
* @param pose The pose to check
* @return If the pose is on the field
*/
- public static boolean onField(Pose2d pose){
- return pose!=null && pose.getX()>0 && pose.getX()<FieldConstants.field.getFieldLength() && pose.getY()>0 && pose.getY()<FieldConstants.field.getFieldWidth();
+ public static boolean onField(Pose2d pose) {
+ return pose != null
+ && pose.getX() > 0
+ && pose.getX() < FieldConstants.field.getFieldLength()
+ && pose.getY() > 0
+ && pose.getY() < FieldConstants.field.getFieldWidth();
}
/**
* Checks if a pose is on or near the field
+ *
* @param pose The pose to check
* @return If the pose is within an area with twice the length and width of the field
*/
- public static boolean nearField(Pose2d pose){
- return pose!=null && pose.getX()>-FieldConstants.field.getFieldLength()/2 && pose.getX()<FieldConstants.field.getFieldLength()*1.5 && pose.getY()>-FieldConstants.field.getFieldWidth()/2 && pose.getY()<FieldConstants.field.getFieldWidth()*1.5;
+ public static boolean nearField(Pose2d pose) {
+ return pose != null
+ && pose.getX() > -FieldConstants.field.getFieldLength() / 2
+ && pose.getX() < FieldConstants.field.getFieldLength() * 1.5
+ && pose.getY() > -FieldConstants.field.getFieldWidth() / 2
+ && pose.getY() < FieldConstants.field.getFieldWidth() * 1.5;
}
-
+
private class VisionCamera implements VisionIO {
private PhotonCamera camera;
private PhotonPoseEstimator photonPoseEstimator;
private double lastTimestamp = 0;
private boolean enabled = true;
private final VisionIOInputs inputs = new VisionIOInputs();
-
+
/**
* Stores information about a camera
+ *
* @param cameraName The name of the camera on PhotonVision
* @param robotToCam The transformation from the robot to the camera
*/
public VisionCamera(String cameraName, Transform3d robotToCam) {
camera = new PhotonCamera(cameraName);
- photonPoseEstimator = new PhotonPoseEstimator(
- FieldConstants.field,
- robotToCam
- );
+ photonPoseEstimator = new PhotonPoseEstimator(FieldConstants.field, robotToCam);
lastPose = null;
}
-
+
/**
* Gets the estimated poses from the camera
+ *
* @param referencePose Pose to use for reference, usually the previous estimated robot pose
* @return estimated robot poses
*/
ArrayList<EstimatedRobotPose> list = new ArrayList<>();
- if(!enabled){
+ if (!enabled) {
return list;
}
- for(PhotonPipelineResult cameraResult : inputs.results){
- if(!cameraResult.hasTargets() || cameraResult.getTimestampSeconds()<0){
- continue;
+ for (PhotonPipelineResult cameraResult : inputs.results) {
+ if (!cameraResult.hasTargets() || cameraResult.getTimestampSeconds() < 0) {
+ continue;
}
-
- // if there is a target detected and the timestamp exists,
+
+ // if there is a target detected and the timestamp exists,
// check the ambiguity isn't too high
List<PhotonTrackedTarget> targetsUsed = cameraResult.targets;
- for (int i = targetsUsed.size()-1; i >= 0; i--) {
+ for (int i = targetsUsed.size() - 1; i >= 0; i--) {
// Remove it from the list if it should not be used or if it has too high of an ambiguity
- if(!useTag(targetsUsed.get(i).getFiducialId()) || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE){
+ if (!useTag(targetsUsed.get(i).getFiducialId())
+ || targetsUsed.get(i).getPoseAmbiguity() > VisionConstants.HIGHEST_AMBIGUITY
+ || targetsUsed.get(i).bestCameraToTarget.getTranslation().getNorm()
+ > VisionConstants.MAX_DISTANCE) {
targetsUsed.remove(i);
}
}
- // If there are no targets, the timestamp doesn't exist, or there there is only 1 tag and the constant is set to only use 2 tags, continue
- if(targetsUsed.size() == 0 || cameraResult.getTimestampSeconds()<0 || targetsUsed.size()==1 && VisionConstants.ONLY_USE_2_TAGS){
+ // If there are no targets, the timestamp doesn't exist, or there there is only 1 tag and
+ // the constant is set to only use 2 tags, continue
+ if (targetsUsed.size() == 0
+ || cameraResult.getTimestampSeconds() < 0
+ || targetsUsed.size() == 1 && VisionConstants.ONLY_USE_2_TAGS) {
continue;
}
// Set strategy to single tag if there is only 1 good tag and update
- PhotonPoseEstimator.PoseStrategy poseStrategy = targetsUsed.size() > 1 ? VisionConstants.POSE_STRATEGY : VisionConstants.MULTITAG_FALLBACK_STRATEGY;
- Optional<EstimatedRobotPose> pose;
- switch (poseStrategy) {
- case AVERAGE_BEST_TARGETS:
- pose = photonPoseEstimator.estimateAverageBestTargetsPose(cameraResult);
- break;
- case CLOSEST_TO_CAMERA_HEIGHT:
- pose = photonPoseEstimator.estimateClosestToCameraHeightPose(cameraResult);
- break;
- case CLOSEST_TO_REFERENCE_POSE:
- pose = photonPoseEstimator.estimateClosestToReferencePose(cameraResult, new Pose3d(referencePose));
- break;
- case LOWEST_AMBIGUITY:
- pose = photonPoseEstimator.estimateLowestAmbiguityPose(cameraResult);
- break;
- case MULTI_TAG_PNP_ON_COPROCESSOR:
- pose = photonPoseEstimator.estimateCoprocMultiTagPose(cameraResult);
- break;
- case PNP_DISTANCE_TRIG_SOLVE:
- pose = photonPoseEstimator.estimatePnpDistanceTrigSolvePose(cameraResult);
- break;
- case CLOSEST_TO_LAST_POSE:
- case CONSTRAINED_SOLVEPNP:
- case MULTI_TAG_PNP_ON_RIO:
- default:
- throw new RuntimeException("Pose estimation method " + poseStrategy.toString() + " is not supported.");
- }
-
- if(pose.isPresent() && pose.get()!=null && onField(pose.get().estimatedPose.toPose2d())){
+ PhotonPoseEstimator.PoseStrategy poseStrategy =
+ targetsUsed.size() > 1
+ ? VisionConstants.POSE_STRATEGY
+ : VisionConstants.MULTITAG_FALLBACK_STRATEGY;
+ Optional<EstimatedRobotPose> pose;
+ switch (poseStrategy) {
+ case AVERAGE_BEST_TARGETS:
+ pose = photonPoseEstimator.estimateAverageBestTargetsPose(cameraResult);
+ break;
+ case CLOSEST_TO_CAMERA_HEIGHT:
+ pose = photonPoseEstimator.estimateClosestToCameraHeightPose(cameraResult);
+ break;
+ case CLOSEST_TO_REFERENCE_POSE:
+ pose =
+ photonPoseEstimator.estimateClosestToReferencePose(
+ cameraResult, new Pose3d(referencePose));
+ break;
+ case LOWEST_AMBIGUITY:
+ pose = photonPoseEstimator.estimateLowestAmbiguityPose(cameraResult);
+ break;
+ case MULTI_TAG_PNP_ON_COPROCESSOR:
+ pose = photonPoseEstimator.estimateCoprocMultiTagPose(cameraResult);
+ break;
+ case PNP_DISTANCE_TRIG_SOLVE:
+ pose = photonPoseEstimator.estimatePnpDistanceTrigSolvePose(cameraResult);
+ break;
+ case CLOSEST_TO_LAST_POSE:
+ case CONSTRAINED_SOLVEPNP:
+ case MULTI_TAG_PNP_ON_RIO:
+ default:
+ throw new RuntimeException(
+ "Pose estimation method " + poseStrategy.toString() + " is not supported.");
+ }
+
+ if (pose.isPresent()
+ && pose.get() != null
+ && onField(pose.get().estimatedPose.toPose2d())) {
double timestamp = cameraResult.getTimestampSeconds();
// If the pose moved too much, don't use it
- if(lastPose==null || lastPose.getTranslation().getDistance(pose.get().estimatedPose.toPose2d().getTranslation()) > DriveConstants.MAX_SPEED*1.25*(timestamp-lastTimestamp) || timestamp < lastTimestamp){
+ if (lastPose == null
+ || lastPose
+ .getTranslation()
+ .getDistance(pose.get().estimatedPose.toPose2d().getTranslation())
+ > DriveConstants.MAX_SPEED * 1.25 * (timestamp - lastTimestamp)
+ || timestamp < lastTimestamp) {
lastPose = pose.get().estimatedPose.toPose2d();
lastTimestamp = timestamp;
continue;
return list;
}
- /**
- * Updates the VisionIOInputs object with the results from PhotonVision for logging
- */
- @Override
+ /** Updates the VisionIOInputs object with the results from PhotonVision for logging */
+ @Override
public void updateInputs() {
inputs.connected = camera.isConnected();
inputs.results = camera.getAllUnreadResults();
- Logger.processInputs("Vision/"+camera.getName(), inputs);
+ Logger.processInputs("Vision/" + camera.getName(), inputs);
}
-
+
/**
* Gets the pose using manual calculations
+ *
* @param yawFunction A unary operator that takes a timestamp and returns the yaw at that time
* @return A list of estimated poses as EstimatedRobotPoses
*/
- public ArrayList<EstimatedRobotPose> getEstimatedPose(DoubleUnaryOperator yawFunction){
+ public ArrayList<EstimatedRobotPose> getEstimatedPose(DoubleUnaryOperator yawFunction) {
ArrayList<EstimatedRobotPose> list = new ArrayList<>();
// Do nothing if this camera is disabled
- if(!enabled){
+ if (!enabled) {
return list;
}
// The latest camera results
- for(PhotonPipelineResult result : inputs.results){
+ for (PhotonPipelineResult result : inputs.results) {
// TODO: This could be improved by averaging all targets instead of only using 1
// Continue if the target doesn't exist or it should be ignored
// Gets the best target to use for the calculations
PhotonTrackedTarget target = result.getBestTarget();
// I don't know why this would happen, but keep it in just in case
- if(target==null){
+ if (target == null) {
continue;
}
// Continue if the id is too high or too low
int id = target.getFiducialId();
- if(!useTag(id) || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY){
+ if (!useTag(id)
+ || target.bestCameraToTarget.getTranslation().getNorm() > VisionConstants.MAX_DISTANCE
+ || target.poseAmbiguity > VisionConstants.HIGHEST_AMBIGUITY) {
continue;
}
// Stores target pose and robot to camera transformation for easy access later
double yaw = yawFunction.applyAsDouble(timestamp);
// Get the tag position relative to the robot, assuming the robot is on the ground
- Translation3d translation = target.getBestCameraToTarget().getTranslation()
- .rotateBy(robotToCamera.getRotation());
- translation = translation//.times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ())
- .plus(robotToCamera.getTranslation())
- .rotateBy(new Rotation3d(0, 0, yaw))
-
- // Invert it to get the robot position relative to the April tag
- // Multiply by a constant. I don't know why this works, but it was consistently 10% off in 2023 Fall Semester
- .times(-VisionConstants.DISTANCE_SCALE)
- // Get the field relative robot pose
- .plus(targetPose.getTranslation());
- try{
+ Translation3d translation =
+ target.getBestCameraToTarget().getTranslation().rotateBy(robotToCamera.getRotation());
+ translation =
+ translation // .times((targetPose.getZ()-robotToCamera.getZ())/translation.getZ())
+ .plus(robotToCamera.getTranslation())
+ .rotateBy(new Rotation3d(0, 0, yaw))
+
+ // Invert it to get the robot position relative to the April tag
+ // Multiply by a constant. I don't know why this works, but it was consistently 10%
+ // off in 2023 Fall Semester
+ .times(-VisionConstants.DISTANCE_SCALE)
+ // Get the field relative robot pose
+ .plus(targetPose.getTranslation());
+ try {
// Adds an EstimatedRobotPose
- list.add(new EstimatedRobotPose(
- new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)),
- timestamp,
- List.of(target),
- VisionConstants.POSE_STRATEGY
- ));
- }catch(Exception e){
+ list.add(
+ new EstimatedRobotPose(
+ new Pose3d(translation.getX(), translation.getY(), 0, new Rotation3d(0, 0, yaw)),
+ timestamp,
+ List.of(target),
+ VisionConstants.POSE_STRATEGY));
+ } catch (Exception e) {
DriverStation.reportError("Error creating EstimatedRobotPose", true);
}
}
return list;
}
- public boolean useTag(int id){
+ public boolean useTag(int id) {
// Never use tags that don't exist
- if(id <= 0 || id > FieldConstants.field.getTags().size()){
+ if (id <= 0 || id > FieldConstants.field.getTags().size()) {
return false;
}
// Return false if it is in the list of tags to ignore
- for(int id2 : VisionConstants.TAGS_TO_IGNORE){
- if(id == id2){
+ for (int id2 : VisionConstants.TAGS_TO_IGNORE) {
+ if (id == id2) {
return false;
}
}
// If it's in the array to only use and not in the array to ignore, return true
- for(int j = 0; onlyUse != null && j < onlyUse.length; j++){
- if(id == onlyUse[j]){
+ for (int j = 0; onlyUse != null && j < onlyUse.length; j++) {
+ if (id == onlyUse[j]) {
return true;
}
}
/**
* Enables or disables this camera
+ *
* @param enable If it should be enabled or disabled
*/
- public void enable(boolean enable){
+ public void enable(boolean enable) {
enabled = enable;
}
}
public static class VisionIOInputs implements LoggableInputs {
public boolean connected = false;
public List<PhotonPipelineResult> results = new ArrayList<>();
-
- // PhotonVision should never return more than 5 results, except possibly for very long loop overruns
+
+ // PhotonVision should never return more than 5 results, except possibly for very long loop
+ // overruns
private static final int maxLength = 5;
private boolean intitalized = false;
@Override
public void toLog(LogTable table) {
- // LogTable does not easily allow removal of logs, especially ProtobufSerializables, so extra values will need to be ignored
- // This is not very efficient, since unused values are still taking up memory, but there is no easy way to remove them
- if (!intitalized){
- for(int i = 0; i < maxLength; i++){
- table.put("Results"+i, new PhotonPipelineResult());
+ // LogTable does not easily allow removal of logs, especially ProtobufSerializables, so extra
+ // values will need to be ignored
+ // This is not very efficient, since unused values are still taking up memory, but there is no
+ // easy way to remove them
+ if (!intitalized) {
+ for (int i = 0; i < maxLength; i++) {
+ table.put("Results" + i, new PhotonPipelineResult());
intitalized = true;
}
}
table.put("Connected", connected);
double length = Math.min(results.size(), maxLength);
table.put("Length", length);
- for(int i = 0; i < length; i++){
- table.put("Results"+i, results.get(i));
+ for (int i = 0; i < length; i++) {
+ table.put("Results" + i, results.get(i));
}
}
results = new ArrayList<>(length);
// Java gets confused when null is used for a generic type argument
PhotonPipelineResult nullResult = null;
- for(int i = 0; i < length; i++){
- PhotonPipelineResult result = table.get("Results"+i, nullResult);
- if(result != null){
+ for (int i = 0; i < length; i++) {
+ PhotonPipelineResult result = table.get("Results" + i, nullResult);
+ if (result != null) {
results.add(result);
}
}
/** Represents a robot pose sample used for pose estimation. */
public static record PoseObservation(
- double timestamp,
- Pose3d pose,
- double ambiguity,
- int tagCount,
- double averageTagDistance) {}
+ double timestamp, Pose3d pose, double ambiguity, int tagCount, double averageTagDistance) {}
public default void updateInputs() {}
-}
\ No newline at end of file
+}
/* Contains values and required settings for common COTS swerve modules. */
public class COTSFalconSwerveConstants {
- public final double wheelDiameter;
- public final double wheelCircumference;
- public final double angleGearRatio;
- public final double driveGearRatio;
- public final double angleKP;
- public final double angleKI;
- public final double angleKD;
- public final double angleKF;
- public final boolean driveMotorInvert;
- public final boolean angleMotorInvert;
- public final boolean canCoderInvert;
-
- public COTSFalconSwerveConstants(double wheelDiameter, double angleGearRatio, double driveGearRatio, double angleKP, double angleKI, double angleKD, double angleKF, boolean driveMotorInvert, boolean angleMotorInvert, boolean canCoderInvert) {
- this.wheelDiameter = wheelDiameter;
- this.wheelCircumference = wheelDiameter * Math.PI;
- this.angleGearRatio = angleGearRatio;
- this.driveGearRatio = driveGearRatio;
- this.angleKP = angleKP;
- this.angleKI = angleKI;
- this.angleKD = angleKD;
- this.angleKF = angleKF;
- this.driveMotorInvert = driveMotorInvert;
- this.angleMotorInvert = angleMotorInvert;
- this.canCoderInvert = canCoderInvert;
- }
-
- /**
- * Swerve Drive Specialties - MK3 Module
- */
- public static COTSFalconSwerveConstants SDSMK3(double driveGearRatio) {
- double wheelDiameter = Units.inchesToMeters(4.0);
-
- /** 12.8 : 1 */
- double angleGearRatio = (12.8);
-
- double angleKP = 0.2;
- double angleKI = 0.0;
- double angleKD = 0.0;
- double angleKF = 0.0;
-
- boolean driveMotorInvert = false;
- boolean angleMotorInvert = false;
- boolean canCoderInvert = false;
- return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
- }
-
- /**
- * Swerve Drive Specialties - MK4 Module
- */
- public static COTSFalconSwerveConstants SDSMK4(double driveGearRatio) {
- double wheelDiameter = Units.inchesToMeters(4.0);
-
- /** 12.8 : 1 */
- double angleGearRatio = (12.8);
-
- double angleKP = 0.2;
- double angleKI = 0.0;
- double angleKD = 0.0;
- double angleKF = 0.0;
-
- boolean driveMotorInvert = false;
- boolean angleMotorInvert = false;
- boolean canCoderInvert = false;
- return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
- }
-
- /**
- * Swerve Drive Specialties - MK4i Module
- */
- public static COTSFalconSwerveConstants SDSMK4i(double driveGearRatio) {
- double wheelDiameter = Units.inchesToMeters(4.0);
-
- /** (150 / 7) : 1 */
- double angleGearRatio = ((150.0 / 7.0));
-
- double angleKP = 0.3;
- double angleKI = 0.0;
- double angleKD = 0.0;
- double angleKF = 0.0;
-
- boolean driveMotorInvert = false;
- boolean angleMotorInvert = true;
- boolean canCoderInvert = false;
- return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
- }
-
- /**
- * Swerve Drive Specialties - MK5n Module
- */
- public static COTSFalconSwerveConstants SDSMK5n(double driveGearRatio) {
- double wheelDiameter = Units.inchesToMeters(4.0);
-
- /** (287 / 11) : 1 */
- double angleGearRatio = ((287.0 / 11.0));
-
- double angleKP = 0.3;
- double angleKI = 0.0;
- double angleKD = 0.0;
- double angleKF = 0.0;
-
- boolean driveMotorInvert = false;
- boolean angleMotorInvert = true;
- boolean canCoderInvert = false;
- return new COTSFalconSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, angleKF, driveMotorInvert, angleMotorInvert, canCoderInvert);
- }
-
- /* Drive Gear Ratios for all supported modules */
- public static class DriveGearRatios {
- /* SDS MK3 */
- /**
- * SDS MK3 - 8.16 : 1
- */
- public static final double SDSMK3_Standard = (8.16);
- /**
- * SDS MK3 - 6.86 : 1
- */
- public static final double SDSMK3_Fast = (6.86);
-
- /* SDS MK4 */
- /**
- * SDS MK4 - 8.14 : 1
- */
- public static final double SDSMK4_L1 = (8.14);
- /**
- * SDS MK4 - 6.75 : 1
- */
- public static final double SDSMK4_L2 = (6.75);
- /**
- * SDS MK4 - 6.12 : 1
- */
- public static final double SDSMK4_L3 = (6.12);
- /**
- * SDS MK4 - 5.14 : 1
- */
- public static final double SDSMK4_L4 = (5.14);
-
- /* SDS MK4i */
- /**
- * SDS MK4i - 8.14 : 1
- */
- public static final double SDSMK4i_L1 = (8.14);
- /**
- * SDS MK4i - 6.75 : 1
- */
- public static final double SDSMK4i_L2 = (6.75);
- /**
- * SDS MK4i - 6.12 : 1
- */
- public static final double SDSMK4i_L3 = (6.12);
-
- /* SDS MK5n */
- /**
- * SDS MK5n - 7.13 : 1
- */
- public static final double SDSMK5n_L1_PLUS = (8.13);
- /**
- * SDS MK4i - 5.9 : 1
- */
- public static final double SDSMK5n_L2_PLUS = (5.9);
- /**
- * SDS MK4i - 5.36 : 1
- */
- public static final double SDSMK5n_L3_PLUS = (5.35);
- }
+ public final double wheelDiameter;
+ public final double wheelCircumference;
+ public final double angleGearRatio;
+ public final double driveGearRatio;
+ public final double angleKP;
+ public final double angleKI;
+ public final double angleKD;
+ public final double angleKF;
+ public final boolean driveMotorInvert;
+ public final boolean angleMotorInvert;
+ public final boolean canCoderInvert;
+
+ public COTSFalconSwerveConstants(
+ double wheelDiameter,
+ double angleGearRatio,
+ double driveGearRatio,
+ double angleKP,
+ double angleKI,
+ double angleKD,
+ double angleKF,
+ boolean driveMotorInvert,
+ boolean angleMotorInvert,
+ boolean canCoderInvert) {
+ this.wheelDiameter = wheelDiameter;
+ this.wheelCircumference = wheelDiameter * Math.PI;
+ this.angleGearRatio = angleGearRatio;
+ this.driveGearRatio = driveGearRatio;
+ this.angleKP = angleKP;
+ this.angleKI = angleKI;
+ this.angleKD = angleKD;
+ this.angleKF = angleKF;
+ this.driveMotorInvert = driveMotorInvert;
+ this.angleMotorInvert = angleMotorInvert;
+ this.canCoderInvert = canCoderInvert;
+ }
+
+ /** Swerve Drive Specialties - MK3 Module */
+ public static COTSFalconSwerveConstants SDSMK3(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** 12.8 : 1 */
+ double angleGearRatio = (12.8);
+
+ double angleKP = 0.2;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = false;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(
+ wheelDiameter,
+ angleGearRatio,
+ driveGearRatio,
+ angleKP,
+ angleKI,
+ angleKD,
+ angleKF,
+ driveMotorInvert,
+ angleMotorInvert,
+ canCoderInvert);
+ }
+
+ /** Swerve Drive Specialties - MK4 Module */
+ public static COTSFalconSwerveConstants SDSMK4(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** 12.8 : 1 */
+ double angleGearRatio = (12.8);
+
+ double angleKP = 0.2;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = false;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(
+ wheelDiameter,
+ angleGearRatio,
+ driveGearRatio,
+ angleKP,
+ angleKI,
+ angleKD,
+ angleKF,
+ driveMotorInvert,
+ angleMotorInvert,
+ canCoderInvert);
+ }
+
+ /** Swerve Drive Specialties - MK4i Module */
+ public static COTSFalconSwerveConstants SDSMK4i(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** (150 / 7) : 1 */
+ double angleGearRatio = ((150.0 / 7.0));
+
+ double angleKP = 0.3;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = true;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(
+ wheelDiameter,
+ angleGearRatio,
+ driveGearRatio,
+ angleKP,
+ angleKI,
+ angleKD,
+ angleKF,
+ driveMotorInvert,
+ angleMotorInvert,
+ canCoderInvert);
+ }
+
+ /** Swerve Drive Specialties - MK5n Module */
+ public static COTSFalconSwerveConstants SDSMK5n(double driveGearRatio) {
+ double wheelDiameter = Units.inchesToMeters(4.0);
+
+ /** (287 / 11) : 1 */
+ double angleGearRatio = ((287.0 / 11.0));
+
+ double angleKP = 0.3;
+ double angleKI = 0.0;
+ double angleKD = 0.0;
+ double angleKF = 0.0;
+
+ boolean driveMotorInvert = false;
+ boolean angleMotorInvert = true;
+ boolean canCoderInvert = false;
+ return new COTSFalconSwerveConstants(
+ wheelDiameter,
+ angleGearRatio,
+ driveGearRatio,
+ angleKP,
+ angleKI,
+ angleKD,
+ angleKF,
+ driveMotorInvert,
+ angleMotorInvert,
+ canCoderInvert);
+ }
+
+ /* Drive Gear Ratios for all supported modules */
+ public static class DriveGearRatios {
+ /* SDS MK3 */
+ /** SDS MK3 - 8.16 : 1 */
+ public static final double SDSMK3_Standard = (8.16);
+
+ /** SDS MK3 - 6.86 : 1 */
+ public static final double SDSMK3_Fast = (6.86);
+
+ /* SDS MK4 */
+ /** SDS MK4 - 8.14 : 1 */
+ public static final double SDSMK4_L1 = (8.14);
+
+ /** SDS MK4 - 6.75 : 1 */
+ public static final double SDSMK4_L2 = (6.75);
+
+ /** SDS MK4 - 6.12 : 1 */
+ public static final double SDSMK4_L3 = (6.12);
+
+ /** SDS MK4 - 5.14 : 1 */
+ public static final double SDSMK4_L4 = (5.14);
+
+ /* SDS MK4i */
+ /** SDS MK4i - 8.14 : 1 */
+ public static final double SDSMK4i_L1 = (8.14);
+
+ /** SDS MK4i - 6.75 : 1 */
+ public static final double SDSMK4i_L2 = (6.75);
+
+ /** SDS MK4i - 6.12 : 1 */
+ public static final double SDSMK4i_L3 = (6.12);
+
+ /* SDS MK5n */
+ /** SDS MK5n - 7.13 : 1 */
+ public static final double SDSMK5n_L1_PLUS = (8.13);
+
+ /** SDS MK4i - 5.9 : 1 */
+ public static final double SDSMK5n_L2_PLUS = (5.9);
+
+ /** SDS MK4i - 5.36 : 1 */
+ public static final double SDSMK5n_L3_PLUS = (5.35);
+ }
}
-
-
\ No newline at end of file
public class CTREModuleState {
- /**
- * Minimize the change in heading the desired swerve module state would require by potentially
- * reversing the direction the wheel spins. Customized from WPILib's version to include placing
- * in appropriate scope for CTRE onboard control.
- *
- * @param desiredState The desired state.
- * @param currentAngle The current module angle.
- */
- public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
- double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
- double targetSpeed = desiredState.speedMetersPerSecond;
- double delta = targetAngle - currentAngle.getDegrees();
- if (Math.abs(delta) > 90) {
- targetSpeed = -targetSpeed;
- if (delta > 90) {
- targetAngle -= 180;
- } else {
- targetAngle += 180;
- }
- }
- return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+ /**
+ * Minimize the change in heading the desired swerve module state would require by potentially
+ * reversing the direction the wheel spins. Customized from WPILib's version to include placing in
+ * appropriate scope for CTRE onboard control.
+ *
+ * @param desiredState The desired state.
+ * @param currentAngle The current module angle.
+ */
+ public static SwerveModuleState optimize(
+ SwerveModuleState desiredState, Rotation2d currentAngle) {
+ double targetAngle =
+ placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
+ double targetSpeed = desiredState.speedMetersPerSecond;
+ double delta = targetAngle - currentAngle.getDegrees();
+ if (Math.abs(delta) > 90) {
+ targetSpeed = -targetSpeed;
+ if (delta > 90) {
+ targetAngle -= 180;
+ } else {
+ targetAngle += 180;
+ }
}
+ return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+ }
- /**
- * @param scopeReference Current Angle
- * @param newAngle Target Angle
- * @return Closest angle within scope
- */
- private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
- double lowerBound;
- double upperBound;
- double lowerOffset = scopeReference % 360;
- if (lowerOffset >= 0) {
- lowerBound = scopeReference - lowerOffset;
- upperBound = scopeReference + (360 - lowerOffset);
- } else {
- upperBound = scopeReference - lowerOffset;
- lowerBound = scopeReference - (360 + lowerOffset);
- }
- while (newAngle < lowerBound) {
- newAngle += 360;
- }
- while (newAngle > upperBound) {
- newAngle -= 360;
- }
- if (newAngle - scopeReference > 180) {
- newAngle -= 360;
- } else if (newAngle - scopeReference < -180) {
- newAngle += 360;
- }
- return newAngle;
+ /**
+ * @param scopeReference Current Angle
+ * @param newAngle Target Angle
+ * @return Closest angle within scope
+ */
+ private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
+ double lowerBound;
+ double upperBound;
+ double lowerOffset = scopeReference % 360;
+ if (lowerOffset >= 0) {
+ lowerBound = scopeReference - lowerOffset;
+ upperBound = scopeReference + (360 - lowerOffset);
+ } else {
+ upperBound = scopeReference - lowerOffset;
+ lowerBound = scopeReference - (360 + lowerOffset);
}
+ while (newAngle < lowerBound) {
+ newAngle += 360;
+ }
+ while (newAngle > upperBound) {
+ newAngle -= 360;
+ }
+ if (newAngle - scopeReference > 180) {
+ newAngle -= 360;
+ } else if (newAngle - scopeReference < -180) {
+ newAngle += 360;
+ }
+ return newAngle;
+ }
}
import Jama.Matrix;
import Jama.QRDecomposition;
-
// NOTE: This file is available at
// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html
* @author Kevin Wayne
*/
public class PolynomialRegression implements Comparable<PolynomialRegression> {
- private final String variableName; // name of the predictor variable
- private int degree; // degree of the polynomial regression
- private final Matrix beta; // the polynomial regression coefficients
- private final double sse; // sum of squares due to error
- private double sst; // total sum of squares
-
- /**
- * Performs a polynomial regression on the data points {@code (y[i], x[i])}. Uses n as the name
- * of the predictor variable.
- *
- * @param x the values of the predictor variable
- * @param y the corresponding values of the response variable
- * @param degree the degree of the polynomial to fit
- * @throws IllegalArgumentException if the lengths of the two arrays are not equal
- */
- public PolynomialRegression(double[] x, double[] y, int degree) {
- this(x, y, degree, "n");
- }
-
- /**
- * Performs a polynomial regression on the data points {@code (y[i], x[i])}.
- *
- * @param x the values of the predictor variable
- * @param y the corresponding values of the response variable
- * @param degree the degree of the polynomial to fit
- * @param variableName the name of the predictor variable
- * @throws IllegalArgumentException if the lengths of the two arrays are not equal
- */
- public PolynomialRegression(double[] x, double[] y, int degree, String variableName) {
- this.degree = degree;
- this.variableName = variableName;
-
- int n = x.length;
- QRDecomposition qr = null;
- Matrix matrixX = null;
-
- // in case Vandermonde matrix does not have full rank, reduce degree until it
- // does
- while (true) {
-
- // build Vandermonde matrix
- double[][] vandermonde = new double[n][this.degree + 1];
- for (int i = 0; i < n; i++) {
- for (int j = 0; j <= this.degree; j++) {
- vandermonde[i][j] = Math.pow(x[i], j);
- }
- }
- matrixX = new Matrix(vandermonde);
-
- // find least squares solution
- qr = new QRDecomposition(matrixX);
- if (qr.isFullRank()) break;
-
- // decrease degree and try again
- this.degree--;
+ private final String variableName; // name of the predictor variable
+ private int degree; // degree of the polynomial regression
+ private final Matrix beta; // the polynomial regression coefficients
+ private final double sse; // sum of squares due to error
+ private double sst; // total sum of squares
+
+ /**
+ * Performs a polynomial regression on the data points {@code (y[i], x[i])}. Uses n as the name of
+ * the predictor variable.
+ *
+ * @param x the values of the predictor variable
+ * @param y the corresponding values of the response variable
+ * @param degree the degree of the polynomial to fit
+ * @throws IllegalArgumentException if the lengths of the two arrays are not equal
+ */
+ public PolynomialRegression(double[] x, double[] y, int degree) {
+ this(x, y, degree, "n");
+ }
+
+ /**
+ * Performs a polynomial regression on the data points {@code (y[i], x[i])}.
+ *
+ * @param x the values of the predictor variable
+ * @param y the corresponding values of the response variable
+ * @param degree the degree of the polynomial to fit
+ * @param variableName the name of the predictor variable
+ * @throws IllegalArgumentException if the lengths of the two arrays are not equal
+ */
+ public PolynomialRegression(double[] x, double[] y, int degree, String variableName) {
+ this.degree = degree;
+ this.variableName = variableName;
+
+ int n = x.length;
+ QRDecomposition qr = null;
+ Matrix matrixX = null;
+
+ // in case Vandermonde matrix does not have full rank, reduce degree until it
+ // does
+ while (true) {
+
+ // build Vandermonde matrix
+ double[][] vandermonde = new double[n][this.degree + 1];
+ for (int i = 0; i < n; i++) {
+ for (int j = 0; j <= this.degree; j++) {
+ vandermonde[i][j] = Math.pow(x[i], j);
}
+ }
+ matrixX = new Matrix(vandermonde);
- // create matrix from vector
- Matrix matrixY = new Matrix(y, n);
-
- // linear regression coefficients
- beta = qr.solve(matrixY);
-
- // mean of y[] values
- double sum = 0.0;
- for (int i = 0; i < n; i++) sum += y[i];
- double mean = sum / n;
-
- // total variation to be accounted for
- for (int i = 0; i < n; i++) {
- double dev = y[i] - mean;
- sst += dev * dev;
- }
+ // find least squares solution
+ qr = new QRDecomposition(matrixX);
+ if (qr.isFullRank()) break;
- // variation not accounted for
- Matrix residuals = matrixX.times(beta).minus(matrixY);
- sse = residuals.norm2() * residuals.norm2();
+ // decrease degree and try again
+ this.degree--;
}
- /**
- * Returns the {@code j}th regression coefficient.
- *
- * @param j the index
- * @return the {@code j}th regression coefficient
- */
- public double beta(int j) {
- // to make -0.0 print as 0.0
- if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0;
- return beta.get(j, 0);
- }
+ // create matrix from vector
+ Matrix matrixY = new Matrix(y, n);
- /**
- * Returns the degree of the polynomial to fit.
- *
- * @return the degree of the polynomial to fit
- */
- public int degree() {
- return degree;
- }
+ // linear regression coefficients
+ beta = qr.solve(matrixY);
- /**
- * Returns the coefficient of determination <em>R</em><sup>2</sup>.
- *
- * @return the coefficient of determination <em>R</em><sup>2</sup>, which is a real number between
- * 0 and 1
- */
- public double R2() {
- if (sst == 0.0) return 1.0; // constant function
- return 1.0 - sse / sst;
- }
+ // mean of y[] values
+ double sum = 0.0;
+ for (int i = 0; i < n; i++) sum += y[i];
+ double mean = sum / n;
- /**
- * Returns the expected response {@code y} given the value of the predictor variable {@code x}.
- *
- * @param x the value of the predictor variable
- * @return the expected response {@code y} given the value of the predictor variable {@code x}
- */
- public double predict(double x) {
- // horner's method
- double y = 0.0;
- for (int j = degree; j >= 0; j--) y = beta(j) + (x * y);
- return y;
+ // total variation to be accounted for
+ for (int i = 0; i < n; i++) {
+ double dev = y[i] - mean;
+ sst += dev * dev;
}
- /**
- * Returns a string representation of the polynomial regression model.
- *
- * @return a string representation of the polynomial regression model, including the best-fit
- * polynomial and the coefficient of determination <em>R</em><sup>2</sup>
- */
- public String toString() {
- StringBuilder s = new StringBuilder();
- int j = degree;
-
- // ignoring leading zero coefficients
- while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--;
-
- // create remaining terms
- while (j >= 0) {
- if (j == 0) s.append(String.format("%.4f ", beta(j)));
- else if (j == 1) s.append(String.format("%.4f %s + ", beta(j), variableName));
- else s.append(String.format("%.4f %s^%d + ", beta(j), variableName, j));
- j--;
- }
- s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")");
-
- // replace "+ -2n" with "- 2n"
- return s.toString().replace("+ -", "- ");
+ // variation not accounted for
+ Matrix residuals = matrixX.times(beta).minus(matrixY);
+ sse = residuals.norm2() * residuals.norm2();
+ }
+
+ /**
+ * Returns the {@code j}th regression coefficient.
+ *
+ * @param j the index
+ * @return the {@code j}th regression coefficient
+ */
+ public double beta(int j) {
+ // to make -0.0 print as 0.0
+ if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0;
+ return beta.get(j, 0);
+ }
+
+ /**
+ * Returns the degree of the polynomial to fit.
+ *
+ * @return the degree of the polynomial to fit
+ */
+ public int degree() {
+ return degree;
+ }
+
+ /**
+ * Returns the coefficient of determination <em>R</em><sup>2</sup>.
+ *
+ * @return the coefficient of determination <em>R</em><sup>2</sup>, which is a real number between
+ * 0 and 1
+ */
+ public double R2() {
+ if (sst == 0.0) return 1.0; // constant function
+ return 1.0 - sse / sst;
+ }
+
+ /**
+ * Returns the expected response {@code y} given the value of the predictor variable {@code x}.
+ *
+ * @param x the value of the predictor variable
+ * @return the expected response {@code y} given the value of the predictor variable {@code x}
+ */
+ public double predict(double x) {
+ // horner's method
+ double y = 0.0;
+ for (int j = degree; j >= 0; j--) y = beta(j) + (x * y);
+ return y;
+ }
+
+ /**
+ * Returns a string representation of the polynomial regression model.
+ *
+ * @return a string representation of the polynomial regression model, including the best-fit
+ * polynomial and the coefficient of determination <em>R</em><sup>2</sup>
+ */
+ public String toString() {
+ StringBuilder s = new StringBuilder();
+ int j = degree;
+
+ // ignoring leading zero coefficients
+ while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--;
+
+ // create remaining terms
+ while (j >= 0) {
+ if (j == 0) s.append(String.format("%.4f ", beta(j)));
+ else if (j == 1) s.append(String.format("%.4f %s + ", beta(j), variableName));
+ else s.append(String.format("%.4f %s^%d + ", beta(j), variableName, j));
+ j--;
}
-
- /**
- * Compare lexicographically.
- */
- public int compareTo(PolynomialRegression that) {
- double EPSILON = 1E-5;
- int maxDegree = Math.max(this.degree(), that.degree());
- for (int j = maxDegree; j >= 0; j--) {
- double term1 = 0.0;
- double term2 = 0.0;
- if (this.degree() >= j) term1 = this.beta(j);
- if (that.degree() >= j) term2 = that.beta(j);
- if (Math.abs(term1) < EPSILON) term1 = 0.0;
- if (Math.abs(term2) < EPSILON) term2 = 0.0;
- if (term1 < term2) return -1;
- else if (term1 > term2) return +1;
- }
- return 0;
+ s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")");
+
+ // replace "+ -2n" with "- 2n"
+ return s.toString().replace("+ -", "- ");
+ }
+
+ /** Compare lexicographically. */
+ public int compareTo(PolynomialRegression that) {
+ double EPSILON = 1E-5;
+ int maxDegree = Math.max(this.degree(), that.degree());
+ for (int j = maxDegree; j >= 0; j--) {
+ double term1 = 0.0;
+ double term2 = 0.0;
+ if (this.degree() >= j) term1 = this.beta(j);
+ if (that.degree() >= j) term2 = that.beta(j);
+ if (Math.abs(term1) < EPSILON) term1 = 0.0;
+ if (Math.abs(term2) < EPSILON) term2 = 0.0;
+ if (term1 < term2) return -1;
+ else if (term1 > term2) return +1;
}
+ return 0;
+ }
}
import java.util.function.BooleanSupplier;
public class Controller {
- protected final Joystick controller;
+ protected final Joystick controller;
- public Controller(int port) {
- this.controller = new Joystick(port);
- }
+ public Controller(int port) {
+ this.controller = new Joystick(port);
+ }
- public Trigger get(BooleanSupplier sup) {
- return new Trigger(sup);
- }
+ public Trigger get(BooleanSupplier sup) {
+ return new Trigger(sup);
+ }
}
import edu.wpi.first.wpilibj2.command.button.Trigger;
public class Ex3DProController extends Controller {
- public Ex3DProController(int port) {
- super(port);
- }
+ public Ex3DProController(int port) {
+ super(port);
+ }
- public enum Ex3DProButton {
- B1(1),
- B2(2),
- B3(3),
- B4(4),
- B6(6),
- B7(7),
- B8(8),
- B9(9),
- B10(10),
- B11(11),
- B12(12);
+ public enum Ex3DProButton {
+ B1(1),
+ B2(2),
+ B3(3),
+ B4(4),
+ B6(6),
+ B7(7),
+ B8(8),
+ B9(9),
+ B10(10),
+ B11(11),
+ B12(12);
- public final int id;
+ public final int id;
- Ex3DProButton(final int id) {
- this.id = id;
- }
+ Ex3DProButton(final int id) {
+ this.id = id;
}
+ }
- public enum Ex3DProAxis {
- X(0),
- Y(1),
- Z(2),
- SLIDER(3);
+ public enum Ex3DProAxis {
+ X(0),
+ Y(1),
+ Z(2),
+ SLIDER(3);
- public final int id;
+ public final int id;
- Ex3DProAxis(final int id) {
- this.id = id;
- }
+ Ex3DProAxis(final int id) {
+ this.id = id;
}
+ }
- public enum Ex3DProHatSwitch {
- UNPRESSED(-1),
- UP(0),
- UP_RIGHT(45),
- RIGHT(90),
- DOWN_RIGHT(135),
- DOWN(180),
- DOWN_LEFT(235),
- LEFT(270),
- UP_LEFT(315);
+ public enum Ex3DProHatSwitch {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
- public final int angle;
+ public final int angle;
- Ex3DProHatSwitch(final int angle) {
- this.angle = angle;
- }
+ Ex3DProHatSwitch(final int angle) {
+ this.angle = angle;
}
+ }
- public Trigger get(Ex3DProButton button) {
- return new Trigger(() -> controller.getRawButton(button.id));
- }
+ public Trigger get(Ex3DProButton button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
- public double get(Ex3DProAxis axis) {
- return controller.getRawAxis(axis.id);
- }
+ public double get(Ex3DProAxis axis) {
+ return controller.getRawAxis(axis.id);
+ }
- public Trigger get(Ex3DProHatSwitch hatSwitch) {
- return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
- }
+ public Trigger get(Ex3DProHatSwitch hatSwitch) {
+ return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
+ }
- public Joystick get() {
- return controller;
- }
+ public Joystick get() {
+ return controller;
+ }
}
import java.util.function.BooleanSupplier;
public class GameController extends Controller {
- // These are the different controller triggers
- public final BooleanSupplier LEFT_TRIGGER_BUTTON = () -> get(Axis.LEFT_TRIGGER) > 0.5,
- RIGHT_TRIGGER_BUTTON = () -> get(Axis.RIGHT_TRIGGER) > 0.5;
- public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
- ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
- ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
- ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
-
- public final BooleanSupplier
- LEFT_STICK_LEFT = () -> get(Axis.LEFT_X) < -0.75,
- LEFT_STICK_RIGHT = () -> get(Axis.LEFT_X) > 0.75,
- LEFT_STICK_UP = () -> get(Axis.LEFT_Y) < -0.75,
- LEFT_STICK_DOWN = () -> get(Axis.LEFT_Y) > 0.75;
- public final BooleanSupplier
- RIGHT_STICK_LEFT = () -> get(Axis.RIGHT_X) < -0.75,
- RIGHT_STICK_RIGHT = () -> get(Axis.RIGHT_X) > 0.75,
- RIGHT_STICK_UP = () -> get(Axis.RIGHT_Y) < -0.75,
- RIGHT_STICK_DOWN = () -> get(Axis.RIGHT_Y) > 0.75;
-
- public GameController(int port) {
- super(port);
+ // These are the different controller triggers
+ public final BooleanSupplier LEFT_TRIGGER_BUTTON = () -> get(Axis.LEFT_TRIGGER) > 0.5,
+ RIGHT_TRIGGER_BUTTON = () -> get(Axis.RIGHT_TRIGGER) > 0.5;
+ public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
+ ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
+ ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
+ ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
+ public final BooleanSupplier LEFT_STICK_LEFT = () -> get(Axis.LEFT_X) < -0.75,
+ LEFT_STICK_RIGHT = () -> get(Axis.LEFT_X) > 0.75,
+ LEFT_STICK_UP = () -> get(Axis.LEFT_Y) < -0.75,
+ LEFT_STICK_DOWN = () -> get(Axis.LEFT_Y) > 0.75;
+ public final BooleanSupplier RIGHT_STICK_LEFT = () -> get(Axis.RIGHT_X) < -0.75,
+ RIGHT_STICK_RIGHT = () -> get(Axis.RIGHT_X) > 0.75,
+ RIGHT_STICK_UP = () -> get(Axis.RIGHT_Y) < -0.75,
+ RIGHT_STICK_DOWN = () -> get(Axis.RIGHT_Y) > 0.75;
+
+ public GameController(int port) {
+ super(port);
+ }
+
+ public enum Button {
+ A(1),
+ B(2),
+ X(3),
+ Y(4),
+ LB(5),
+ RB(6),
+ BACK(7),
+ START(8),
+ LEFT_JOY(9),
+ RIGHT_JOY(10);
+
+ public final int id;
+
+ Button(final int id) {
+ this.id = id;
}
+ }
- public enum Button {
- A(1),
- B(2),
- X(3),
- Y(4),
- LB(5),
- RB(6),
- BACK(7),
- START(8),
- LEFT_JOY(9),
- RIGHT_JOY(10);
-
- public final int id;
-
- Button(final int id) {
- this.id = id;
- }
- }
-
- public enum Axis {
- LEFT_X(0),
- LEFT_Y(1),
- LEFT_TRIGGER(2),
- RIGHT_TRIGGER(3),
- RIGHT_X(4),
- RIGHT_Y(5);
+ public enum Axis {
+ LEFT_X(0),
+ LEFT_Y(1),
+ LEFT_TRIGGER(2),
+ RIGHT_TRIGGER(3),
+ RIGHT_X(4),
+ RIGHT_Y(5);
- public final int id;
+ public final int id;
- Axis(final int id) {
- this.id = id;
- }
+ Axis(final int id) {
+ this.id = id;
}
-
- public enum DPad {
- UNPRESSED(-1),
- UP(0),
- UP_RIGHT(45),
- RIGHT(90),
- DOWN_RIGHT(135),
- DOWN(180),
- DOWN_LEFT(235),
- LEFT(270),
- UP_LEFT(315);
-
- public final int angle;
-
- DPad(final int angle) {
- this.angle = angle;
- }
+ }
+
+ public enum DPad {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ DPad(final int angle) {
+ this.angle = angle;
}
+ }
- public enum RumbleStatus {
- RUMBLE_ON(0.7),
- RUMBLE_OFF(0);
+ public enum RumbleStatus {
+ RUMBLE_ON(0.7),
+ RUMBLE_OFF(0);
- public final double rumbleValue;
+ public final double rumbleValue;
- RumbleStatus(final double rumbleValue) {
- this.rumbleValue = rumbleValue;
- }
+ RumbleStatus(final double rumbleValue) {
+ this.rumbleValue = rumbleValue;
}
+ }
- public Trigger get(Button button) {
- return new Trigger(() -> controller.getRawButton(button.id));
- }
+ public Trigger get(Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
- public double get(Axis axis) {
- return controller.getRawAxis(axis.id);
- }
+ public double get(Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
- public Trigger get(DPad dPad) {
- return new Trigger(() -> controller.getPOV() == dPad.angle);
- }
+ public Trigger get(DPad dPad) {
+ return new Trigger(() -> controller.getPOV() == dPad.angle);
+ }
- public Joystick get() {
- return controller;
- }
+ public Joystick get() {
+ return controller;
+ }
- public void setRumble(RumbleStatus rumbleStatus) {
- controller.setRumble(RumbleType.kLeftRumble, rumbleStatus.rumbleValue);
- controller.setRumble(RumbleType.kRightRumble, rumbleStatus.rumbleValue);
- }
-}
\ No newline at end of file
+ public void setRumble(RumbleStatus rumbleStatus) {
+ controller.setRumble(RumbleType.kLeftRumble, rumbleStatus.rumbleValue);
+ controller.setRumble(RumbleType.kRightRumble, rumbleStatus.rumbleValue);
+ }
+}
import edu.wpi.first.wpilibj2.command.button.Trigger;
public class MadCatzController extends Controller {
- public final Trigger
- ALL_UP = get(MadCatzHatSwitch.UP).or(get(MadCatzHatSwitch.UP_LEFT)).or(get(MadCatzHatSwitch.UP_RIGHT)),
- ALL_DOWN = get(MadCatzHatSwitch.DOWN).or(get(MadCatzHatSwitch.DOWN_LEFT)).or(get(MadCatzHatSwitch.DOWN_RIGHT)),
- ALL_LEFT = get(MadCatzHatSwitch.LEFT).or(get(MadCatzHatSwitch.UP_LEFT)).or(get(MadCatzHatSwitch.DOWN_LEFT)),
- ALL_RIGHT = get(MadCatzHatSwitch.RIGHT).or(get(MadCatzHatSwitch.UP_RIGHT)).or(get(MadCatzHatSwitch.DOWN_RIGHT));
-
- public MadCatzController(int port) {
- super(port);
+ public final Trigger
+ ALL_UP =
+ get(MadCatzHatSwitch.UP)
+ .or(get(MadCatzHatSwitch.UP_LEFT))
+ .or(get(MadCatzHatSwitch.UP_RIGHT)),
+ ALL_DOWN =
+ get(MadCatzHatSwitch.DOWN)
+ .or(get(MadCatzHatSwitch.DOWN_LEFT))
+ .or(get(MadCatzHatSwitch.DOWN_RIGHT)),
+ ALL_LEFT =
+ get(MadCatzHatSwitch.LEFT)
+ .or(get(MadCatzHatSwitch.UP_LEFT))
+ .or(get(MadCatzHatSwitch.DOWN_LEFT)),
+ ALL_RIGHT =
+ get(MadCatzHatSwitch.RIGHT)
+ .or(get(MadCatzHatSwitch.UP_RIGHT))
+ .or(get(MadCatzHatSwitch.DOWN_RIGHT));
+
+ public MadCatzController(int port) {
+ super(port);
+ }
+
+ public enum MadCatzButton {
+ B1(1),
+ B2(2),
+ B3(3),
+ B4(4),
+ B6(6),
+ B7(7);
+
+ public final int id;
+
+ MadCatzButton(final int id) {
+ this.id = id;
}
+ }
- public enum MadCatzButton {
- B1(1),
- B2(2),
- B3(3),
- B4(4),
- B6(6),
- B7(7);
+ public enum MadCatzAxis {
+ X(0),
+ Y(1),
+ SLIDER(2),
+ ZROTATE(3);
- public final int id;
+ public final int id;
- MadCatzButton(final int id) {
- this.id = id;
- }
+ MadCatzAxis(final int id) {
+ this.id = id;
}
-
- public enum MadCatzAxis {
- X(0),
- Y(1),
- SLIDER(2),
- ZROTATE(3);
-
- public final int id;
-
- MadCatzAxis(final int id) {
- this.id = id;
- }
+ }
+
+ public enum MadCatzHatSwitch {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ MadCatzHatSwitch(final int angle) {
+ this.angle = angle;
}
+ }
- public enum MadCatzHatSwitch {
- UNPRESSED(-1),
- UP(0),
- UP_RIGHT(45),
- RIGHT(90),
- DOWN_RIGHT(135),
- DOWN(180),
- DOWN_LEFT(235),
- LEFT(270),
- UP_LEFT(315);
-
- public final int angle;
-
- MadCatzHatSwitch(final int angle) {
- this.angle = angle;
- }
- }
+ public Trigger get(MadCatzButton button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
- public Trigger get(MadCatzButton button) {
- return new Trigger(() -> controller.getRawButton(button.id));
- }
+ public double get(MadCatzAxis axis) {
+ return controller.getRawAxis(axis.id);
+ }
- public double get(MadCatzAxis axis) {
- return controller.getRawAxis(axis.id);
- }
-
- public Trigger get(MadCatzHatSwitch hatSwitch) {
- return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
- }
+ public Trigger get(MadCatzHatSwitch hatSwitch) {
+ return new Trigger(() -> controller.getPOV() == hatSwitch.angle);
+ }
- public Joystick get() {
- return controller;
- }
+ public Joystick get() {
+ return controller;
+ }
}
import java.util.function.BooleanSupplier;
public class PS5Controller extends Controller {
- // These are the different controller triggers
- public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
- ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
- ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
- ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
-
- public final BooleanSupplier
- LEFT_STICK_LEFT = () -> get(PS5Axis.LEFT_X) < -0.75,
- LEFT_STICK_RIGHT = () -> get(PS5Axis.LEFT_X) > 0.75,
- LEFT_STICK_UP = () -> get(PS5Axis.LEFT_Y) < -0.75,
- LEFT_STICK_DOWN = () -> get(PS5Axis.LEFT_Y) > 0.75;
- public final BooleanSupplier
- RIGHT_STICK_LEFT = () -> get(PS5Axis.RIGHT_X) < -0.75,
- RIGHT_STICK_RIGHT = () -> get(PS5Axis.RIGHT_X) > 0.75,
- RIGHT_STICK_UP = () -> get(PS5Axis.RIGHT_Y) < -0.75,
- RIGHT_STICK_DOWN = () -> get(PS5Axis.RIGHT_Y) > 0.75;
-
- public PS5Controller(int port) {
- super(port);
+ // These are the different controller triggers
+ public final Trigger ALL_UP = get(DPad.UP).or(get(DPad.UP_LEFT)).or(get(DPad.UP_RIGHT)),
+ ALL_DOWN = get(DPad.DOWN).or(get(DPad.DOWN_LEFT)).or(get(DPad.DOWN_RIGHT)),
+ ALL_LEFT = get(DPad.LEFT).or(get(DPad.UP_LEFT)).or(get(DPad.DOWN_LEFT)),
+ ALL_RIGHT = get(DPad.RIGHT).or(get(DPad.UP_RIGHT)).or(get(DPad.DOWN_RIGHT));
+ public final BooleanSupplier LEFT_STICK_LEFT = () -> get(PS5Axis.LEFT_X) < -0.75,
+ LEFT_STICK_RIGHT = () -> get(PS5Axis.LEFT_X) > 0.75,
+ LEFT_STICK_UP = () -> get(PS5Axis.LEFT_Y) < -0.75,
+ LEFT_STICK_DOWN = () -> get(PS5Axis.LEFT_Y) > 0.75;
+ public final BooleanSupplier RIGHT_STICK_LEFT = () -> get(PS5Axis.RIGHT_X) < -0.75,
+ RIGHT_STICK_RIGHT = () -> get(PS5Axis.RIGHT_X) > 0.75,
+ RIGHT_STICK_UP = () -> get(PS5Axis.RIGHT_Y) < -0.75,
+ RIGHT_STICK_DOWN = () -> get(PS5Axis.RIGHT_Y) > 0.75;
+
+ public PS5Controller(int port) {
+ super(port);
+ }
+
+ public enum PS5Button {
+ SQUARE(1),
+ CROSS(2),
+ CIRCLE(3),
+ TRIANGLE(4),
+ LB(5),
+ RB(6),
+ LEFT_TRIGGER(7),
+ RIGHT_TRIGGER(8),
+ CREATE(9),
+ OPTIONS(10),
+ LEFT_JOY(11),
+ RIGHT_JOY(12),
+ PS(13),
+ TOUCHPAD(14),
+ MUTE(15);
+
+ public final int id;
+
+ PS5Button(final int id) {
+ this.id = id;
}
-
- public enum PS5Button {
- SQUARE(1),
- CROSS(2),
- CIRCLE(3),
- TRIANGLE(4),
- LB(5),
- RB(6),
- LEFT_TRIGGER(7),
- RIGHT_TRIGGER(8),
- CREATE(9),
- OPTIONS(10),
- LEFT_JOY(11),
- RIGHT_JOY(12),
- PS(13),
- TOUCHPAD(14),
- MUTE(15);
-
- public final int id;
-
- PS5Button(final int id) {
- this.id = id;
- }
- }
-
- public enum PS5Axis {
- LEFT_X(0),
- LEFT_Y(1),
- RIGHT_X(2),
- /**
- * note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully
- * pressed
- */
- LEFT_TRIGGER(3),
- /**
- * note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully
- * pressed
- */
- RIGHT_TRIGGER(4),
- RIGHT_Y(5);
-
- public final int id;
-
- PS5Axis(final int id) {
- this.id = id;
- }
+ }
+
+ public enum PS5Axis {
+ LEFT_X(0),
+ LEFT_Y(1),
+ RIGHT_X(2),
+ /** note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully pressed */
+ LEFT_TRIGGER(3),
+ /** note: ps5 controller trigger goes from -1 when unpressed, to 1 when fully pressed */
+ RIGHT_TRIGGER(4),
+ RIGHT_Y(5);
+
+ public final int id;
+
+ PS5Axis(final int id) {
+ this.id = id;
}
-
- public enum DPad {
- UNPRESSED(-1),
- UP(0),
- UP_RIGHT(45),
- RIGHT(90),
- DOWN_RIGHT(135),
- DOWN(180),
- DOWN_LEFT(235),
- LEFT(270),
- UP_LEFT(315);
-
- public final int angle;
-
- DPad(final int angle) {
- this.angle = angle;
- }
+ }
+
+ public enum DPad {
+ UNPRESSED(-1),
+ UP(0),
+ UP_RIGHT(45),
+ RIGHT(90),
+ DOWN_RIGHT(135),
+ DOWN(180),
+ DOWN_LEFT(235),
+ LEFT(270),
+ UP_LEFT(315);
+
+ public final int angle;
+
+ DPad(final int angle) {
+ this.angle = angle;
}
+ }
- public Trigger get(PS5Button button) {
- return new Trigger(() -> controller.getRawButton(button.id));
- }
+ public Trigger get(PS5Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
- public double get(PS5Axis axis) {
- return controller.getRawAxis(axis.id);
- }
+ public double get(PS5Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
- public Trigger get(DPad dPad) {
- return new Trigger(() -> controller.getPOV() == dPad.angle);
- }
+ public Trigger get(DPad dPad) {
+ return new Trigger(() -> controller.getPOV() == dPad.angle);
+ }
- public Joystick get() {
- return controller;
- }
-}
\ No newline at end of file
+ public Joystick get() {
+ return controller;
+ }
+}
import edu.wpi.first.wpilibj2.command.button.Trigger;
public class PistolController extends Controller {
- public final Trigger TOP_BACK_ONLY = get(Button.TOP_BACK).and(get(Button.TOP_FRONT).negate()),
- TOP_FRONT_ONLY = get(Button.TOP_FRONT).and(get(Button.TOP_BACK).negate()),
- BOTTOM_BACK_ONLY = get(Button.BOTTOM_BACK).and(get(Button.BOTTOM_FRONT).negate()),
- BOTTOM_FRONT_ONLY = get(Button.BOTTOM_FRONT).and(get(Button.BOTTOM_BACK).negate());
-
- public PistolController(int port) {
- super(port);
- }
-
- public enum Button {
- TOP_BACK(1),
- TOP_FRONT(2),
- BOTTOM_FRONT(3),
- BOTTOM_BACK(4),
- BOTTOM(5);
-
- public final int id;
-
- Button(final int id) {
- this.id = id;
- }
+ public final Trigger TOP_BACK_ONLY = get(Button.TOP_BACK).and(get(Button.TOP_FRONT).negate()),
+ TOP_FRONT_ONLY = get(Button.TOP_FRONT).and(get(Button.TOP_BACK).negate()),
+ BOTTOM_BACK_ONLY = get(Button.BOTTOM_BACK).and(get(Button.BOTTOM_FRONT).negate()),
+ BOTTOM_FRONT_ONLY = get(Button.BOTTOM_FRONT).and(get(Button.BOTTOM_BACK).negate());
+
+ public PistolController(int port) {
+ super(port);
+ }
+
+ public enum Button {
+ TOP_BACK(1),
+ TOP_FRONT(2),
+ BOTTOM_FRONT(3),
+ BOTTOM_BACK(4),
+ BOTTOM(5);
+
+ public final int id;
+
+ Button(final int id) {
+ this.id = id;
}
+ }
- public enum Axis {
- WHEEL(0),
- TRIGGER(1);
+ public enum Axis {
+ WHEEL(0),
+ TRIGGER(1);
- public final int id;
+ public final int id;
- Axis(final int id) {
- this.id = id;
- }
+ Axis(final int id) {
+ this.id = id;
}
+ }
- public Trigger get(Button button) {
- return new Trigger(() -> controller.getRawButton(button.id));
- }
+ public Trigger get(Button button) {
+ return new Trigger(() -> controller.getRawButton(button.id));
+ }
- public double get(Axis axis) {
- return controller.getRawAxis(axis.id);
- }
+ public double get(Axis axis) {
+ return controller.getRawAxis(axis.id);
+ }
- public Joystick get() {
- return controller;
- }
+ public Joystick get() {
+ return controller;
+ }
}
import edu.wpi.first.math.geometry.Transform3d;
import frc.robot.util.Vision.Vision;
-/**
- * Tests if all of the AprilTags are in the right spot
- */
+/** Tests if all of the AprilTags are in the right spot */
public class AprilTagPoseTest {
/**
- * Tests if there are the right number of AprilTags, that the tags in Vision match the ones in FieldConstants, and that they are on the right side of the field
+ * Tests if there are the right number of AprilTags, that the tags in Vision match the ones in
+ * FieldConstants, and that they are on the right side of the field
*/
@Test
public void testTagPoses() {
// Check each tag in the field layout
for (int i = 0; i < vision.getAprilTagFieldLayout().getTags().size(); i++) {
- // The expected tagId. The ArrayList is zero-based and our tags start at 1, so the tagId is i+1.
+ // The expected tagId. The ArrayList is zero-based and our tags start at 1, so the tagId is
+ // i+1.
int tagId = i + 1;
// Get the poses from the two sources
assertEquals(p1.getRotation().getZ(), p2.getRotation().getZ(), 0.0001);
// 1-16 should be on the right, and 17-36 should be on the left
- if(tagId > 16){
- assertTrue(p1.getX() < FieldConstants.field.getFieldLength()/2);
- }else{
- assertTrue(p1.getX() > FieldConstants.field.getFieldLength()/2);
+ if (tagId > 16) {
+ assertTrue(p1.getX() < FieldConstants.field.getFieldLength() / 2);
+ } else {
+ assertTrue(p1.getX() > FieldConstants.field.getFieldLength() / 2);
}
}
}
@Test
- public void testReefTags(){
- List<Pose3d> redPoses = FieldConstants.field.getTags().subList(1, 16).stream().map(tag->tag.pose).toList();
- List<Pose3d> bluePoses = FieldConstants.field.getTags().subList(17, 32).stream().map(tag->tag.pose).toList();
+ public void testReefTags() {
+ List<Pose3d> redPoses =
+ FieldConstants.field.getTags().subList(1, 16).stream().map(tag -> tag.pose).toList();
+ List<Pose3d> bluePoses =
+ FieldConstants.field.getTags().subList(17, 32).stream().map(tag -> tag.pose).toList();
Pose3d redCenter = findCenter(redPoses);
Pose3d blueCenter = findCenter(bluePoses);
-
+
// The tags should be symmetrical, so the total rotation should be 0
assertEquals(redCenter.getRotation().getX(), 0, 0.0001);
assertEquals(blueCenter.getRotation().getX(), 0, 0.0001);
assertEquals(redCenter.getZ(), blueCenter.getZ(), 0.0001);
// X should be mirrored
- assertEquals(redCenter.getX(), FieldConstants.field.getFieldLength()-blueCenter.getX(), 0.01);
+ assertEquals(redCenter.getX(), FieldConstants.field.getFieldLength() - blueCenter.getX(), 0.01);
// Compare each matching pair of tags
- for(int i = 1; i < 17; i++){
+ for (int i = 1; i < 17; i++) {
Pose3d red = FieldConstants.field.getTagPose(i).get();
- Pose3d blue = FieldConstants.field.getTagPose(i+16).get();
+ Pose3d blue = FieldConstants.field.getTagPose(i + 16).get();
assertEquals(red.getY(), FieldConstants.field.getFieldWidth() - blue.getY(), 0.01);
assertEquals(red.getZ(), blue.getZ(), 0.0001);
- assertEquals(red.getX(), FieldConstants.field.getFieldLength()-blue.getX(), 0.01);
- assertEquals(MathUtil.angleModulus(red.getRotation().getZ()), MathUtil.angleModulus(blue.getRotation().getZ() + Math.PI), 0.0001);
+ assertEquals(red.getX(), FieldConstants.field.getFieldLength() - blue.getX(), 0.01);
+ assertEquals(
+ MathUtil.angleModulus(red.getRotation().getZ()),
+ MathUtil.angleModulus(blue.getRotation().getZ() + Math.PI),
+ 0.0001);
}
}
/**
* Gets the center pose with the sum of the rotations, used for checking the reef
+ *
* @param poses The poses to find the center of
* @return A pose with the translation at the center and the sum of the rotations
*/
- private Pose3d findCenter(List<Pose3d> poses){
+ private Pose3d findCenter(List<Pose3d> poses) {
double x = 0;
double y = 0;
double z = 0;
Rotation3d rot = new Rotation3d();
- for(Pose3d pose : poses){
+ for (Pose3d pose : poses) {
x += pose.getX();
y += pose.getY();
z += pose.getZ();
import static org.junit.jupiter.api.Assertions.assertEquals;
-/**
- * Check some robot constants/parameters.
- */
+/** Check some robot constants/parameters. */
public class ConstantsTest {
- @Test
- public void testRobotSize() {
- // The competition robot frame width and length is 26 inches.
- // It has 3/16 inch plates on all sides,
- // so frame width is 26.375!
- double widthFrame = Units.inchesToMeters(26.0);
- double widthFrameAndPlates = widthFrame + 2.0 * Units.inchesToMeters(3.0 / 16.0);
-
- // At Port Hueneme, the frame perimeter was 105 inches
- // frame perimeter is 105.5...
- assertEquals(105.0, 4 * Units.metersToInches(widthFrameAndPlates), 0.501);
-
- // Bumpers.
- // The backing board is 0.75 inches.
- // The noodles are 2.5 inches.
- // Board + noodles = 3.25 inches
- // Measure the red bumpers, and they are 3.5 inches.
- //
- // The latch studs are centered on the 1x2 frame rails. That's 0.5 inches
- // The bumper latch bracket holes for the studs are 3/4 in from the inside surface of the bumpers.
- // That puts the bumper inside surface at 3/4 - (0.5) = 1/4 inch out from widthFrame
- double thickBumpers = Units.inchesToMeters(3.5 + 0.25);
-
- // so width with bumpers is
- @SuppressWarnings("unused")
- double widthFrameWithBumpers = widthFrame + 2 * thickBumpers;
+ @Test
+ public void testRobotSize() {
+ // The competition robot frame width and length is 26 inches.
+ // It has 3/16 inch plates on all sides,
+ // so frame width is 26.375!
+ double widthFrame = Units.inchesToMeters(26.0);
+ double widthFrameAndPlates = widthFrame + 2.0 * Units.inchesToMeters(3.0 / 16.0);
- // check with values in DriveConstants
- // System.out.printf("widthFrameWithBumpers = %8f %8f\n", widthFrameWithBumpers, Units.metersToInches(widthFrameWithBumpers));
- // System.out.printf("kRobotWidthWithBumpers = %8f %8f\n", DriveConstants.kRobotWidthWithBumpers, Units.metersToInches(DriveConstants.kRobotWidthWithBumpers));
- // assertEquals(widthFrameWithBumpers, DriveConstants.kRobotWidthWithBumpers, 0.001);
- }
+ // At Port Hueneme, the frame perimeter was 105 inches
+ // frame perimeter is 105.5...
+ assertEquals(105.0, 4 * Units.metersToInches(widthFrameAndPlates), 0.501);
- /**
- * MK4i module
- * https://www.swervedrivespecialties.com/products/mk4i-swerve-module
- */
- enum SwerveDriveSpecialties {
- // Gearbox ratios from the SDS webpage
- // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
- L1((50.0 / 14.0) * (19.0 / 25.0) * (45.0 / 15.0)),
- L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)),
- L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)),
- // 16-tooth pinion
- // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
- L1P((50.0 / 16.0) * (19.0 / 25.0) * (45.0 / 15.0)),
- L2P((50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0)),
- L3P((50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0));
+ // Bumpers.
+ // The backing board is 0.75 inches.
+ // The noodles are 2.5 inches.
+ // Board + noodles = 3.25 inches
+ // Measure the red bumpers, and they are 3.5 inches.
+ //
+ // The latch studs are centered on the 1x2 frame rails. That's 0.5 inches
+ // The bumper latch bracket holes for the studs are 3/4 in from the inside surface of the
+ // bumpers.
+ // That puts the bumper inside surface at 3/4 - (0.5) = 1/4 inch out from widthFrame
+ double thickBumpers = Units.inchesToMeters(3.5 + 0.25);
- /**
- * Drive gear ratio varies for each module
- */
- final double driveRatio;
- /**
- * Steering Gear ratio (same for all MK4i modules)
- */
- final double steerRatio = 150.0 / 7.0;
+ // so width with bumpers is
+ @SuppressWarnings("unused")
+ double widthFrameWithBumpers = widthFrame + 2 * thickBumpers;
- SwerveDriveSpecialties(double drive) {
- this.driveRatio = drive;
- }
- }
+ // check with values in DriveConstants
+ // System.out.printf("widthFrameWithBumpers = %8f %8f\n", widthFrameWithBumpers,
+ // Units.metersToInches(widthFrameWithBumpers));
+ // System.out.printf("kRobotWidthWithBumpers = %8f %8f\n",
+ // DriveConstants.kRobotWidthWithBumpers,
+ // Units.metersToInches(DriveConstants.kRobotWidthWithBumpers));
+ // assertEquals(widthFrameWithBumpers, DriveConstants.kRobotWidthWithBumpers, 0.001);
+ }
+
+ /** MK4i module https://www.swervedrivespecialties.com/products/mk4i-swerve-module */
+ enum SwerveDriveSpecialties {
+ // Gearbox ratios from the SDS webpage
+ // https://www.swervedrivespecialties.com/products/mk4i-swerve-module
+ L1((50.0 / 14.0) * (19.0 / 25.0) * (45.0 / 15.0)),
+ L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)),
+ L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)),
+ // 16-tooth pinion
+ // https://www.swervedrivespecialties.com/products/kit-adapter-16t-drive-pinion-gear-mk4i
+ L1P((50.0 / 16.0) * (19.0 / 25.0) * (45.0 / 15.0)),
+ L2P((50.0 / 16.0) * (17.0 / 27.0) * (45.0 / 15.0)),
+ L3P((50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0));
- @Test
- public void testSwerveRatios() {
- // check the mroe exact ratios against the published-to-2-digits ratios
- assertEquals(8.14, SwerveDriveSpecialties.L1.driveRatio, 0.01);
- assertEquals(6.75, SwerveDriveSpecialties.L2.driveRatio, 0.01);
- assertEquals(6.12, SwerveDriveSpecialties.L3.driveRatio, 0.01);
+ /** Drive gear ratio varies for each module */
+ final double driveRatio;
- // The drive ratio could be more accurate, but does not hurt
- assertEquals(SwerveDriveSpecialties.L2P.driveRatio, DriveConstants.DRIVE_GEAR_RATIO, 0.01);
+ /** Steering Gear ratio (same for all MK4i modules) */
+ final double steerRatio = 150.0 / 7.0;
- // The steer ratio
- // print the relative error: 0.6e-4. After 100 rotations, error would be 0.6e-2 rotations (about 1.5 degrees)
- // System.out.println((DriveConstants.kSteerGearRatio - SwerveDriveSpecialties.L2.steerRatio) / SwerveDriveSpecialties.L2.steerRatio);
- assertEquals(SwerveDriveSpecialties.L2P.steerRatio, DriveConstants.STEER_GEAR_RATIO, 0.01);
+ SwerveDriveSpecialties(double drive) {
+ this.driveRatio = drive;
}
+ }
+
+ @Test
+ public void testSwerveRatios() {
+ // check the mroe exact ratios against the published-to-2-digits ratios
+ assertEquals(8.14, SwerveDriveSpecialties.L1.driveRatio, 0.01);
+ assertEquals(6.75, SwerveDriveSpecialties.L2.driveRatio, 0.01);
+ assertEquals(6.12, SwerveDriveSpecialties.L3.driveRatio, 0.01);
+
+ // The drive ratio could be more accurate, but does not hurt
+ assertEquals(SwerveDriveSpecialties.L2P.driveRatio, DriveConstants.DRIVE_GEAR_RATIO, 0.01);
+
+ // The steer ratio
+ // print the relative error: 0.6e-4. After 100 rotations, error would be 0.6e-2 rotations (about
+ // 1.5 degrees)
+ // System.out.println((DriveConstants.kSteerGearRatio - SwerveDriveSpecialties.L2.steerRatio) /
+ // SwerveDriveSpecialties.L2.steerRatio);
+ assertEquals(SwerveDriveSpecialties.L2P.steerRatio, DriveConstants.STEER_GEAR_RATIO, 0.01);
+ }
}
import static org.junit.jupiter.api.Assertions.assertEquals;
/**
- * Example of a JUnit test class.
- * This test should run everytime someone builds the robot code.
- * See https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/unit-testing.html
- * <p>
- * To disable a test, annotate with Disabled
+ * Example of a JUnit test class. This test should run everytime someone builds the robot code. See
+ * https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/unit-testing.html
+ *
+ * <p>To disable a test, annotate with Disabled
*/
public class ArithTest {
- @BeforeEach
- public void prepare() {
- }
+ @BeforeEach
+ public void prepare() {}
- @AfterEach
- public void cleanup() {
- }
+ @AfterEach
+ public void cleanup() {}
- /**
- * Test if floating point addition works.
- */
- @Test
- public void testSimpleArith() {
- assertEquals(5.0, 2.0 + 3.0, 0.0001);
- }
-
- /**
- * Here is a disabled test
- */
- @Disabled
- @Test
- public void testNaught() {
- assertEquals(0, 0);
- }
+ /** Test if floating point addition works. */
+ @Test
+ public void testSimpleArith() {
+ assertEquals(5.0, 2.0 + 3.0, 0.0001);
+ }
+ /** Here is a disabled test */
+ @Disabled
+ @Test
+ public void testNaught() {
+ assertEquals(0, 0);
+ }
}
import frc.robot.util.Vision.DetectedObject;
import frc.robot.util.Vision.DetectedObject.ObjectType;
-/**
- * Tests DetectedObject
- */
+/** Tests DetectedObject */
public class DetectedObjectTest {
@BeforeEach
@AfterEach
public void cleanup() {}
- /**
- * Tests if the objec pose is correct
- */
+ /** Tests if the objec pose is correct */
@Test
public void testObjectPose() {
- DetectedObject object = new DetectedObject(
- Units.degreesToRadians(45),
- 0,
- 1,
- ObjectType.NONE,
- new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI/2, Math.PI/2))
- );
- Translation3d expected = new Translation3d(Math.sqrt(2)/2, 0, Math.sqrt(2)/2+1);
- assertEquals(expected.getX(), object.pose.getX(), 0.001);
- assertEquals(expected.getY(), object.pose.getY(), 0.001);
- assertEquals(expected.getZ(), object.pose.getZ(), 0.001);
+ DetectedObject object =
+ new DetectedObject(
+ Units.degreesToRadians(45),
+ 0,
+ 1,
+ ObjectType.NONE,
+ new Transform3d(
+ new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI / 2, Math.PI / 2)));
+ Translation3d expected = new Translation3d(Math.sqrt(2) / 2, 0, Math.sqrt(2) / 2 + 1);
+ assertEquals(expected.getX(), object.pose.getX(), 0.001);
+ assertEquals(expected.getY(), object.pose.getY(), 0.001);
+ assertEquals(expected.getZ(), object.pose.getZ(), 0.001);
}
-
- /**
- * Tests the position of an object when distance is not specified
- */
+
+ /** Tests the position of an object when distance is not specified */
@Test
- public void testObjectPoseWithoutDistance(){
- DetectedObject object = new DetectedObject(
- 0,
- -Units.degreesToRadians(20),
- ObjectType.NONE,
- new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Units.degreesToRadians(25), 0))
- );
- Translation3d expected = new Translation3d(1, 0, 0);
- assertEquals(expected.getX(), object.pose.getX(), 0.001);
- assertEquals(expected.getY(), object.pose.getY(), 0.001);
- assertEquals(expected.getZ(), object.pose.getZ(), 0.001);
+ public void testObjectPoseWithoutDistance() {
+ DetectedObject object =
+ new DetectedObject(
+ 0,
+ -Units.degreesToRadians(20),
+ ObjectType.NONE,
+ new Transform3d(
+ new Translation3d(0, 0, 1), new Rotation3d(0, Units.degreesToRadians(25), 0)));
+ Translation3d expected = new Translation3d(1, 0, 0);
+ assertEquals(expected.getX(), object.pose.getX(), 0.001);
+ assertEquals(expected.getY(), object.pose.getY(), 0.001);
+ assertEquals(expected.getZ(), object.pose.getZ(), 0.001);
}
- /**
- * Tests if the object is on the ground using random offsets
- */
+ /** Tests if the object is on the ground using random offsets */
@Test
- public void testObjectOnGround(){
+ public void testObjectOnGround() {
Random random = new Random();
- DetectedObject object = new DetectedObject(
- random.nextDouble(-Math.PI, Math.PI),
- random.nextDouble(0.001, Math.PI/4),
- ObjectType.NONE,
- new Transform3d(new Translation3d(
- random.nextDouble(0, 100),
- random.nextDouble(0, 100),
- random.nextDouble(0.1, 100)
- ), new Rotation3d(
- 0, random.nextDouble(0.001, Math.PI/4), random.nextDouble(-Math.PI, Math.PI)
- ))
- );
+ DetectedObject object =
+ new DetectedObject(
+ random.nextDouble(-Math.PI, Math.PI),
+ random.nextDouble(0.001, Math.PI / 4),
+ ObjectType.NONE,
+ new Transform3d(
+ new Translation3d(
+ random.nextDouble(0, 100),
+ random.nextDouble(0, 100),
+ random.nextDouble(0.1, 100)),
+ new Rotation3d(
+ 0,
+ random.nextDouble(0.001, Math.PI / 4),
+ random.nextDouble(-Math.PI, Math.PI))));
assertEquals(object.pose.getZ(), 0, 0.001);
}
}
import edu.wpi.first.wpilibj2.command.InstantCommand;
-/**
- * Simple check on PathPlanner path
- */
+/** Simple check on PathPlanner path */
public class PathCheck {
- /**
- * Register placeholder commands for testing.
- * These are normally registered in RobotContainer.registerCommands(),
- * but the test doesn't instantiate RobotContainer.
- */
- @BeforeAll
- public static void registerPlaceholderCommands() {
- NamedCommands.registerCommand("Extend intake", new InstantCommand());
- NamedCommands.registerCommand("Intake", new InstantCommand());
- }
+ /**
+ * Register placeholder commands for testing. These are normally registered in
+ * RobotContainer.registerCommands(), but the test doesn't instantiate RobotContainer.
+ */
+ @BeforeAll
+ public static void registerPlaceholderCommands() {
+ NamedCommands.registerCommand("Extend intake", new InstantCommand());
+ NamedCommands.registerCommand("Intake", new InstantCommand());
+ }
- /**
- * Load the path groups.
- * <p>
- * We have had problems with syntax errors in a path.
- */
- @Test
- public void pathGroupLoaderTest() {
- // load the paths
- // may throw a ParseException; that error will fail this test
- PathGroupLoader.loadPathGroups();
- }
+ /**
+ * Load the path groups.
+ *
+ * <p>We have had problems with syntax errors in a path.
+ */
+ @Test
+ public void pathGroupLoaderTest() {
+ // load the paths
+ // may throw a ParseException; that error will fail this test
+ PathGroupLoader.loadPathGroups();
+ }
}
public class PolynomialRegressionTest {
- /**
- * Unit tests the {@code PolynomialRegression} data type.
- */
- @Test
- public void testRegression() {
- double[] x = {10, 20, 40, 80, 160, 200};
- double[] y = {100, 350, 1500, 6700, 20160, 40000};
- PolynomialRegression regression = new PolynomialRegression(x, y, 3);
-
- assertEquals(regression.beta(3), 0.0092, 0.0001);
- assertEquals(regression.beta(2), -1.6395, 0.0001);
- assertEquals(regression.beta(1), 168.9232, 0.0001);
- assertEquals(regression.beta(0), -2113.7306, 0.0001);
- }
+ /** Unit tests the {@code PolynomialRegression} data type. */
+ @Test
+ public void testRegression() {
+ double[] x = {10, 20, 40, 80, 160, 200};
+ double[] y = {100, 350, 1500, 6700, 20160, 40000};
+ PolynomialRegression regression = new PolynomialRegression(x, y, 3);
+ assertEquals(regression.beta(3), 0.0092, 0.0001);
+ assertEquals(regression.beta(2), -1.6395, 0.0001);
+ assertEquals(regression.beta(1), 168.9232, 0.0001);
+ assertEquals(regression.beta(0), -2113.7306, 0.0001);
+ }
}