Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
Team2890HawkCollective
2026-02-19 16:33:09 -05:00
commit 6b173fab83
50 changed files with 25061 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

165
src/main/java/frc/robot/Constants.java Normal file
View File

@@ -0,0 +1,165 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import swervelib.math.Matter;
/**
* The Constants class provides a convenient place for teams to hold robot-wide
* numerical or boolean constants. This
* class should not be used for any other purpose. All constants should be
* declared globally (i.e. public static). Do
* not put anything functional in this class.
*
* <p>
* It is advised to statically import this class (or one of its inner classes)
* wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
private static ShuffleboardTab programmingTab = Shuffleboard.getTab("Programming");
public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound
public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag
public static final double MAX_SPEED = Units.feetToMeters(14.5);
// Maximum speed of the robot in meters per second, used to limit acceleration.
// public static final class AutonConstants
// {
//
// public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0,
// 0);
// public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01);
// }
public static final class DrivebaseConstants {
// Hold time on motor brakes when disabled
public static final double WHEEL_LOCK_TIME = 10; // seconds
}
public static class OperatorConstants {
// Joystick Deadband
public static final double DEADBAND = 0.1;
public static final double LEFT_Y_DEADBAND = 0.1;
public static final double RIGHT_X_DEADBAND = 0.1;
public static final double TURN_CONSTANT = 6;
}
public static class ShooterConstants {
private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter Velocity", -0.5)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
public static double SHOOTER_VELOCITY = -0.6;
public static double SHOOTER_POWER = -0.45;
public static void getShooterVelocity() {
SHOOTER_VELOCITY = shooterVelocity.getDouble(-0.5);
}
public static final int CENTER_SHOOTER_MOTOR_ID = 42;
public static final int LEFT_SHOOTER_MOTOR_ID = 41;
public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
public static final int INDEXER_MOTOR_ID = 43;
public static double INDEXER_MOTOR_SPEED = 0.6;
private static GenericEntry indexerAndRampSpeed = programmingTab.add("Desired Ramp + Indexer Speed", 0.6)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
// this method called in robot periodic so values updated in elastic are
// constantly read and applied to RAMP_MOTOR_SPEED
public static void getRampMotorSpeed() {
INDEXER_MOTOR_SPEED = indexerAndRampSpeed.getDouble(.6);
}
public static final int LEFT_ACTUATOR_PWM_PORT = 1;
public static final int RIGHT_ACTUATOR_PWM_PORT = 3;
public static double DESIRED_POTENTIOMETER_DISTANCE;
// TO DO: need to equation that calculates desired potentiometer distance
}
public static class IntakeConstants {
private static GenericEntry intakeSpeed = programmingTab.add("Desired Intake Speed", -0.4)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
public static double INTAKE_WHEELS_MOTOR_SPEED;
public static void getIntakeWheelsSpeed() {
INTAKE_WHEELS_MOTOR_SPEED = intakeSpeed.getDouble(-0.4);
}
public static final int INTAKE_WHEELS_MOTOR_ID = 50;
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = 2.2550222873687744;
public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
public static class IntakeRotatorPID {
public static final double INTAKE_ROTATOR_P = 0.05;
public static final double INTAKE_ROTATOR_I = 0;
public static final double INTAKE_ROTATOR_D = 0.001;
}
}
public static class RampConstants {
public static final int RAMP_MOTOR_ID = 45;
public static double RAMP_MOTOR_SPEED = .6;
// create object and a new widget under programming tab in Elastic where object
// retrieves value from widget
}
public static class LimeLight {
public static final String LIMELIGHT_NAME = "limelight";
// public static final int[] ALL_REEF_APRILTAGS = { 6, 7, 8, 9, 10, 11, 17, 18,
// 19, 20, 21, 22 };
public static final AprilTagFieldLayout APRILTAG_FIELD_LAYOUT = AprilTagFieldLayout
.loadField(AprilTagFields.k2026RebuiltAndymark);
public static final double BUMPER_WIDTH = Units.inchesToMeters(0.0); // Get This Value // Original: 2.75
// public static final double ROBOT_WIDTH = Units.inchesToMeters(30 +
// BUMPER_WIDTH); // Tis a square, don't need this
public static final double ROBOT_SIDE_LENGTH = Units.inchesToMeters(27);
// public static final Transform2d HALF_ROBOT = new
// Transform2d(ROBOT_SIDE_LENGTH / 3.0, 0, new Rotation2d());
public static double LIMELIGHT_TY;
}
public static class TargetingConstants {
public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
}
public static class ClimberConstants {
public static final int CLIMB_MOTOR_ID = 60;
public static final int RATCHET_PWM_PORT = 0;
public static final double RATCHET_UNLOCK_ANGLE = 0;
public static final double RATCHET_LOCK_ANGLE = 180;
public static final double CLIMBER_SPEED = .5;
}
}

1947
src/main/java/frc/robot/LimelightHelpers.java Normal file
View File

File diff suppressed because it is too large Load Diff

29
src/main/java/frc/robot/Main.java Normal file
View File

@@ -0,0 +1,29 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what you are doing, do
* not modify this file except to change the parameter class to the startRobot call.
*/
public final class 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);
}
}

190
src/main/java/frc/robot/Robot.java Normal file
View File

@@ -0,0 +1,190 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.subsystems.TargetingSubsystems;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as
* described in the TimedRobot documentation. If you change the name of this
* class or the package after creating this
* project, you must also update the build.gradle file in the project.
*/
public class Robot extends TimedRobot {
private static Robot instance;
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
private Timer disabledTimer;
private static NetworkTable table;
private static GenericEntry distanceFromLimelight;
public Robot() {
instance = this;
}
public static Robot getInstance() {
return instance;
}
/**
* This function is run when the robot is first started up and should be used
* for any initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings,
// and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
// Create a timer to disable motor brake a few seconds after disable. This will
// let the robot stop
// immediately when disabled, but then also let it be pushed more
disabledTimer = new Timer();
if (isSimulation()) {
DriverStation.silenceJoystickConnectionWarning(true);
}
table = NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
distanceFromLimelight = Shuffleboard.getTab("Programming").add("Distance from Limelight", 0).getEntry();
}
/**
* This function is called every 20 ms, 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();
// Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
Constants.IntakeConstants.getIntakeWheelsSpeed();
Constants.ShooterConstants.getShooterVelocity();
Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
//distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
m_robotContainer.setMotorBrake(true);
disabledTimer.reset();
disabledTimer.start();
}
@Override
public void disabledPeriodic() {
if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) {
m_robotContainer.setMotorBrake(false);
disabledTimer.stop();
disabledTimer.reset();
}
}
/**
* This autonomous runs the autonomous command selected by your
* {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_robotContainer.setMotorBrake(true);
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// Print the selected autonomous command upon autonomous init
System.out.println("Auto selected: " + m_autonomousCommand);
// schedule the autonomous command selected in the autoChooser
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
}
@Override
public void teleopInit() {
// 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 (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
} else {
CommandScheduler.getInstance().cancelAll();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
@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() {
}
/**
* This function is called once when the robot is first started up.
*/
@Override
public void simulationInit() {
}
/**
* This function is called periodically whilst in simulation.
*/
@Override
public void simulationPeriodic() {
}
}

308
src/main/java/frc/robot/RobotContainer.java Normal file
View File

