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

Delete unnecessary files. Clean up code. Change over to BangBangController + Feedforward for Velocity Control. Change over to PIDController + Feedforward for Position Control.

Browse Source
This commit is contained in:
Podcakero
2026-02-25 10:28:58 -05:00
parent 3d7601387a
commit 40f8c4e08e
11 changed files with 1050 additions and 3181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
gradlew vendored Normal file → Executable file
View File

120
src/main/java/frc/robot/Constants.java
View File

@@ -4,17 +4,13 @@
package frc.robot;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
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.Translation3d;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
@@ -41,7 +37,7 @@ public final class Constants {
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);
public static final double MAX_SPEED_MPS = Units.feetToMeters(14.5);
// Maximum speed of the robot in meters per second, used to limit acceleration.
// public static final class AutonConstants
@@ -81,14 +77,25 @@ public final class Constants {
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 final int SHOOTER_MOTOR_CURRENT_LIMIT = 80;
public static double SHOOTER_MOTOR_P = 0.0018;
public static double SHOOTER_MOTOR_I = 0;
public static double SHOOTER_MOTOR_D = 0;
public static final double SHOOTER_MOTOR_P = 0.0018;
public static final double SHOOTER_MOTOR_I = 0;
public static final double SHOOTER_MOTOR_D = 0;
public static double INDEXER_MOTOR_P = 0.0001;
public static double INDEXER_MOTOR_I = 0;
public static double INDEXER_MOTOR_D = 0;
public static final double INDEXER_MOTOR_P = 0.0001;
public static final double INDEXER_MOTOR_I = 0;
public static final double INDEXER_MOTOR_D = 0;
public static final double CENTER_MOTOR_S = 0.0;
public static final double CENTER_MOTOR_V = 0.0;
public static final double LEFT_MOTOR_S = 0.0;
public static final double LEFT_MOTOR_V = 0.0;
public static final double RIGHT_MOTOR_S = 0.0;
public static final double RIGHT_MOTOR_V = 0.0;
/*private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer RPM", 2000)
@@ -115,20 +122,37 @@ public final class Constants {
}*/
public static final int INTAKE_WHEELS_MOTOR_ID = 50;
public static final int INTAKE_WHEELS_CURRENT_LIMIT = 60;
public static final double INTAKE_WHEELS_POSITION_CONVERSION_FACTOR = 2 * Math.PI; // Encoder Unit * Conversion Factor = Radians. 1 Rotation = 2PI Radians
public static final double INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR = INTAKE_WHEELS_POSITION_CONVERSION_FACTOR * 60.0; // Encoder Units per Minute * Conversion Factor = Radians per Second
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
public static final int INTAKE_ROTATOR_CURRENT_LIMIT = 40;
public static final double INTAKE_ROTATOR_INITIAL_ENCODER_VALUE = Units.degreesToRadians(-90);
public static final double INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR = (2 * Math.PI) / 12.0; // Encoder Unit * Conversion Factor = Radians. 1 Rotation = 2PI Radians. Gear ratio is 12:1, so 12 Rotations = 1 full intake Rotation.
public static final double INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR = INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR * 60.0; // Encoder Units per Minute * Conversion Factor = Radians per Second
public static class IntakeRotatorPID {
public static final double INTAKE_ROTATOR_P = 0.03;
public static final double INTAKE_ROTATOR_I = 0;
public static final double INTAKE_ROTATOR_D = 0;
}
public static final double INTAKE_MOTOR_P = 0.0001;
public static final double INTAKE_MOTOR_I = 0;
public static final double INTAKE_MOTOR_D = 0;
public static final double INTAKE_WHEELS_MOTOR_P = 0.0001; // Radians -> Radians Per Second
public static final double INTAKE_WHEELS_MOTOR_I = 0.0; // Radians -> Radians Per Second
public static final double INTAKE_WHEELS_MOTOR_D = 0.0; // Radians -> Radians Per Second
public static final double INTAKE_WHEELS_MOTOR_S = 0.0; // Voltage to overcome static friction/inertia
public static final double INTAKE_WHEELS_MOTOR_V = 0.0; // Voltage per Rads/Second gain
public static final double INTAKE_COLLECT_ENCODER_VALUE = 5;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5;
public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
public static final double INTAKE_ROTATOR_DOWN_P = 0.03; // Radians -> Radians Per Second
public static final double INTAKE_ROTATOR_DOWN_I = 0.0; // Radians -> Radians Per Second
public static final double INTAKE_ROTATOR_DOWN_D = 0.0; // Radians -> Radians Per Second
public static final double INTAKE_ROTATOR_UP_P = 0.06; // Radians -> Radians Per Second
public static final double INTAKE_ROTATOR_UP_I = 0.0; // Radians -> Radians Per Second
public static final double INTAKE_ROTATOR_UP_D = 0.0; // Radians -> Radians Per Second
public static final double INTAKE_ROTATOR_MOTOR_S = 0.0; // Voltage to overcome static friction/inertia. V
public static final double INTAKE_ROTATOR_MOTOR_G = 0.0; // Voltage to overcome Gravity. V
public static final double INTAKE_ROTATOR_MOTOR_V = 0.0; // Voltage per Radians/Second. V/(Rad/s)
public static final double INTAKE_COLLECT_POSITION_RADS = 0.0;
public static final double INTAKE_MIDDLE_POSITION_RADS = -Units.degreesToRadians(45);
public static final double INTAKE_RETRACT_POSITION_RADS = -Units.degreesToRadians(90);
}
@@ -138,45 +162,31 @@ public final class Constants {
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 final Pose2d RIGHT_CLIMB_POSE_METERS = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
public static final Pose2d LEFT_CLIMB_POSE_METERS = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Rear Left Camera");
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Rear Right Camera");
public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera");
public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera");
public static final Vector<N3> SINGLE_TAG_STD_DEVS = VecBuilder.fill(4, 4, 8);
public static final Vector<N3> MULTI_TAG_STD_DEVS = VecBuilder.fill(0.5, 0.5, 1);
public static final Pose2d HUB_POSE = new Pose2d(4.625, 4.03, new Rotation2d());
public static final Pose2d HUB_POSE_METERS = new Pose2d(4.625, 4.03, new Rotation2d());
public static final Transform3d ORANGE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d BLACK_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d RED_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Transform3d PURPLE_ROBOT_TO_CAM = new Transform3d(new Translation3d(0, 0, 0),
new Rotation3d(0, 0, 0));
public static final Translation3d FRONT_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
public static final Translation3d FRONT_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
public static final Translation3d REAR_LEFT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
public static final Translation3d REAR_RIGHT_CAMERA_LOCATION_METERS = new Translation3d(0, 0, 0);
public static final PhotonPoseEstimator ORANGE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
ORANGE_ROBOT_TO_CAM);
public static final PhotonPoseEstimator BLACK_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
BLACK_ROBOT_TO_CAM);
public static final PhotonPoseEstimator RED_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
RED_ROBOT_TO_CAM);
public static final PhotonPoseEstimator PURPLE_PHOTON_ESTIMATOR = new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
PURPLE_ROBOT_TO_CAM);
public static final Rotation3d FRONT_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
public static final Rotation3d FRONT_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
public static final Rotation3d REAR_LEFT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
public static final Rotation3d REAR_RIGHT_CAMERA_ANGLE_RADIANS = new Rotation3d(0, 0, 0);
}
public static class ClimberConstants {
public static final int CLIMB_MOTOR_ID = 60;
public static final int RATCHET_PWM_PORT = 9;
public static final double RATCHET_UNLOCK_ANGLE = 0;
public static final double RATCHET_LOCK_ANGLE = 180;
public static final double CLIMBER_SPEED = .5;
public static final double RATCHET_UNLOCK_ANGLE_DEGREES = 0;
public static final double RATCHET_LOCK_ANGLE_DEGREES = 180;
public static final double CLIMBER_SPEED_DUTY_CYCLE = .5;
}
}

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