@@ -0,0 +1,308 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.OperatorConstants;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import java.io.File;
import java.lang.annotation.Target;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.function.DoubleSupplier;
import frc.robot.subsystems.TargetingSubsystems;
import swervelib.SwerveInputStream;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.ClimberSubsystem;
/**
* 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 {
// Replace with CommandPS4Controller or CommandJoystick if needed
final CommandXboxController driverXbox = new CommandXboxController(0);
// The robot's subsystems and commands are defined here...
private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
"swerve/neo"));
// Establish a Sendable Chooser that will be able to be sent to the
// SmartDashboard, allowing selection of desired auto
private final SendableChooser<Command> autoChooser;
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
// private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
/**
* Converts driver input into a field-relative ChassisSpeeds that is controlled
* by angular velocity.
*/
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> driverXbox.getLeftY() * -1,
() -> driverXbox.getLeftX() * -1)
.withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
/**
* Clone's the angular velocity input stream and converts it to a fieldRelative
* input stream.
*/
SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX,
driverXbox::getRightY)
.headingWhile(true);
/**
* Clone's the angular velocity input stream and converts it to a robotRelative
* input stream.
*/
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
.allianceRelativeControl(true);
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> -driverXbox.getLeftY(),
() -> -driverXbox.getLeftX())
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
2))
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
// Derive the heading axis with math!
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
.withControllerHeadingAxis(() -> Math.sin(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2),
() -> Math.cos(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2))
.headingWhile(true)
.translationHeadingOffset(true)
.translationHeadingOffset(Rotation2d.fromDegrees(
0));
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
DriverStation.silenceJoystickConnectionWarning(true);
// Create the NamedCommands that will be used in PathPlanner
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
// Have the autoChooser pull in all PathPlanner autos as options
autoChooser = AutoBuilder.buildAutoChooser();
// Set the default auto (do nothing)
autoChooser.setDefaultOption("Do Nothing", Commands.none());
// Add a simple auto option to have the robot drive forward for 1 second then
// stop
autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
// Put the autoChooser on the SmartDashboard
SmartDashboard.putData("Auto Chooser", autoChooser);
}
/**
* Use this method to define your trigger->command mappings. Triggers can be
* created via the
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
* an arbitrary predicate, or via the
* named factories in
* {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
* for
* {@link CommandXboxController
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
* controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
* Flight joysticks}.
*/
private void configureBindings() {
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngle);
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard);
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngleKeyboard);
driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
// command for
// full shooting system including linear actuators
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()
.andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand().andThen(m_IntakeSubsystem.deployintakeCommand()));
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
// () -> -driverXbox.getLeftX()));
if (driverXbox.getRightY() < -0.4) {
m_ClimberSubsystem.liftRobotCommand();
} else if (driverXbox.getRightY() > 0.4) {
m_ClimberSubsystem.lowerRobotCommand();
} else {
m_ClimberSubsystem.stopClimberCommand();
}
// driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
}
if (Robot.isSimulation()) {
Pose2d target = new Pose2d(new Translation2d(1, 4),
Rotation2d.fromDegrees(90));
// drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
driveDirectAngleKeyboard.driveToPose(() -> target,
new ProfiledPIDController(5,
0,
0,
new Constraints(5, 2)),
new ProfiledPIDController(5,
0,
0,
new Constraints(Units.degreesToRadians(360),
Units.degreesToRadians(180))));
driverXbox.start()
.onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
() -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
// driverXbox.b().whileTrue(
// drivebase.driveToPose(
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
// );
}
if (DriverStation.isTest()) {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above!
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
driverXbox.leftBumper().onTrue(Commands.none());
driverXbox.rightBumper().onTrue(Commands.none());
} else {
driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
driverXbox.start().whileTrue(Commands.none());
driverXbox.back().whileTrue(Commands.none());
driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.rightBumper().onTrue(Commands.none());
}
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Pass in the selected auto from the SmartDashboard as our desired autnomous
// commmand
return autoChooser.getSelected();
}
public void setMotorBrake(boolean brake) {
drivebase.setMotorBrake(brake);
}
public Command aimAtHopperCommand(DoubleSupplier xSup, DoubleSupplier ySup) {
try (PIDController aimPIDs = new PIDController(0.3, 0, 0.001)) {
aimPIDs.setTolerance(1.0);
return Commands.run(() -> {
double xSpeed = xSup.getAsDouble();
double ySpeed = ySup.getAsDouble();
double rot = 0.0;
if (LimelightHelpers.getTV("limelight")) {
double tx = LimelightHelpers.getTX("limelight");
rot = aimPIDs.calculate(tx, 0);
rot = MathUtil.clamp(rot, -1.5, 1.5);
}
drivebase.drive(new Translation2d(xSpeed, ySpeed), rot, false);
},
drivebase);
}
}
public SwerveSubsystem getSwerveDriveBase() {
return drivebase;
}
public CommandXboxController getDriverXbox() {
return driverXbox;
}
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
// m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
}

86
src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java Normal file
View File

@@ -0,0 +1,86 @@
package frc.robot.commands.swervedrive.auto;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
/**
* Auto Balance command using a simple PID controller. Created by Team 3512
* <a href="https://github.com/frc3512/Robot-2023/blob/main/src/main/java/frc3512/robot/commands/AutoBalance.java">...</a>
*/
public class AutoBalanceCommand extends Command
{
private final SwerveSubsystem swerveSubsystem;
private final PIDController controller;
public AutoBalanceCommand(SwerveSubsystem swerveSubsystem)
{
this.swerveSubsystem = swerveSubsystem;
controller = new PIDController(1.0, 0.0, 0.0);
controller.setTolerance(1);
controller.setSetpoint(0.0);
// each subsystem used by the command must be passed into the
// addRequirements() method (which takes a vararg of Subsystem)
addRequirements(this.swerveSubsystem);
}
/**
* The initial subroutine of a command. Called once when the command is initially scheduled.
*/
@Override
public void initialize()
{
}
/**
* The main body of a command. Called repeatedly while the command is scheduled. (That is, it is called repeatedly
* until {@link #isFinished()}) returns true.)
*/
@Override
public void execute()
{
SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint());
double translationVal = MathUtil.clamp(controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5,
0.5);
swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true);
}
/**
* <p>
* Returns whether this command has finished. Once a command finishes -- indicated by this method returning true --
* the scheduler will call its {@link #end(boolean)} method.
* </p><p>
* Returning false will result in the command never ending automatically. It may still be cancelled manually or
* interrupted by another command. Hard coding this command to always return true will result in the command executing
* once and finishing immediately. It is recommended to use *
* {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an operation.
* </p>
*
* @return whether this command has finished.
*/
@Override
public boolean isFinished()
{
return controller.atSetpoint();
}
/**
* The action to take when the command ends. Called when either the command finishes normally -- that is it is called
* when {@link #isFinished()} returns true -- or when it is interrupted/canceled. This is where you may want to wrap
* up loose ends, like shutting off a motor that was being used in the command.
*
* @param interrupted whether the command was interrupted/canceled
*/
@Override
public void end(boolean interrupted)
{
swerveSubsystem.lock();
}
}

120
src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java Normal file
View File

@@ -0,0 +1,120 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands.swervedrive.drivebase;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import java.util.List;
import java.util.function.DoubleSupplier;
import swervelib.SwerveController;
import swervelib.math.SwerveMath;
/**
* An example command that uses an example subsystem.
*/
public class AbsoluteDrive extends Command
{
private final SwerveSubsystem swerve;
private final DoubleSupplier vX, vY;
private final DoubleSupplier headingHorizontal, headingVertical;
private boolean initRotation = false;
/**
* Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is
* torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian
* coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot
* will rotate to.
*
* @param swerve The swerve drivebase subsystem.
* @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1
* to 1 with deadband already accounted for. Positive X is away from the alliance wall.
* @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1
* to 1 with deadband already accounted for. Positive Y is towards the left wall when
* looking through the driver station glass.
* @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's heading angle. In the
* robot coordinate system, this is along the same axis as vY. Should range from -1 to 1 with
* no deadband. Positive is towards the left wall when looking through the driver station
* glass.
* @param headingVertical DoubleSupplier that supplies the vertical component of the robot's heading angle. In the
* robot coordinate system, this is along the same axis as vX. Should range from -1 to 1
* with no deadband. Positive is away from the alliance wall.
*/
public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingHorizontal,
DoubleSupplier headingVertical)
{
this.swerve = swerve;
this.vX = vX;
this.vY = vY;
this.headingHorizontal = headingHorizontal;
this.headingVertical = headingVertical;
addRequirements(swerve);
}
@Override
public void initialize()
{
initRotation = true;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute()
{
// Get the desired chassis speeds based on a 2 joystick module.
ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(),
headingHorizontal.getAsDouble(),
headingVertical.getAsDouble());
// Prevent Movement After Auto
if (initRotation)
{
if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0)
{
// Get the curretHeading
Rotation2d firstLoopHeading = swerve.getHeading();
// Set the Current Heading to the desired Heading
desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos());
}
//Dont Init Rotation Again
initRotation = false;
}
// Limit velocity to prevent tippy
Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),
Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS),
swerve.getSwerveDriveConfiguration());
SmartDashboard.putNumber("LimitedTranslation", translation.getX());
SmartDashboard.putString("Translation", translation.toString());
// Make the robot move
swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted)
{
}
// Returns true when the command should end.
@Override
public boolean isFinished()
{
return false;
}
}

154
src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java Normal file
View File

@@ -0,0 +1,154 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands.swervedrive.drivebase;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import swervelib.SwerveController;
import swervelib.math.SwerveMath;
/**
* A more advanced Swerve Control System that has 4 buttons for which direction to face
*/
public class AbsoluteDriveAdv extends Command
{
private final SwerveSubsystem swerve;
private final DoubleSupplier vX, vY;
private final DoubleSupplier headingAdjust;
private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight;
private boolean resetHeading = false;
/**
* Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is
* torwards/away from alliance wall and y is left/right. Heading Adjust changes the current heading after being
* multipied by a constant. The look booleans are shortcuts to get the robot to face a certian direction. Based off of
* ideas in https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172
*
* @param swerve The swerve drivebase subsystem.
* @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 to 1
* with deadband already accounted for. Positive X is away from the alliance wall.
* @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 to 1
* with deadband already accounted for. Positive Y is towards the left wall when looking through
* the driver station glass.
* @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle that should be
* adjusted. Should range from -1 to 1 with deadband already accounted for.
* @param lookAway Face the robot towards the opposing alliance's wall in the same direction the driver is
* facing
* @param lookTowards Face the robot towards the driver
* @param lookLeft Face the robot left
* @param lookRight Face the robot right
*/
public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust,
BooleanSupplier lookAway, BooleanSupplier lookTowards, BooleanSupplier lookLeft,
BooleanSupplier lookRight)
{
this.swerve = swerve;
this.vX = vX;
this.vY = vY;
this.headingAdjust = headingAdjust;
this.lookAway = lookAway;
this.lookTowards = lookTowards;
this.lookLeft = lookLeft;
this.lookRight = lookRight;
addRequirements(swerve);
}
@Override
public void initialize()
{
resetHeading = true;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute()
{
double headingX = 0;
double headingY = 0;
// These are written to allow combinations for 45 angles
// Face Away from Drivers
if (lookAway.getAsBoolean())
{
headingY = -1;
}
// Face Right
if (lookRight.getAsBoolean())
{
headingX = 1;
}
// Face Left
if (lookLeft.getAsBoolean())
{
headingX = -1;
}
// Face Towards the Drivers
if (lookTowards.getAsBoolean())
{
headingY = 1;
}
// Prevent Movement After Auto
if (resetHeading)
{
if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0)
{
// Get the curret Heading
Rotation2d currentHeading = swerve.getHeading();
// Set the Current Heading to the desired Heading
headingX = currentHeading.getSin();
headingY = currentHeading.getCos();
}
//Dont reset Heading Again
resetHeading = false;
}
ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY);
// Limit velocity to prevent tippy
Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),
Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS),
swerve.getSwerveDriveConfiguration());
SmartDashboard.putNumber("LimitedTranslation", translation.getX());
SmartDashboard.putString("Translation", translation.toString());
// Make the robot move
if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0)
{
resetHeading = true;
swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true);
} else
{
swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted)
{
}
// Returns true when the command should end.
@Override
public boolean isFinished()
{
return false;
}
}

95
src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java Normal file
View File

@@ -0,0 +1,95 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands.swervedrive.drivebase;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import java.util.List;
import java.util.function.DoubleSupplier;
import swervelib.SwerveController;
import swervelib.math.SwerveMath;
/**
* An example command that uses an example subsystem.
*/
public class AbsoluteFieldDrive extends Command
{
private final SwerveSubsystem swerve;
private final DoubleSupplier vX, vY, heading;
/**
* Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is
* torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian
* coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot
* will rotate to.
*
* @param swerve The swerve drivebase subsystem.
* @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 to 1 with
* deadband already accounted for. Positive X is away from the alliance wall.
* @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 to 1 with
* deadband already accounted for. Positive Y is towards the left wall when looking through the driver
* station glass.
* @param heading DoubleSupplier that supplies the robot's heading angle.
*/
public AbsoluteFieldDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY,
DoubleSupplier heading)
{
this.swerve = swerve;
this.vX = vX;
this.vY = vY;
this.heading = heading;
addRequirements(swerve);
}
@Override
public void initialize()
{
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute()
{
// Get the desired chassis speeds based on a 2 joystick module.
ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(),
new Rotation2d(heading.getAsDouble() * Math.PI));
// Limit velocity to prevent tippy
Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),
Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS),
swerve.getSwerveDriveConfiguration());
SmartDashboard.putNumber("LimitedTranslation", translation.getX());
SmartDashboard.putString("Translation", translation.toString());
// Make the robot move
swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted)
{
}
// Returns true when the command should end.
@Override
public boolean isFinished()
{
return false;
}
}

55
src/main/java/frc/robot/subsystems/ClimberSubsystem.java Normal file
View File

@@ -0,0 +1,55 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class ClimberSubsystem extends SubsystemBase{
private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT);
public void liftRobot() {
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED);
}
public void lowerRobot() {
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
}
public void stopClimber() {
climberMotor.set(0);
}
public Command liftRobotCommand() {
return runOnce(() -> toggleRatchet(true)).andThen(() -> liftRobot());
}
public Command lowerRobotCommand() {
return runOnce(() -> toggleRatchet(false)).andThen(() -> lowerRobot());
}
public Command stopClimberCommand() {
return runOnce(() -> stopClimber());
}
public static void toggleRatchet(boolean toggle) {
if (toggle == true) {
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
} else
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
}
public Command toggleRatchetCommand(boolean toggle) {
return runOnce(() -> toggleRatchet(toggle));
}
@Override
public void periodic()
{
SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition());
}
}

94
src/main/java/frc/robot/subsystems/IntakeSubsystem.java Normal file
View File