File diff suppressed because it is too large Load Diff

23
src/main/java/frc/robot/Robot.java
View File

@@ -4,22 +4,11 @@
package frc.robot;
import org.photonvision.targeting.PhotonPipelineResult;
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
@@ -35,8 +24,6 @@ public class Robot extends TimedRobot {
private RobotContainer m_robotContainer;
private Timer disabledTimer;
private static NetworkTable table;
private static GenericEntry distanceFromLimelight;
public Robot() {
@@ -95,16 +82,6 @@ public class Robot extends TimedRobot {
//Constants.IntakeConstants.updateIntakeWheelsRPM();
Constants.ShooterConstants.updateShooterRPM();
//Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM,
Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.BLACK_PHOTON_CAM,
Constants.TargetingConstants.BLACK_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.RED_PHOTON_CAM,
Constants.TargetingConstants.RED_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.PURPLE_PHOTON_CAM,
Constants.TargetingConstants.PURPLE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());
}
/**

9
src/main/java/frc/robot/RobotContainer.java
View File

@@ -7,8 +7,6 @@ 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;
@@ -18,7 +16,6 @@ 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;
@@ -30,13 +27,7 @@ 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 javax.lang.model.util.ElementScanner14;
import frc.robot.subsystems.TargetingSubsystems;
import swervelib.SwerveInputStream;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.subsystems.ClimberSubsystem;

16
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
View File

@@ -8,16 +8,16 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class ClimberSubsystem extends SubsystemBase{
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);
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE);
}
public void lowerRobot() {
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED_DUTY_CYCLE * -1);
}
public void stopClimber() {
@@ -37,11 +37,11 @@ public class ClimberSubsystem extends SubsystemBase{
}
public static void toggleRatchet(boolean toggle) {
if (toggle == true) {
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
} else
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
}
if (toggle == true) {
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE_DEGREES);
} else
climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE_DEGREES);
}
public Command toggleRatchetCommand(boolean toggle) {
return runOnce(() -> toggleRatchet(toggle));

79
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@@ -1,66 +1,65 @@
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.BangBangController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
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 intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
MotorType.kBrushless);
private static SparkFlex intakeWheelsMotor = 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();
private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, MotorType.kBrushless);
private static SparkClosedLoopController intakeWheelsMotorPIDController;
public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
private static BangBangController intakeWheelsMotorBBController = new BangBangController();
private static SimpleMotorFeedforward intakeWheelsMotorFeedforward;
public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
private static PIDController intakeRotatorMotorPIDController;
private static ArmFeedforward intakeRotatorMotorFeedforward;
public IntakeSubsystem() {
intakeRotatorConfig.closedLoop
.p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P)
.i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
.d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
intakeWheelsMotorConfig
.smartCurrentLimit(Constants.IntakeConstants.INTAKE_WHEELS_CURRENT_LIMIT)
.encoder
.positionConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_POSITION_CONVERSION_FACTOR)
.velocityConversionFactor(Constants.IntakeConstants.INTAKE_WHEELS_VELOCITY_CONVERSION_FACTOR);
intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
.p(.06, ClosedLoopSlot.kSlot1)
.i(0, ClosedLoopSlot.kSlot1)
.d(0, ClosedLoopSlot.kSlot1);
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
intakeWheelsMotorFeedforward = new SimpleMotorFeedforward(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_S, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
intakeRotatorConfig
.smartCurrentLimit(Constants.IntakeConstants.INTAKE_ROTATOR_CURRENT_LIMIT)
.encoder
.positionConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_POSITION_CONVERSION_FACTOR)
.velocityConversionFactor(Constants.IntakeConstants.INTAKE_ROTATOR_VELOCITY_CONVERSION_FACTOR);
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorMotor.getEncoder().setPosition(Constants.IntakeConstants.INTAKE_ROTATOR_INITIAL_ENCODER_VALUE);
intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P,
Constants.IntakeConstants.INTAKE_MOTOR_I,
Constants.IntakeConstants.INTAKE_MOTOR_D);
intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
intakeRotatorMotorPIDController = new PIDController(Constants.IntakeConstants.INTAKE_ROTATOR_UP_P, Constants.IntakeConstants.INTAKE_ROTATOR_UP_I, Constants.IntakeConstants.INTAKE_ROTATOR_UP_D);
intakeRotatorMotorFeedforward = new ArmFeedforward(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_S, Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_G, Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_V);
}
public void startIntakeMotor() {
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, ControlType.kVelocity);
intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM);
}
public void reverseIntakeMotor() {
intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1, ControlType.kVelocity);
intakeWheelsMotorBBController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1);
}
public void stopIntakeMotor() {
intakeWheelsMotor.set(0);
intakeWheelsMotorBBController.setSetpoint(0.0);
}
public Command startIntakeMotorCommand() {
@@ -76,7 +75,7 @@ public class IntakeSubsystem extends SubsystemBase {
}
public void deployIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0);
intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_POSITION_RADS);
}
public Command deployintakeCommand() {
@@ -84,7 +83,7 @@ public class IntakeSubsystem extends SubsystemBase {
}
public void retractIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1);
intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_POSITION_RADS);
}
public Command retractIntakeCommand() {
@@ -92,8 +91,7 @@ public class IntakeSubsystem extends SubsystemBase {
}
public void assistFuelIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
ControlType.kPosition);
intakeRotatorMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_POSITION_RADS);
}
public Command assistFuelIntakeCommand() {
@@ -103,6 +101,13 @@ public class IntakeSubsystem extends SubsystemBase {
@Override
public void periodic() {
intakeWheelsMotor.setVoltage(
12.0 * intakeWheelsMotorBBController.calculate(intakeWheelsMotor.getEncoder().getVelocity())
+ 0.9 * intakeWheelsMotorFeedforward.calculate(intakeWheelsMotorBBController.getSetpoint()));
intakeRotatorMotor.setVoltage(
12.0 * intakeRotatorMotorPIDController.calculate(intakeRotatorMotor.getEncoder().getPosition())
+ intakeRotatorMotorFeedforward.calculate(intakeRotatorMotorPIDController.getSetpoint(), 0.0));
SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
}
}

107
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View File

@@ -1,30 +1,17 @@
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.SparkClosedLoopController;
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;
import edu.wpi.first.math.controller.BangBangController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
public class ShooterSubsystem extends SubsystemBase {
private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
@@ -39,46 +26,41 @@ public class ShooterSubsystem extends SubsystemBase {
private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
MotorType.kBrushless);
private static SparkClosedLoopController centerShooterMotorPIDController;
public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig();
private static BangBangController centerShooterMotorBBController = new BangBangController();
private static SimpleMotorFeedforward centerShooterMotorFeedforward;
private static SparkClosedLoopController leftShooterMotorPIDController;
public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
private static BangBangController leftShooterMotorBBController = new BangBangController();
private static SimpleMotorFeedforward leftShooterMotorFeedforward;
private static SparkClosedLoopController rightShooterMotorPIDController;
public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig();
private static BangBangController rightShooterMotorBBController = new BangBangController();
private static SimpleMotorFeedforward rightShooterMotorFeedforward;
private static SparkClosedLoopController indexerAndRampMotorPIDController;
public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig();
private static PIDController indexerAndRampMotorPIDController;
public ShooterSubsystem() {
centerShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
Constants.ShooterConstants.SHOOTER_MOTOR_I,
Constants.ShooterConstants.SHOOTER_MOTOR_D);
centerShooterMotorConfig.smartCurrentLimit(60);
centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
com.revrobotics.PersistMode.kNoPersistParameters);
centerShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.CENTER_MOTOR_S, Constants.ShooterConstants.CENTER_MOTOR_V);
leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
Constants.ShooterConstants.SHOOTER_MOTOR_I,
Constants.ShooterConstants.SHOOTER_MOTOR_D);
leftShooterMotorConfig.smartCurrentLimit(60);
leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController();
com.revrobotics.PersistMode.kNoPersistParameters);
leftShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.LEFT_MOTOR_S, Constants.ShooterConstants.LEFT_MOTOR_V);
rightShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
Constants.ShooterConstants.SHOOTER_MOTOR_I,
Constants.ShooterConstants.SHOOTER_MOTOR_D);
rightShooterMotorConfig.smartCurrentLimit(60);
rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController();
com.revrobotics.PersistMode.kNoPersistParameters);
rightShooterMotorFeedforward = new SimpleMotorFeedforward(Constants.ShooterConstants.RIGHT_MOTOR_S, Constants.ShooterConstants.RIGHT_MOTOR_V);
indexerAndRampMotorConfig.closedLoop.pid(0.0001,
0,
0);
indexerAndRampMotorConfig.smartCurrentLimit(60);
indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController();
com.revrobotics.PersistMode.kNoPersistParameters);
indexerAndRampMotorPIDController = new PIDController(Constants.ShooterConstants.INDEXER_MOTOR_P, Constants.ShooterConstants.INDEXER_MOTOR_I, Constants.ShooterConstants.INDEXER_MOTOR_D);
}
@@ -94,49 +76,49 @@ public class ShooterSubsystem extends SubsystemBase {
private static double currentPotentiometerPosition; // might need second value for the right potentiometer
public void setShooterMotorsRPM() {
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
}
//test individual motor code
public void setLeftShooterMotorRPM() {
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
leftShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
}
public Command testLeftShooterCommand() {
return runOnce(() -> setLeftShooterMotorRPM());
}
public void setRightShooterMotorRPM() {
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
rightShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
}
public Command testRightShooterCommand() {
return runOnce(() -> setRightShooterMotorRPM());
}
public void setCenterShooterMotorRPM() {
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
centerShooterMotorBBController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM);
}
public Command testCenterShooterCommand() {
return runOnce(() -> setCenterShooterMotorRPM());
}
public void stopLeftShooterMotorRPM() {
leftShooterMotor.set(0);
leftShooterMotorBBController.setSetpoint(0.0);
}
public Command stopLeftShooterCommand() {
return runOnce(() -> stopLeftShooterMotorRPM());
}
public void stopCenterShooterMotorRPM() {
centerShooterMotor.set(0);
centerShooterMotorBBController.setSetpoint(0.0);
}
public Command stopCenterShooterCommand() {
return runOnce(() -> stopCenterShooterMotorRPM());
}
public void stopRightShooterMotorRPM() {
rightShooterMotor.set(0);
rightShooterMotorBBController.setSetpoint(0.0);
}
public Command stopRightShooterCommand() {
return runOnce(() -> stopRightShooterMotorRPM());
@@ -148,11 +130,11 @@ public class ShooterSubsystem extends SubsystemBase {
}
public void setIndexerAndRampMotorRPM() {
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity);
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM);
}
public void reverseIndexerAndRampMotorRPM() {
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, ControlType.kVelocity);
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1);
}
public Command reverseIndexerAndRampMotorRPMCommand() {
@@ -164,7 +146,7 @@ public class ShooterSubsystem extends SubsystemBase {
}
public Command stopIndexerAndRampMotorCommand() {
return runOnce(()-> indexerAndRampMotor.set(0));
return runOnce(()-> indexerAndRampMotorPIDController.setSetpoint(0.0));
}
/*public Command shootFuelCommand() {
@@ -181,12 +163,10 @@ public class ShooterSubsystem extends SubsystemBase {
public void stopShooters() {
centerShooterMotor.set(0);
leftShooterMotor.set(0);
rightShooterMotor.set(0);
indexerAndRampMotor.set(0);
centerShooterMotorBBController.setSetpoint(0.0);
leftShooterMotorBBController.setSetpoint(0.0);
rightShooterMotorBBController.setSetpoint(0.0);
indexerAndRampMotorPIDController.setSetpoint(0.0);
}
public Command stopShooterCommand() {
@@ -217,6 +197,17 @@ public class ShooterSubsystem extends SubsystemBase {
@Override
public void periodic() {
centerShooterMotor.setVoltage(
12.0 * centerShooterMotorBBController.calculate(centerShooterMotor.getEncoder().getVelocity())
+ 0.9 * centerShooterMotorFeedforward.calculate(centerShooterMotorBBController.getSetpoint()));
leftShooterMotor.setVoltage(
12.0 * leftShooterMotorBBController.calculate(leftShooterMotor.getEncoder().getVelocity())
+ 0.9 * leftShooterMotorFeedforward.calculate(leftShooterMotorBBController.getSetpoint()));
rightShooterMotor.setVoltage(
12.0 * rightShooterMotorBBController.calculate(rightShooterMotor.getEncoder().getVelocity())
+ 0.9 * rightShooterMotorFeedforward.calculate(rightShooterMotorBBController.getSetpoint()));
indexerAndRampMotor.setVoltage(
12.0 * indexerAndRampMotorPIDController.calculate(indexerAndRampMotor.getEncoder().getVelocity()));
SmartDashboard.putNumber("Shooter Velocity", leftShooterMotor.getEncoder().getVelocity());
/* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());

207
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
View File

@@ -1,207 +0,0 @@
package frc.robot.subsystems;
import java.util.List;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonUtils;
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 {
PIDController photonAimPIDController = new PIDController(3, 0, 0.001);
public TargetingSubsystems() {
photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI);
}
Pose2d currentRobotPose;
public List<Waypoint> pathWaypoints;
public Command pathPlanToPoseCommand(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
GoalEndState goalEndState = new GoalEndState(0, desiredPose.getRotation());
PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
currentRobotPose = swerveDrive.getPose();
pathWaypoints = PathPlannerPath.waypointsFromPoses(
currentRobotPose, desiredPose);
PathPlannerPath goToDesiredPose = new PathPlannerPath(pathWaypoints, pathConstraints, null,
goalEndState);
goToDesiredPose.preventFlipping = true;
return swerveDrive.getAutonomousCommand("goToDesiredPose");
}
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);
double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
desiredPose.getRotation().getRadians());
swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, false);
}, swerveDrive);
}
public Command aimAtHub(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
return new RunCommand(() -> {
currentRobotPose = swerveDrive.getPose();
// Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose,
Constants.TargetingConstants.HUB_POSE);
double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
angleDifference.getRadians());
angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0);
swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1, -driverXbox.getLeftY() * -1), angleSpeed,
false);
}, swerveDrive);
}
Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
return new RunCommand(() -> {
double rot = 0.0;
var result = Constants.TargetingConstants.RED_PHOTON_CAM.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.getLeftX() * -1,
driverXbox.getLeftY() * -1), rot, false);
}, swerveDrive);
}
public PhotonPoseEstimator getPhotonPoseEstimator(PhotonPoseEstimator poseEstimator) {
return poseEstimator;
}
/*
* // 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 static Optional<EstimatedRobotPose> visionEst = Optional.empty();
public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) {
/* for (var result : camera.getAllUnreadResults()) {
visionEst = poseEstimator.estimateCoprocMultiTagPose(result);
if (visionEst.isPresent()) {
EstimatedRobotPose estimatedPose = visionEst.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");
*/
}
}