@@ -0,0 +1,94 @@
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.Constants;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkBase.ControlType;
public class IntakeSubsystem extends SubsystemBase {
private static SparkFlex intakeMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
MotorType.kBrushless);
private static SparkClosedLoopController intakeRotatorPIDController;
public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
public IntakeSubsystem() {
intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D);
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
}
public void startIntakeMotor() {
intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED);
}
public void reverseIntakeMotor() {
intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED * -1);
}
public void stopIntakeMotor() {
intakeMotor.set(0);
}
public Command startIntakeMotorCommand() {
return runOnce(() -> startIntakeMotor());
}
public Command reverseIntakeMotorCommand() {
return runOnce(() -> reverseIntakeMotor());
}
public Command stopIntakeMotorCommand() {
return runOnce(() -> stopIntakeMotor());
}
public void deployIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
ControlType.kPosition);
}
public Command deployintakeCommand() {
return runOnce(() -> deployIntake());
}
public void retractIntake() {
intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition);
}
public Command retractIntakeCommand() {
return runOnce(() -> retractIntake());
}
public void assistFuelIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
ControlType.kPosition);
}
public Command assistFuelIntakeCommand() {
return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
.andThen(new WaitCommand(2));
}
@Override
public void periodic() {
SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
}
}

124
src/main/java/frc/robot/subsystems/ShooterSubsystem.java Normal file
View File

@@ -0,0 +1,124 @@
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import java.util.function.BooleanSupplier;
import com.ctre.phoenix6.controls.Follower;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkBase;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkBaseConfig;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.LimelightHelpers;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
public class ShooterSubsystem extends SubsystemBase {
private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex indexerMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
MotorType.kBrushless);
//private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
// MotorType.kBrushless);
//private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
//MotorType.kBrushless);
//private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0);
//private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0);
private static double currentPotentiometerPosition; // might need second value for the right potentiometer
public void startShooterMotors() {
centerShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
leftShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
rightShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
}
public double getShooterMotorVelocity() {
return leftShooterMotor.getEncoder().getVelocity();
}
public void startIndexerMotor() {
// if (LimelightHelpers.getTX("limelight") < 1.5 &&
// LimelightHelpers.getTX("limelight") > -1.5) {
indexerMotor.set(Constants.ShooterConstants.INDEXER_MOTOR_SPEED);
// } else
// indexerMotor.set(0);
}
/* public Command shootFuelCommand() {
return run(() -> startShooterMotors())
.until(() -> {
return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY);
})
.andThen(() -> startIndexerMotor());
} */
public Command shootFuelCommand() {
return runOnce(() -> startShooterMotors()).andThen(new WaitCommand(2))
.andThen(() -> startIndexerMotor());
};
public void stopShooters() {
centerShooterMotor.set(0);
leftShooterMotor.set(0);
rightShooterMotor.set(0);
indexerMotor.set(0);
}
public Command stopShooterCommand() {
return runOnce(() -> stopShooters());
}
public void moveActuator(double desiredPotentiometerPosition) {
if (desiredPotentiometerPosition > currentPotentiometerPosition) {
//TODO: Test for positive or negative power
//leftActuatorMotor.set(0.1);
//rightActuatorMotor.set(0.1);
} else {
//leftActuatorMotor.set(-0.1);
//rightActuatorMotor.set(-0.1);
}
}
public void stopActuator() {
//leftActuatorMotor.set(0);
//rightActuatorMotor.set(0);
}
public Command moveActuatorCommand(double desiredPotentiometerPosition) {
return run(() -> moveActuator(desiredPotentiometerPosition))
.until(() -> currentPotentiometerPosition == currentPotentiometerPosition)
.andThen(() -> stopActuator());
}
@Override
public void periodic() {
/* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
currentPotentiometerPosition = leftPotentiometer.get(); */
}
}

182
src/main/java/frc/robot/subsystems/TargetingSubsystems.java Normal file
View File

@@ -0,0 +1,182 @@
package frc.robot.subsystems;
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonTrackedTarget;
import com.pathplanner.lib.path.GoalEndState;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.path.PathPoint;
import com.pathplanner.lib.path.RotationTarget;
import com.pathplanner.lib.path.Waypoint;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import frc.robot.Constants;
import frc.robot.LimelightHelpers;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc.robot.RobotContainer;
import frc.robot.Constants;
public class TargetingSubsystems extends SubsystemBase {
PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera");
Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
BACK_LEFT_CAMERA_OFFSETS);
PIDController photonAimPIDController = new PIDController(0.3, 0, 0.001);
public TargetingSubsystems() {
photonAimPIDController.enableContinuousInput(-180, 180);
}
Pose2d currentRobotPose;
public List<Waypoint> rightClimbWaypoints;
public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) {
GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
currentRobotPose = swerveDrive.getPose();
rightClimbWaypoints = PathPlannerPath.waypointsFromPoses(
currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE);
PathPlannerPath goToClimbPath = new PathPlannerPath(rightClimbWaypoints, goToClimbConstraints, null,
goalEndState);
goToClimbPath.preventFlipping = true;
return swerveDrive.getAutonomousCommand("goToClimbPath");
}
public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
return new RunCommand(() -> {
currentRobotPose = swerveDrive.getPose();
Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
double xError = errorFromDesiredPose.getX();
double yError = errorFromDesiredPose.getY();
double angleError = errorFromDesiredPose.getRotation().getRadians();
PIDController xController = new PIDController(1.5, 0, 0);
PIDController yController = new PIDController(1.5, 0, 0);
PIDController angleController = new PIDController(3.0, 0, 0);
angleController.enableContinuousInput(-Math.PI, Math.PI);
double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(),
desiredPose.getRotation().getRadians());
swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
}, swerveDrive);
}
Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
return new RunCommand(() -> {
double rot = 0.0;
var result = photonVision.getLatestResult();
if (result.hasTargets()) {
double yawError = result.getBestTarget().getYaw();
rot = photonAimPIDController.calculate(yawError, 0);
}
rot = MathUtil.clamp(rot, -3.0, 3.0);
swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
driverXbox.getLeftX() * -1), rot, true);
}, swerveDrive);
}
public PhotonPoseEstimator getPhotonPoseEstimator() {
return photonEstimator;
}
// static public NetworkTable table =
// NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
// static public NetworkTableEntry ty = table.getEntry("ty");
// static double targetOffsetAngle_Vertical = ty.getDouble(0.0);
// how many degrees back is your limelight rotated from perfectly vertical?
static double limelightMountAngleDegrees = 25.0;
// distance from the center of the Limelight lens to the floor
static double limelightLensHeightInches = 27.5;
// distance from the target to the floor
static double goalHeightInches = 44;
static double angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
// calculate distance
static double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
/ Math.tan(angleToGoalRadians);
public static double getDistanceFromAprilTag() {
angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
/ Math.tan(angleToGoalRadians);
return distanceFromLimelightToGoalInches;
}
public void updateRobotPose(SwerveSubsystem swerveDrive) {
Optional<EstimatedRobotPose> result = photonEstimator.update(photonVision.getLatestResult());
if (result.isPresent()) {
EstimatedRobotPose estimatedPose = result.get();
swerveDrive.getSwerveDrive()
.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
}
}
@Override
public void periodic() {
/*
* Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
* photonVision.getLatestResult().getBestTarget().getYaw());
* Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value",
* photonVision.getLatestResult().getBestTarget().getPitch());
* Shuffleboard.getTab("Vision").add("Limelight TX Value",
* LimelightHelpers.getTX("limelight"));
* Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
* LimelightHelpers.getFiducialID("limelight"));
* Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null);
* Shuffleboard.getTab("Vision").addCamera("Photon",
* "Arducam_OV9281_USB_Camera",
* "http://photonvision.local:5800");
*/
}
}

720
src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java Normal file
View File

@@ -0,0 +1,720 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems.swervedrive;
import static edu.wpi.first.units.Units.Meter;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.commands.PathfindingCommand;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.DriveFeedforwards;
import com.pathplanner.lib.util.swerve.SwerveSetpoint;
import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import frc.robot.Constants;
import frc.robot.subsystems.swervedrive.Vision.Cameras;
import java.io.File;
import java.io.IOException;
import java.util.Arrays;
import java.util.Optional;
import java.util.concurrent.atomic.AtomicReference;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import org.json.simple.parser.ParseException;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.targeting.PhotonPipelineResult;
import swervelib.SwerveController;
import swervelib.SwerveDrive;
import swervelib.SwerveDriveTest;
import swervelib.math.SwerveMath;
import swervelib.parser.SwerveControllerConfiguration;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.parser.SwerveParser;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
public class SwerveSubsystem extends SubsystemBase {
/**
* Swerve drive object.
*/
private final SwerveDrive swerveDrive;
/**
* Enable vision odometry updates while driving.
*/
private final boolean visionDriveTest = false;
/**
* PhotonVision class to keep an accurate odometry.
*/
private Vision vision;
/**
* Initialize {@link SwerveDrive} with the directory provided.
*
* @param directory Directory of swerve drive config files.
*/
public SwerveSubsystem(File directory) {
boolean blueAlliance = false;
Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1),
Meter.of(4)),
Rotation2d.fromDegrees(0))
: new Pose2d(new Translation2d(Meter.of(16),
Meter.of(4)),
Rotation2d.fromDegrees(180));
// Configure the Telemetry before creating the SwerveDrive to avoid unnecessary
// objects being created.
SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
try {
swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose);
// Alternative method if you don't want to supply the conversion factor via JSON
// files.
// swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed,
// angleConversionFactor, driveConversionFactor);
} catch (Exception e) {
throw new RuntimeException(e);
}
swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot
// via
// angle.
swerveDrive.setCosineCompensator(false);// !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation
// for
// simulations since it causes discrepancies not seen in real life.
swerveDrive.setAngularVelocityCompensation(true,
true,
0.1); // Correct for skew that gets worse as angular velocity increases. Start with a
// coefficient of 0.1.
swerveDrive.setModuleEncoderAutoSynchronize(false,
1); // Enable if you want to resynchronize your absolute encoders and motor encoders
// periodically when they are not moving.
// swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used
// over the internal encoder and push the offsets onto it. Throws warning if not
// possible
if (visionDriveTest) {
setupPhotonVision();
// Stop the odometry thread if we are using vision that way we can synchronize
// updates better.
swerveDrive.stopOdometryThread();
}
setupPathPlanner();
}
/**
* Construct the swerve drive.
*
* @param driveCfg SwerveDriveConfiguration for the swerve.
* @param controllerCfg Swerve Controller.
*/
public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) {
swerveDrive = new SwerveDrive(driveCfg,
controllerCfg,
Constants.MAX_SPEED,
new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)),
Rotation2d.fromDegrees(0)));
}
/**
* Setup the photon vision class.
*/
public void setupPhotonVision() {
vision = new Vision(swerveDrive::getPose, swerveDrive.field);
}
@Override
public void periodic() {
// When vision is enabled we must manually update odometry in SwerveDrive
if (visionDriveTest) {
swerveDrive.updateOdometry();
vision.updatePoseEstimation(swerveDrive);
}
}
@Override
public void simulationPeriodic() {
}
/**
* Setup AutoBuilder for PathPlanner.
*/
public void setupPathPlanner() {
// Load the RobotConfig from the GUI settings. You should probably
// store this in your Constants file
RobotConfig config;
try {
config = RobotConfig.fromGUISettings();
final boolean enableFeedforward = true;
// Configure AutoBuilder last
AutoBuilder.configure(
this::getPose,
// Robot pose supplier
this::resetOdometry,
// Method to reset odometry (will be called if your auto has a starting pose)
this::getRobotVelocity,
// ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speedsRobotRelative, moduleFeedForwards) -> {
if (enableFeedforward) {
swerveDrive.drive(
speedsRobotRelative,
swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative),
moduleFeedForwards.linearForces());
} else {
swerveDrive.setChassisSpeeds(speedsRobotRelative);
}
},
// Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also
// optionally outputs individual module feedforwards
new PPHolonomicDriveController(
// PPHolonomicController is the built in path following controller for holonomic
// drive trains
new PIDConstants(5.0, 0.0, 0.0),
// Translation PID constants
new PIDConstants(3.8, 0.0, 0.0)
// Rotation PID constants
),
config,
// The robot configuration
() -> {
// Boolean supplier that controls when the path will be mirrored for the red
// alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this
// Reference to this subsystem to set requirements
);
} catch (Exception e) {
// Handle exception as needed
e.printStackTrace();
}
// Preload PathPlanner Path finding
// IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE
PathfindingCommand.warmupCommand().schedule();
}
/**
* Aim the robot at the target returned by PhotonVision.
*
* @return A {@link Command} which will run the alignment.
*/
public Command aimAtTarget(Cameras camera) {
return run(() -> {
Optional<PhotonPipelineResult> resultO = camera.getBestResult();
if (resultO.isPresent()) {
var result = resultO.get();
if (result.hasTargets()) {
drive(getTargetSpeeds(0,
0,
Rotation2d.fromDegrees(result.getBestTarget()
.getYaw()))); // Not sure if this will work, more math may be required.
}
}
});
}
/**
* Get the path follower with events.
*
* @param pathName PathPlanner path name.
* @return {@link AutoBuilder#followPath(PathPlannerPath)} path command.
*/
public Command getAutonomousCommand(String pathName) {
// Create a path following command using AutoBuilder. This will also trigger
// event markers.
return new PathPlannerAuto(pathName);
}
/**
* Use PathPlanner Path finding to go to a point on the field.
*
* @param pose Target {@link Pose2d} to go to.
* @return PathFinding command
*/
public Command driveToPose(Pose2d pose) {
// Create the constraints to use while pathfinding
PathConstraints constraints = new PathConstraints(
swerveDrive.getMaximumChassisVelocity(), 4.0,
swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720));
// Since AutoBuilder is configured, we can use it to build pathfinding commands
return AutoBuilder.pathfindToPose(
pose,
constraints,
edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
);
}
/**
* Drive with {@link SwerveSetpointGenerator} from 254, implemented by
* PathPlanner.
*
* @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to
* achieve.
* @return {@link Command} to run.
* @throws IOException If the PathPlanner GUI settings is invalid
* @throws ParseException If PathPlanner GUI settings is nonexistent.
*/
private Command driveWithSetpointGenerator(Supplier<ChassisSpeeds> robotRelativeChassisSpeed)
throws IOException, ParseException {
SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(),
swerveDrive.getMaximumChassisAngularVelocity());
AtomicReference<SwerveSetpoint> prevSetpoint = new AtomicReference<>(
new SwerveSetpoint(swerveDrive.getRobotVelocity(),
swerveDrive.getStates(),
DriveFeedforwards.zeros(swerveDrive.getModules().length)));
AtomicReference<Double> previousTime = new AtomicReference<>();
return startRun(() -> previousTime.set(Timer.getFPGATimestamp()),
() -> {
double newTime = Timer.getFPGATimestamp();
SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint(prevSetpoint.get(),
robotRelativeChassisSpeed.get(),
newTime - previousTime.get());
swerveDrive.drive(newSetpoint.robotRelativeSpeeds(),
newSetpoint.moduleStates(),
newSetpoint.feedforwards().linearForces());
prevSetpoint.set(newSetpoint);
previousTime.set(newTime);
});
}
/**
* Drive with 254's Setpoint generator; port written by PathPlanner.
*
* @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds}
* @return Command to drive the robot using the setpoint generator.
*/
public Command driveWithSetpointGeneratorFieldRelative(Supplier<ChassisSpeeds> fieldRelativeSpeeds) {
try {
return driveWithSetpointGenerator(() -> {
return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading());
});
} catch (Exception e) {
DriverStation.reportError(e.toString(), true);
}
return Commands.none();
}
/**
* Command to characterize the robot drive motors using SysId
*
* @return SysId Drive Command
*/
public Command sysIdDriveMotorCommand() {
return SwerveDriveTest.generateSysIdCommand(
SwerveDriveTest.setDriveSysIdRoutine(
new Config(),
this, swerveDrive, 12, true),
3.0, 5.0, 3.0);
}
/**
* Command to characterize the robot angle motors using SysId
*
* @return SysId Angle Command
*/
public Command sysIdAngleMotorCommand() {
return SwerveDriveTest.generateSysIdCommand(
SwerveDriveTest.setAngleSysIdRoutine(
new Config(),
this, swerveDrive),
3.0, 5.0, 3.0);
}
/**
* Returns a Command that centers the modules of the SwerveDrive subsystem.
*
* @return a Command that centers the modules of the SwerveDrive subsystem
*/
public Command centerModulesCommand() {
return run(() -> Arrays.asList(swerveDrive.getModules())
.forEach(it -> it.setAngle(0.0)));
}
/**
* Returns a Command that tells the robot to drive forward until the command
* ends.
*
* @return a Command that tells the robot to drive forward until the command
* ends
*/
public Command driveForward() {
return run(() -> {
swerveDrive.drive(new Translation2d(1, 0), 0, false, false);
}).finallyDo(() -> swerveDrive.drive(new Translation2d(0, 0), 0, false, false));
}
/**
* Replaces the swerve module feedforward with a new SimpleMotorFeedforward
* object.
*
* @param kS the static gain of the feedforward
* @param kV the velocity gain of the feedforward
* @param kA the acceleration gain of the feedforward
*/
public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) {
swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA));
}
/**
* Command to drive the robot using translative values and heading as angular
* velocity.
*
* @param translationX Translation in the X direction. Cubed for smoother
* controls.
* @param translationY Translation in the Y direction. Cubed for smoother
* controls.
* @param angularRotationX Angular velocity of the robot to set. Cubed for
* smoother controls.
* @return Drive command.
*/
public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY,
DoubleSupplier angularRotationX) {
return run(() -> {
// Make the robot move
swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d(
translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(),
translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), 0.8),
Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(),
true,
false);
});
}
/**
* Command to drive the robot using translative values and heading as a
* setpoint.
*
* @param translationX Translation in the X direction. Cubed for smoother
* controls.
* @param translationY Translation in the Y direction. Cubed for smoother
* controls.
* @param headingX Heading X to calculate angle of the joystick.
* @param headingY Heading Y to calculate angle of the joystick.
* @return Drive command.
*/
public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX,
DoubleSupplier headingY) {
// swerveDrive.setHeadingCorrection(true); // Normally you would want heading
// correction for this kind of control.
return run(() -> {
Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(),
translationY.getAsDouble()), 0.8);
// Make the robot move
driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(),
headingX.getAsDouble(),
headingY.getAsDouble(),
swerveDrive.getOdometryHeading().getRadians(),
swerveDrive.getMaximumChassisVelocity()));
});
}
/**
* The primary method for controlling the drivebase. Takes a
* {@link Translation2d} and a rotation rate, and
* calculates and commands module states accordingly. Can use either open-loop
* or closed-loop velocity control for
* the wheel velocities. Also has field- and robot-relative modes, which affect
* how the translation vector is used.
*
* @param translation {@link Translation2d} that is the commanded linear
* velocity of the robot, in meters per
* second. In robot-relative mode, positive x is torwards
* the bow (front) and positive y is
* torwards port (left). In field-relative mode, positive x
* is away from the alliance wall
* (field North) and positive y is torwards the left wall
* when looking through the driver station
* glass (field West).
* @param rotation Robot angular rate, in radians per second. CCW positive.
* Unaffected by field/robot
* relativity.
* @param fieldRelative Drive mode. True for field-relative, false for
* robot-relative.
*/
public void drive(Translation2d translation, double rotation, boolean fieldRelative) {
swerveDrive.drive(translation,
rotation,
fieldRelative,
false); // Open loop is disabled since it shouldn't be used most of the time.
}
/**
* Drive the robot given a chassis field oriented velocity.
*
* @param velocity Velocity according to the field.
*/
public void driveFieldOriented(ChassisSpeeds velocity) {
swerveDrive.driveFieldOriented(velocity);
}
/**
* Drive the robot given a chassis field oriented velocity.
*
* @param velocity Velocity according to the field.
*/
public Command driveFieldOriented(Supplier<ChassisSpeeds> velocity) {
return run(() -> {
swerveDrive.driveFieldOriented(velocity.get());
});
}
/**
* Drive according to the chassis robot oriented velocity.
*
* @param velocity Robot oriented {@link ChassisSpeeds}
*/
public void drive(ChassisSpeeds velocity) {
swerveDrive.drive(velocity);
}
/**
* Get the swerve drive kinematics object.
*
* @return {@link SwerveDriveKinematics} of the swerve drive.
*/
public SwerveDriveKinematics getKinematics() {
return swerveDrive.kinematics;
}
/**
* Resets odometry to the given pose. Gyro angle and module positions do not
* need to be reset when calling this
* method. However, if either gyro angle or module position is reset, this must
* be called in order for odometry to
* keep working.
*
* @param initialHolonomicPose The pose to set the odometry to
*/
public void resetOdometry(Pose2d initialHolonomicPose) {
swerveDrive.resetOdometry(initialHolonomicPose);
}
/**
* Gets the current pose (position and rotation) of the robot, as reported by
* odometry.
*
* @return The robot's pose
*/
public Pose2d getPose() {
return swerveDrive.getPose();
}
/**
* Set chassis speeds with closed-loop velocity control.
*
* @param chassisSpeeds Chassis Speeds to set.
*/
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
swerveDrive.setChassisSpeeds(chassisSpeeds);
}
/**
* Post the trajectory to the field.
*
* @param trajectory The trajectory to post.
*/
public void postTrajectory(Trajectory trajectory) {
swerveDrive.postTrajectory(trajectory);
}
/**
* Resets the gyro angle to zero and resets odometry to the same position, but
* facing toward 0.
*/
public void zeroGyro() {
swerveDrive.zeroGyro();
}
/**
* Checks if the alliance is red, defaults to false if alliance isn't available.
*
* @return true if the red alliance, false if blue. Defaults to false if none is
* available.
*/
private boolean isRedAlliance() {
var alliance = DriverStation.getAlliance();
return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
}
/**
* This will zero (calibrate) the robot to assume the current position is facing
* forward
* <p>
* If red alliance rotate the robot 180 after the drviebase zero command
*/
public void zeroGyroWithAlliance() {
if (isRedAlliance()) {
zeroGyro();
// Set the pose 180 degrees
resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180)));
} else {
zeroGyro();
}
}
/**
* Sets the drive motors to brake/coast mode.
*
* @param brake True to set motors to brake mode, false for coast.
*/
public void setMotorBrake(boolean brake) {
swerveDrive.setMotorIdleMode(brake);
}
/**
* Gets the current yaw angle of the robot, as reported by the swerve pose
* estimator in the underlying drivebase.
* Note, this is not the raw gyro reading, this may be corrected from calls to
* resetOdometry().
*
* @return The yaw angle
*/
public Rotation2d getHeading() {
return getPose().getRotation();
}
/**
* Get the chassis speeds based on controller input of 2 joysticks. One for
* speeds in which direction. The other for
* the angle of the robot.
*
* @param xInput X joystick input for the robot to move in the X direction.
* @param yInput Y joystick input for the robot to move in the Y direction.
* @param headingX X joystick which controls the angle of the robot.
* @param headingY Y joystick which controls the angle of the robot.
* @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
*/
public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) {
Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
scaledInputs.getY(),
headingX,
headingY,
getHeading().getRadians(),
Constants.MAX_SPEED);
}
/**
* Get the chassis speeds based on controller input of 1 joystick and one angle.
* Control the robot at an offset of
* 90deg.
*
* @param xInput X joystick input for the robot to move in the X direction.
* @param yInput Y joystick input for the robot to move in the Y direction.
* @param angle The angle in as a {@link Rotation2d}.
* @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
*/
public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) {
Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(),
scaledInputs.getY(),
angle.getRadians(),
getHeading().getRadians(),
Constants.MAX_SPEED);
}
/**
* Gets the current field-relative velocity (x, y and omega) of the robot
*
* @return A ChassisSpeeds object of the current field-relative velocity
*/
public ChassisSpeeds getFieldVelocity() {
return swerveDrive.getFieldVelocity();
}
/**
* Gets the current velocity (x, y and omega) of the robot
*
* @return A {@link ChassisSpeeds} object of the current velocity
*/
public ChassisSpeeds getRobotVelocity() {
return swerveDrive.getRobotVelocity();
}
/**
* Get the {@link SwerveController} in the swerve drive.
*
* @return {@link SwerveController} from the {@link SwerveDrive}.
*/
public SwerveController getSwerveController() {
return swerveDrive.swerveController;
}
/**
* Get the {@link SwerveDriveConfiguration} object.
*
* @return The {@link SwerveDriveConfiguration} fpr the current drive.
*/
public SwerveDriveConfiguration getSwerveDriveConfiguration() {
return swerveDrive.swerveDriveConfiguration;
}
/**
* Lock the swerve drive to prevent it from moving.
*/
public void lock() {
swerveDrive.lockPose();
}
/**
* Gets the current pitch angle of the robot, as reported by the imu.
*
* @return The heading as a {@link Rotation2d} angle
*/
public Rotation2d getPitch() {
return swerveDrive.getPitch();
}
/**
* Add a fake vision reading for testing purposes.
*/
public void addFakeVisionReading() {
swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp());
}
/**
* Gets the swerve drive object.
*
* @return {@link SwerveDrive}
*/
public SwerveDrive getSwerveDrive() {
return swerveDrive;
}
}