1279
src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View File

File diff suppressed because it is too large Load Diff

444
src/main/java/frc/robot/subsystems/swervedrive/Vision.java
View File

@@ -16,12 +16,12 @@ 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 frc.robot.Constants;
import java.awt.Desktop;
import java.util.ArrayList;
import java.util.List;
@@ -40,57 +40,59 @@ 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
* 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 {
public class Vision
{
/**
* April Tag Field Layout of the year.
*/
public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(
AprilTagFields.k2026RebuiltAndymark);
public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(
AprilTagFields.k2025ReefscapeAndyMark);
/**
* Ambiguity defined as a value between (0,1). Used in
* {@link Vision#filterPose}.
* Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
*/
private final double maximumAmbiguity = 0.25;
private final double maximumAmbiguity = 0.25;
/**
* Photon Vision Simulation
*/
public VisionSystemSim visionSim;
public VisionSystemSim visionSim;
/**
* Count of times that the odom thinks we're more than 10meters away from the
* april tag.
* Count of times that the odom thinks we're more than 10meters away from the april tag.
*/
private double longDistangePoseEstimationCount = 0;
private double longDistangePoseEstimationCount = 0;
/**
* Current pose from the pose estimator using wheel odometry.
*/
private Supplier<Pose2d> currentPose;
private Supplier<Pose2d> currentPose;
/**
* Field from {@link swervelib.SwerveDrive#field}
*/
private Field2d field2d;
private Field2d field2d;
/**
* Constructor for the Vision class.
*
* @param currentPose Current pose supplier, should reference
* {@link SwerveDrive#getPose()}
* @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) {
public Vision(Supplier<Pose2d> currentPose, Field2d field)
{
this.currentPose = currentPose;
this.field2d = field;
if (Robot.isSimulation()) {
if (Robot.isSimulation())
{
visionSim = new VisionSystemSim("Vision");
visionSim.addAprilTags(fieldLayout);
for (Cameras c : Cameras.values()) {
for (Cameras c : Cameras.values())
{
c.addToVisionSim(visionSim);
}
@@ -102,48 +104,50 @@ public class Vision {
* 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
* @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) {
public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset)
{
Optional<Pose3d> aprilTagPose3d = fieldLayout.getTagPose(aprilTag);
if (aprilTagPose3d.isPresent()) {
if (aprilTagPose3d.isPresent())
{
return aprilTagPose3d.get().toPose2d().transformBy(robotOffset);
} else {
} 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.
* 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()) {
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.
* 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.
* 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.
* 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()) {
for (Cameras camera : Cameras.values())
{
Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
if (poseEst.isPresent()) {
if (poseEst.isPresent())
{
var pose = poseEst.get();
swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(),
pose.timestampSeconds,
camera.curStdDevs);
pose.timestampSeconds,
camera.curStdDevs);
}
}
@@ -152,22 +156,24 @@ public class Vision {
/**
* 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>
* <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
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera) {
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera)
{
Optional<EstimatedRobotPose> poseEst = camera.getEstimatedGlobalPose();
if (Robot.isSimulation()) {
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()),
est ->
debugField
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
() -> {
debugField.getObject("VisionEstimation").setPoses();
});
@@ -175,39 +181,46 @@ public class Vision {
return poseEst;
}
/**
* Filter pose via the ambiguity and find best estimate between all of the
* camera's throwing out distances more than
* 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()) {
private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose)
{
if (pose.isPresent())
{
double bestTargetAmbiguity = 1; // 1 is max ambiguity
for (PhotonTrackedTarget target : pose.get().targetsUsed) {
for (PhotonTrackedTarget target : pose.get().targetsUsed)
{
double ambiguity = target.getPoseAmbiguity();
if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) {
if (ambiguity != -1 && ambiguity < bestTargetAmbiguity)
{
bestTargetAmbiguity = ambiguity;
}
}
// ambiguity to high dont use estimate
if (bestTargetAmbiguity > maximumAmbiguity) {
//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) {
//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) {
//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 {
} else
{
longDistangePoseEstimationCount = 0;
}
return pose;
@@ -215,13 +228,15 @@ public class Vision {
return Optional.empty();
}
/**
* Get distance of the robot from the AprilTag pose.
*
* @param id AprilTag ID
* @return Distance
*/
public double getDistanceFromAprilTag(int id) {
public double getDistanceFromAprilTag(int id)
{
Optional<Pose3d> tag = fieldLayout.getTagPose(id);
return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0);
}
@@ -233,12 +248,17 @@ public class Vision {
* @param camera Camera to check.
* @return Tracked target.
*/
public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) {
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) {
for (PhotonPipelineResult result : camera.resultsList)
{
if (result.hasTargets())
{
for (PhotonTrackedTarget i : result.getTargets())
{
if (i.getFiducialId() == id)
{
return i;
}
}
@@ -253,46 +273,54 @@ public class Vision {
*
* @return Vision Simulation
*/
public VisionSystemSim getVisionSim() {
public VisionSystemSim getVisionSim()
{
return visionSim;
}
/**
* Open up the photon vision camera streams on the localhost, assumes running
* photon vision on localhost.
* 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();
// }
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() {
public void updateVisionField()
{
List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>();
for (Cameras c : Cameras.values()) {
if (!c.resultsList.isEmpty()) {
for (Cameras c : Cameras.values())
{
if (!c.resultsList.isEmpty())
{
PhotonPipelineResult latest = c.resultsList.get(0);
if (latest.hasTargets()) {
if (latest.hasTargets())
{
targets.addAll(latest.targets);
}
}
}
List<Pose2d> poses = new ArrayList<>();
for (PhotonTrackedTarget target : targets) {
if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) {
for (PhotonTrackedTarget target : targets)
{
if (fieldLayout.getTagPose(target.getFiducialId()).isPresent())
{
Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d();
poses.add(targetPose);
}
@@ -304,100 +332,98 @@ public class Vision {
/**
* Camera Enum to select each camera
*/
enum Cameras {
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)),
FRONT_LEFT("Front Left Camera",
Constants.TargetingConstants.FRONT_LEFT_CAMERA_ANGLE_RADIANS,
Constants.TargetingConstants.FRONT_LEFT_CAMERA_LOCATION_METERS,
Constants.TargetingConstants.SINGLE_TAG_STD_DEVS,
Constants.TargetingConstants.MULTI_TAG_STD_DEVS),
/**
* 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)),
FRONT_RIGHT("Front Right Camera",
Constants.TargetingConstants.FRONT_RIGHT_CAMERA_ANGLE_RADIANS,
Constants.TargetingConstants.FRONT_RIGHT_CAMERA_LOCATION_METERS,
Constants.TargetingConstants.SINGLE_TAG_STD_DEVS,
Constants.TargetingConstants.MULTI_TAG_STD_DEVS),
/**
* 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));
REAR_LEFT("Rear Left Camera",
Constants.TargetingConstants.REAR_LEFT_CAMERA_ANGLE_RADIANS,
Constants.TargetingConstants.REAR_LEFT_CAMERA_LOCATION_METERS,
Constants.TargetingConstants.SINGLE_TAG_STD_DEVS,
Constants.TargetingConstants.MULTI_TAG_STD_DEVS),
REAR_RIGHT("Rear Right Camera",
Constants.TargetingConstants.REAR_RIGHT_CAMERA_ANGLE_RADIANS,
Constants.TargetingConstants.REAR_RIGHT_CAMERA_LOCATION_METERS,
Constants.TargetingConstants.SINGLE_TAG_STD_DEVS,
Constants.TargetingConstants.MULTI_TAG_STD_DEVS);
/**
* Latency alert to use when high latency is detected.
*/
public final Alert latencyAlert;
public final Alert latencyAlert;
/**
* Camera instance for comms.
*/
public final PhotonCamera camera;
public final PhotonCamera camera;
/**
* Pose estimator for camera.
*/
public final PhotonPoseEstimator poseEstimator;
public final PhotonPoseEstimator poseEstimator;
/**
* Standard Deviation for single tag readings for pose estimation.
*/
private final Matrix<N3, N1> singleTagStdDevs;
private final Matrix<N3, N1> singleTagStdDevs;
/**
* Standard deviation for multi-tag readings for pose estimation.
*/
private final Matrix<N3, N1> multiTagStdDevs;
private final Matrix<N3, N1> multiTagStdDevs;
/**
* Transform of the camera rotation and translation relative to the center of
* the robot
* Transform of the camera rotation and translation relative to the center of the robot
*/
private final Transform3d robotToCamTransform;
private final Transform3d robotToCamTransform;
/**
* Current standard deviations used.
*/
public Matrix<N3, N1> curStdDevs;
public Matrix<N3, N1> curStdDevs;
/**
* Estimated robot pose.
*/
public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
/**
* Simulated camera instance which only exists during simulations.
*/
public PhotonCameraSim cameraSim;
public PhotonCameraSim cameraSim;
/**
* Results list to be updated periodically and cached to avoid unnecessary
* queries.
* Results list to be updated periodically and cached to avoid unnecessary queries.
*/
public List<PhotonPipelineResult> resultsList = new ArrayList<>();
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);
private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
/**
* Construct a Photon Camera class with help. Standard deviations are fake
* values, experiment and determine
* 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 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.
* @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) {
Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix)
{
latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning);
camera = new PhotonCamera(name);
@@ -406,22 +432,21 @@ public class Vision {
robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation);
poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout,
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
robotToCamTransform);
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
robotToCamTransform);
poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
this.singleTagStdDevs = singleTagStdDevs;
this.multiTagStdDevs = multiTagStdDevsMatrix;
if (Robot.isSimulation()) {
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.
// 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).
// 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);
@@ -437,31 +462,35 @@ public class Vision {
*
* @param systemSim {@link VisionSystemSim} to use.
*/
public void addToVisionSim(VisionSystemSim systemSim) {
if (Robot.isSimulation()) {
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
* 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!
* @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()) {
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) {
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) {
if (currentAmbiguity < amiguity && currentAmbiguity > 0)
{
bestResult = result;
amiguity = currentAmbiguity;
}
@@ -472,63 +501,63 @@ public class Vision {
/**
* Get the latest result from the current cache.
*
* @return Empty optional if nothing is found. Latest result if something is
* there.
* @return Empty optional if nothing is found. Latest result if something is there.
*/
public Optional<PhotonPipelineResult> getLatestResult() {
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
* 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() {
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.
* Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp.
*/
private void updateUnreadResults() {
private void updateUnreadResults()
{
double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds();
for (PhotonPipelineResult result : resultsList) {
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();
}
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
* 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
* <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.
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for
* estimation.
*/
private void updateEstimatedGlobalPose() {
private void updateEstimatedGlobalPose()
{
Optional<EstimatedRobotPose> visionEst = Optional.empty();
for (var change : resultsList) {
for (var change : resultsList)
{
visionEst = poseEstimator.update(change);
updateEstimationStdDevs(visionEst, change.getTargets());
}
@@ -536,54 +565,63 @@ public class Vision {
}
/**
* Calculates new standard deviations This algorithm is a heuristic that creates
* dynamic standard deviations based
* 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()) {
Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets)
{
if (estimatedPose.isEmpty())
{
// No pose input. Default to single-tag std devs
curStdDevs = singleTagStdDevs;
} else {
} else
{
// Pose present. Start running Heuristic
var estStdDevs = singleTagStdDevs;
int numTags = 0;
double avgDist = 0;
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) {
// 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()) {
if (tagPose.isEmpty())
{
continue;
}
numTags++;
avgDist += tagPose
.get()
.toPose2d()
.getTranslation()
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
avgDist +=
tagPose
.get()
.toPose2d()
.getTranslation()
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
}
if (numTags == 0) {
if (numTags == 0)
{
// No tags visible. Default to single-tag std devs
curStdDevs = singleTagStdDevs;
} else {
} else
{
// One or more tags visible, run the full heuristic.
avgDist /= numTags;
// Decrease std devs if multiple targets are visible
if (numTags > 1) {
if (numTags > 1)
{
estStdDevs = multiTagStdDevs;
}
// Increase std devs based on (average) distance
if (numTags == 1 && avgDist > 4) {
if (numTags == 1 && avgDist > 4)
{
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
} else {
} else
{
estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
}
curStdDevs = estStdDevs;