629
src/main/java/frc/robot/subsystems/swervedrive/Vision.java Normal file
View File

@@ -0,0 +1,629 @@
package frc.robot.subsystems.swervedrive;
import static edu.wpi.first.units.Units.Microseconds;
import static edu.wpi.first.units.Units.Seconds;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import frc.robot.Robot;
import java.awt.Desktop;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import java.util.function.Supplier;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.PhotonUtils;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import swervelib.SwerveDrive;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from
* https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads
*/
public class Vision
{
/**
* April Tag Field Layout of the year.
*/
public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(
AprilTagFields.k2026RebuiltAndymark);
/**
* Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
*/
private final double maximumAmbiguity = 0.25;
/**
* Photon Vision Simulation
*/
public VisionSystemSim visionSim;
/**
* Count of times that the odom thinks we're more than 10meters away from the april tag.
*/
private double longDistangePoseEstimationCount = 0;
/**
* Current pose from the pose estimator using wheel odometry.
*/
private Supplier<Pose2d> currentPose;
/**
* Field from {@link swervelib.SwerveDrive#field}
*/
private Field2d field2d;
/**
* Constructor for the Vision class.
*
* @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()}
* @param field Current field, should be {@link SwerveDrive#field}
*/
public Vision(Supplier<Pose2d> currentPose, Field2d field)
{
this.currentPose = currentPose;
this.field2d = field;
if (Robot.isSimulation())
{
visionSim = new VisionSystemSim("Vision");
visionSim.addAprilTags(fieldLayout);
for (Cameras c : Cameras.values())
{
c.addToVisionSim(visionSim);
}
openSimCameraViews();
}
}
/**
* Calculates a target pose relative to an AprilTag on the field.
*
* @param aprilTag The ID of the AprilTag.
* @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position
* itself correctly.
* @return The target pose of the AprilTag.
*/
public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset)
{
Optional<Pose3d> aprilTagPose3d = fieldLayout.getTagPose(aprilTag);
if (aprilTagPose3d.isPresent())
{
return aprilTagPose3d.get().toPose2d().transformBy(robotOffset);
} else
{
throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString());
}
}
/**
* Update the pose estimation inside of {@link SwerveDrive} with all of the given poses.
*
* @param swerveDrive {@link SwerveDrive} instance.
*/
public void updatePoseEstimation(SwerveDrive swerveDrive)
{
if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent())
{
/*
* In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting.
* As a result, the odometry may not always be 100% accurate.
* However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect.
* (This is why teams implement vision system to correct odometry.)
* Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation.
*/
visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
}
for (Cameras camera : Cameras.values())
{
Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
if (poseEst.isPresent())
{
var pose = poseEst.get();
swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(),
pose.timestampSeconds,
camera.curStdDevs);
}
}
}
/**
* Generates the estimated robot pose. Returns empty if:
* <ul>
* <li> No Pose Estimates could be generated</li>
* <li> The generated pose estimate was considered not accurate</li>
* </ul>
*
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera)
{
Optional<EstimatedRobotPose> poseEst = camera.getEstimatedGlobalPose();
if (Robot.isSimulation())
{
Field2d debugField = visionSim.getDebugField();
// Uncomment to enable outputting of vision targets in sim.
poseEst.ifPresentOrElse(
est ->
debugField
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
() -> {
debugField.getObject("VisionEstimation").setPoses();
});
}
return poseEst;
}
/**
* Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than
* 10m for a short amount of time.
*
* @param pose Estimated robot pose.
* @return Could be empty if there isn't a good reading.
*/
@Deprecated(since = "2024", forRemoval = true)
private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose)
{
if (pose.isPresent())
{
double bestTargetAmbiguity = 1; // 1 is max ambiguity
for (PhotonTrackedTarget target : pose.get().targetsUsed)
{
double ambiguity = target.getPoseAmbiguity();
if (ambiguity != -1 && ambiguity < bestTargetAmbiguity)
{
bestTargetAmbiguity = ambiguity;
}
}
//ambiguity to high dont use estimate
if (bestTargetAmbiguity > maximumAmbiguity)
{
return Optional.empty();
}
//est pose is very far from recorded robot pose
if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1)
{
longDistangePoseEstimationCount++;
//if it calculates that were 10 meter away for more than 10 times in a row its probably right
if (longDistangePoseEstimationCount < 10)
{
return Optional.empty();
}
} else
{
longDistangePoseEstimationCount = 0;
}
return pose;
}
return Optional.empty();
}
/**
* Get distance of the robot from the AprilTag pose.
*
* @param id AprilTag ID
* @return Distance
*/
public double getDistanceFromAprilTag(int id)
{
Optional<Pose3d> tag = fieldLayout.getTagPose(id);
return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0);
}
/**
* Get tracked target from a camera of AprilTagID
*
* @param id AprilTag ID
* @param camera Camera to check.
* @return Tracked target.
*/
public PhotonTrackedTarget getTargetFromId(int id, Cameras camera)
{
PhotonTrackedTarget target = null;
for (PhotonPipelineResult result : camera.resultsList)
{
if (result.hasTargets())
{
for (PhotonTrackedTarget i : result.getTargets())
{
if (i.getFiducialId() == id)
{
return i;
}
}
}
}
return target;
}
/**
* Vision simulation.
*
* @return Vision Simulation
*/
public VisionSystemSim getVisionSim()
{
return visionSim;
}
/**
* Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost.
*/
private void openSimCameraViews()
{
if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE))
{
// try
// {
// Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
// Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
// Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
// } catch (IOException | URISyntaxException e)
// {
// e.printStackTrace();
// }
}
}
/**
* Update the {@link Field2d} to include tracked targets/
*/
public void updateVisionField()
{
List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>();
for (Cameras c : Cameras.values())
{
if (!c.resultsList.isEmpty())
{
PhotonPipelineResult latest = c.resultsList.get(0);
if (latest.hasTargets())
{
targets.addAll(latest.targets);
}
}
}
List<Pose2d> poses = new ArrayList<>();
for (PhotonTrackedTarget target : targets)
{
if (fieldLayout.getTagPose(target.getFiducialId()).isPresent())
{
Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d();
poses.add(targetPose);
}
}
field2d.getObject("tracked targets").setPoses(poses);
}
/**
* Camera Enum to select each camera
*/
enum Cameras
{
/**
* Left Camera
*/
LEFT_CAM("left",
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
new Translation3d(Units.inchesToMeters(12.056),
Units.inchesToMeters(10.981),
Units.inchesToMeters(8.44)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
/**
* Right Camera
*/
RIGHT_CAM("right",
new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
new Translation3d(Units.inchesToMeters(12.056),
Units.inchesToMeters(-10.981),
Units.inchesToMeters(8.44)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
/**
* Center Camera
*/
CENTER_CAM("center",
new Rotation3d(0, Units.degreesToRadians(18), 0),
new Translation3d(Units.inchesToMeters(-4.628),
Units.inchesToMeters(-10.687),
Units.inchesToMeters(16.129)),
VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
/**
* Latency alert to use when high latency is detected.
*/
public final Alert latencyAlert;
/**
* Camera instance for comms.
*/
public final PhotonCamera camera;
/**
* Pose estimator for camera.
*/
public final PhotonPoseEstimator poseEstimator;
/**
* Standard Deviation for single tag readings for pose estimation.
*/
private final Matrix<N3, N1> singleTagStdDevs;
/**
* Standard deviation for multi-tag readings for pose estimation.
*/
private final Matrix<N3, N1> multiTagStdDevs;
/**
* Transform of the camera rotation and translation relative to the center of the robot
*/
private final Transform3d robotToCamTransform;
/**
* Current standard deviations used.
*/
public Matrix<N3, N1> curStdDevs;
/**
* Estimated robot pose.
*/
public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
/**
* Simulated camera instance which only exists during simulations.
*/
public PhotonCameraSim cameraSim;
/**
* Results list to be updated periodically and cached to avoid unnecessary queries.
*/
public List<PhotonPipelineResult> resultsList = new ArrayList<>();
/**
* Last read from the camera timestamp to prevent lag due to slow data fetches.
*/
private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
/**
* Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine
* estimation noise on an actual robot.
*
* @param name Name of the PhotonVision camera found in the PV UI.
* @param robotToCamRotation {@link Rotation3d} of the camera.
* @param robotToCamTranslation {@link Translation3d} relative to the center of the robot.
* @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera.
* @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera.
*/
Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation,
Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix)
{
latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning);
camera = new PhotonCamera(name);
// https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html
robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation);
poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
robotToCamTransform);
poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
this.singleTagStdDevs = singleTagStdDevs;
this.multiTagStdDevs = multiTagStdDevsMatrix;
if (Robot.isSimulation())
{
SimCameraProperties cameraProp = new SimCameraProperties();
// A 640 x 480 camera with a 100 degree diagonal FOV.
cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100));
// Approximate detection noise with average and standard deviation error in pixels.
cameraProp.setCalibError(0.25, 0.08);
// Set the camera image capture framerate (Note: this is limited by robot loop rate).
cameraProp.setFPS(30);
// The average and standard deviation in milliseconds of image data latency.
cameraProp.setAvgLatencyMs(35);
cameraProp.setLatencyStdDevMs(5);
cameraSim = new PhotonCameraSim(camera, cameraProp);
cameraSim.enableDrawWireframe(true);
}
}
/**
* Add camera to {@link VisionSystemSim} for simulated photon vision.
*
* @param systemSim {@link VisionSystemSim} to use.
*/
public void addToVisionSim(VisionSystemSim systemSim)
{
if (Robot.isSimulation())
{
systemSim.addCamera(cameraSim, robotToCamTransform);
}
}
/**
* Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most
* recent result!
*
* @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result!
*/
public Optional<PhotonPipelineResult> getBestResult()
{
if (resultsList.isEmpty())
{
return Optional.empty();
}
PhotonPipelineResult bestResult = resultsList.get(0);
double amiguity = bestResult.getBestTarget().getPoseAmbiguity();
double currentAmbiguity = 0;
for (PhotonPipelineResult result : resultsList)
{
currentAmbiguity = result.getBestTarget().getPoseAmbiguity();
if (currentAmbiguity < amiguity && currentAmbiguity > 0)
{
bestResult = result;
amiguity = currentAmbiguity;
}
}
return Optional.of(bestResult);
}
/**
* Get the latest result from the current cache.
*
* @return Empty optional if nothing is found. Latest result if something is there.
*/
public Optional<PhotonPipelineResult> getLatestResult()
{
return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0));
}
/**
* Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the
* cache of results.
*
* @return Estimated pose.
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose()
{
updateUnreadResults();
return estimatedRobotPose;
}
/**
* Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp.
*/
private void updateUnreadResults()
{
double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds();
for (PhotonPipelineResult result : resultsList)
{
mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
}
resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
});
if (!resultsList.isEmpty())
{
updateEstimatedGlobalPose();
}
}
/**
* The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once
* per loop.
*
* <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
* {@link Cameras#updateEstimationStdDevs}
*
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for
* estimation.
*/
private void updateEstimatedGlobalPose()
{
Optional<EstimatedRobotPose> visionEst = Optional.empty();
for (var change : resultsList)
{
visionEst = poseEstimator.update(change);
updateEstimationStdDevs(visionEst, change.getTargets());
}
estimatedRobotPose = visionEst;
}
/**
* Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based
* on number of tags, estimation strategy, and distance from the tags.
*
* @param estimatedPose The estimated pose to guess standard deviations for.
* @param targets All targets in this camera frame
*/
private void updateEstimationStdDevs(
Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets)
{
if (estimatedPose.isEmpty())
{
// No pose input. Default to single-tag std devs
curStdDevs = singleTagStdDevs;
} else
{
// Pose present. Start running Heuristic
var estStdDevs = singleTagStdDevs;
int numTags = 0;
double avgDist = 0;
// Precalculation - see how many tags we found, and calculate an average-distance metric
for (var tgt : targets)
{
var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
if (tagPose.isEmpty())
{
continue;
}
numTags++;
avgDist +=
tagPose
.get()
.toPose2d()
.getTranslation()
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
}
if (numTags == 0)
{
// No tags visible. Default to single-tag std devs
curStdDevs = singleTagStdDevs;
} else
{
// One or more tags visible, run the full heuristic.
avgDist /= numTags;
// Decrease std devs if multiple targets are visible
if (numTags > 1)
{
estStdDevs = multiTagStdDevs;
}
// Increase std devs based on (average) distance
if (numTags == 1 && avgDist > 4)
{
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
} else
{
estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
}
curStdDevs = estStdDevs;
}
}
}
}
}