mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-08 16:28:04 -06:00
Work on adding advantagekit
This commit is contained in:
Michael Mikovsky
@@ -15,9 +15,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||
|
||||
import edu.wpi.first.wpilibj.Alert;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj.Alert.AlertType;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.robot.constants.BuildConstants;
|
||||
|
||||
@@ -55,11 +55,10 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
|
||||
import frc4388.robot.subsystems.Elevator;
|
||||
// Subsystems
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.Elevator.CoordinationState;
|
||||
// import frc4388.robot.subsystems.Endeffector;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
// Utilites
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
@@ -695,8 +694,8 @@ public class RobotContainer {
|
||||
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
|
||||
|
||||
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust *= 2;}))
|
||||
.onFalse(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust /= 2;}));
|
||||
.onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;}))
|
||||
.onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;}));
|
||||
|
||||
new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod()))
|
||||
|
||||
@@ -21,9 +21,12 @@ import frc4388.robot.constants.Constants.ElevatorConstants;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
import frc4388.robot.constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
import frc4388.robot.subsystems.lidar.LidarIO;
|
||||
import frc4388.robot.subsystems.lidar.LidarLiteV2;
|
||||
import frc4388.robot.subsystems.vision.VisionIO;
|
||||
import frc4388.robot.subsystems.vision.VisionPhotonvision;
|
||||
import frc4388.utility.status.FaultCANCoder;
|
||||
import frc4388.utility.status.FaultPhotonCamera;
|
||||
import frc4388.utility.status.FaultPidgeon2;
|
||||
import frc4388.utility.status.FaultTalonFX;
|
||||
|
||||
@@ -35,11 +38,13 @@ public class RobotMap {
|
||||
// private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
|
||||
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
public final PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
|
||||
public final PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
|
||||
public final VisionIO leftCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME), VisionConstants.LEFT_CAMERA_POS);
|
||||
public final VisionIO rightCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME), VisionConstants.RIGHT_CAMERA_POS);
|
||||
|
||||
public final Lidar reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
|
||||
public final Lidar reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
|
||||
// public final LiDAR lidar = new
|
||||
|
||||
public final LiDAR reefLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL), "Reef");
|
||||
public final LiDAR reverseLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL), "Reverse");
|
||||
|
||||
|
||||
/* LED Subsystem */
|
||||
@@ -69,8 +74,8 @@ public class RobotMap {
|
||||
public RobotMap() {
|
||||
configureDriveMotorControllers();
|
||||
|
||||
FaultPhotonCamera.addDevice(leftCamera, "Left Camera");
|
||||
FaultPhotonCamera.addDevice(rightCamera, "Right Camera");
|
||||
// FaultPhotonCamera.addDevice(leftCamera, "Left Camera");
|
||||
// FaultPhotonCamera.addDevice(rightCamera, "Right Camera");
|
||||
|
||||
FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro");
|
||||
|
||||
@@ -111,8 +116,8 @@ public class RobotMap {
|
||||
cameraProp.setAvgLatencyMs(35);
|
||||
cameraProp.setLatencyStdDevMs(5);
|
||||
|
||||
sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
|
||||
sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
|
||||
// sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
|
||||
// sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
|
||||
|
||||
|
||||
sim.leftCamera.enableRawStream(true);
|
||||
|
||||
@@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.compute.ReefPositionHelper;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.compute.ReefPositionHelper.Side;
|
||||
@@ -131,7 +131,7 @@ public class DriveToReef extends Command {
|
||||
(Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
|
||||
(Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) &&
|
||||
(Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
|
||||
(!waitVelocityZero || swerveDrive.state.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
|
||||
(!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
|
||||
);
|
||||
// System.out.println(finished);
|
||||
|
||||
|
||||
@@ -2,22 +2,22 @@ package frc4388.robot.commands.alignment;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
|
||||
// Command to repeat a joystick movement for a specific time.
|
||||
public class DriveUntilLiDAR extends Command {
|
||||
private final SwerveDrive swerveDrive;
|
||||
private final Translation2d leftStick;
|
||||
private final Translation2d rightStick;
|
||||
private final Lidar m_lidar;
|
||||
private final LiDAR m_lidar;
|
||||
private final double mindistance;
|
||||
|
||||
public DriveUntilLiDAR(
|
||||
SwerveDrive swerveDrive,
|
||||
Translation2d leftStick,
|
||||
Translation2d rightStick,
|
||||
Lidar lidar,
|
||||
LiDAR lidar,
|
||||
double mindistance) {
|
||||
addRequirements(swerveDrive);
|
||||
|
||||
|
||||
@@ -8,13 +8,13 @@ import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.lidar.LiDAR;
|
||||
|
||||
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
|
||||
public class LidarAlign extends Command {
|
||||
private SwerveDrive swerveDrive;
|
||||
private Lidar lidar;
|
||||
private LiDAR lidar;
|
||||
|
||||
private int currentFinderTick;
|
||||
// private int tickFoundPipe;
|
||||
@@ -26,7 +26,7 @@ public class LidarAlign extends Command {
|
||||
// private final boolean constructedHeadedRight;
|
||||
|
||||
/** Creates a new LidarAlign. */
|
||||
public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) {
|
||||
public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
|
||||
this.swerveDrive = swerveDrive;
|
||||
|
||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2025RidgeScape";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 170;
|
||||
public static final String GIT_SHA = "2a38f94d5eef00a093f47df192f7c5c8a2b8cf8d";
|
||||
public static final String GIT_DATE = "2025-07-15 10:24:11 MDT";
|
||||
public static final int GIT_REVISION = 171;
|
||||
public static final String GIT_SHA = "3130f647c83cc82b45a5299e19108f9eec45e6f6";
|
||||
public static final String GIT_DATE = "2025-07-15 11:07:01 MDT";
|
||||
public static final String GIT_BRANCH = "advantagekit";
|
||||
public static final String BUILD_DATE = "2025-07-15 11:04:29 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752599069523L;
|
||||
public static final String BUILD_DATE = "2025-07-15 13:40:35 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752608435113L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
|
||||
@@ -4,8 +4,6 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
@@ -11,7 +11,6 @@ import org.littletonrobotics.junction.AutoLogOutput;
|
||||
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.LEDConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
|
||||
@@ -1,71 +0,0 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.Counter;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
|
||||
public class Lidar extends SubsystemBase implements Queryable {
|
||||
|
||||
private Counter LidarPWM;
|
||||
private String name = "Lidar";
|
||||
|
||||
private double distance = -1;
|
||||
public Lidar(int port, String name) {
|
||||
FaultReporter.register(this);
|
||||
|
||||
this.name = name;
|
||||
LidarPWM = new Counter(port);
|
||||
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
|
||||
LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
|
||||
LidarPWM.reset();
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
if(LidarPWM.get() < 1)
|
||||
distance = -1;
|
||||
else
|
||||
distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
|
||||
}
|
||||
|
||||
@AutoLogOutput(key = "Lidar/{name}")
|
||||
public double getDistance(){
|
||||
return distance;
|
||||
}
|
||||
|
||||
public boolean withinDistance(){
|
||||
if(distance == -1) return false;
|
||||
return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Lidar " + name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
if(distance == -1)
|
||||
s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
|
||||
|
||||
s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance);
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -6,7 +6,6 @@ package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
@@ -20,15 +19,11 @@ 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.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.vision.Vision;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
@@ -44,28 +39,28 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
private Vision vision;
|
||||
|
||||
@AutoLog
|
||||
public class SwerveDriveState {
|
||||
public int gear_index = DriveConstants.STARTING_GEAR;
|
||||
public boolean stopped = false;
|
||||
public boolean robotKnowsWhereItIs = false;
|
||||
// @AutoLog
|
||||
// public class SwerveDriveState {
|
||||
public int gear_index = DriveConstants.STARTING_GEAR;
|
||||
public boolean stopped = false;
|
||||
public boolean robotKnowsWhereItIs = false;
|
||||
|
||||
public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
|
||||
public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
|
||||
// 25%
|
||||
public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
|
||||
public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
|
||||
// 25%
|
||||
|
||||
public double lastOdomSpeed;
|
||||
public double lastOdomSpeed;
|
||||
|
||||
public Pose2d initalPose2d = null;
|
||||
public Pose2d initalPose2d = null;
|
||||
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
}
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
// }
|
||||
|
||||
public SwerveDriveState state = new SwerveDriveState();
|
||||
// public SwerveDriveState state = new SwerveDriveState();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
|
||||
@@ -86,7 +81,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// DoubleSupplier a = () -> 1.d;
|
||||
AutoBuilder.configure(
|
||||
() -> {
|
||||
return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(state.initalPose2d);
|
||||
return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
|
||||
}, // Robot pose supplier
|
||||
this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
|
||||
// pose)
|
||||
@@ -135,7 +130,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// null,
|
||||
// null,
|
||||
// null,
|
||||
// (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
|
||||
// (state) -> Logger.recordOutput("Drive/SysIdState", toString())),
|
||||
// new SysIdRoutine.Mechanism(
|
||||
// (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
|
||||
|
||||
@@ -143,7 +138,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
public void setOdoPose(Pose2d pose) {
|
||||
if (pose == null) return;
|
||||
state.initalPose2d = pose;
|
||||
initalPose2d = pose;
|
||||
swerveDriveTrain.resetPose(pose);
|
||||
}
|
||||
|
||||
@@ -162,7 +157,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// }
|
||||
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve drive is still going:
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
|
||||
@@ -170,7 +165,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
state.stopped = false;
|
||||
stopped = false;
|
||||
if (fieldRelative) {
|
||||
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
@@ -178,18 +173,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) {
|
||||
state.rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
|
||||
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
// .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
|
||||
SmartDashboard.putBoolean("drift correction", false);
|
||||
} else {
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(state.rotTarget));
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
|
||||
ctrl.HeadingController.setPID(
|
||||
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
|
||||
DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
|
||||
@@ -202,20 +197,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
|
||||
} else { // Create robot-relative speeds.
|
||||
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(-leftStick.getY() * state.speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(-leftStick.getY() * speedAdjust)
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
}
|
||||
}
|
||||
|
||||
public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
|
||||
state.stopped = false;
|
||||
stopped = false;
|
||||
// Create robot-relative speeds.
|
||||
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
|
||||
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
|
||||
.withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
|
||||
.withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
|
||||
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
|
||||
|
||||
}
|
||||
|
||||
@@ -225,7 +220,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
// relitive version of
|
||||
// this, and no pre
|
||||
// provided version
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve
|
||||
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
|
||||
// drive is still going:
|
||||
stopModules(); // stop the swerve
|
||||
|
||||
@@ -235,8 +230,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(rightStick.getAngle()));
|
||||
}
|
||||
|
||||
@@ -244,8 +239,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
ctrl.HeadingController.setPID(
|
||||
DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
@@ -259,8 +254,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
leftStick = leftStick.rotateBy(heading);
|
||||
|
||||
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withVelocityX(leftStick.getX() * speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(heading);
|
||||
// ctrl.HeadingController.setPID(
|
||||
// DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
|
||||
@@ -304,7 +299,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
public boolean isStopped() {
|
||||
return state.lastOdomSpeed < AutoConstants.STOP_VELOCITY;
|
||||
return lastOdomSpeed < AutoConstants.STOP_VELOCITY;
|
||||
}
|
||||
|
||||
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
|
||||
@@ -318,8 +313,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
|
||||
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX() * -state.speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * state.speedAdjust)
|
||||
.withVelocityX(leftStick.getX() * -speedAdjust)
|
||||
.withVelocityY(leftStick.getY() * speedAdjust)
|
||||
.withTargetDirection(rot));
|
||||
// double
|
||||
}
|
||||
@@ -329,19 +324,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
}
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(state.initalPose2d);
|
||||
return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
swerveDriveTrain.tareEverything();
|
||||
state.robotKnowsWhereItIs = false;
|
||||
state.rotTarget = 0;
|
||||
robotKnowsWhereItIs = false;
|
||||
rotTarget = 0;
|
||||
// vision.resetRotations();
|
||||
}
|
||||
|
||||
|
||||
public void softStop() {
|
||||
state.stopped = true;
|
||||
stopped = true;
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
|
||||
.withVelocityX(0)
|
||||
.withVelocityY(0)
|
||||
@@ -359,7 +354,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
|
||||
SmartDashboard.putNumber("RotTartget", state.rotTarget);
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
|
||||
double time = Vision.getTime();
|
||||
double freq = swerveDriveTrain.getOdometryFrequency();
|
||||
@@ -370,86 +365,95 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
vision.setLastOdomPose(curpose);
|
||||
setLastOdomSpeed(curpose, lastPose, freq);
|
||||
|
||||
// if (vision.isTag()) {5
|
||||
if (vision.isTag()) {
|
||||
Pose2d pose = vision.getPose2d();
|
||||
if (!robotKnowsWhereItIs) {
|
||||
robotKnowsWhereItIs = true;
|
||||
Pose2d curPose = getPose2d();
|
||||
rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
|
||||
}
|
||||
|
||||
vision.addVisionMeasurement(swerveDriveTrain);
|
||||
}
|
||||
|
||||
// if(e.isPresent())
|
||||
}
|
||||
|
||||
private void reset_index() {
|
||||
state.gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
|
||||
gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
|
||||
// robot start in?)
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length)
|
||||
if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
|
||||
reset_index(); // If outof bounds: reset index
|
||||
int i = state.gear_index - 1;
|
||||
int i = gear_index - 1;
|
||||
if (i == -1)
|
||||
i = 0;
|
||||
setPercentOutput(DriveConstants.GEARS[i]);
|
||||
state.gear_index = i;
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length)
|
||||
if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
|
||||
reset_index(); // If outof bounds: reset index
|
||||
int i = state.gear_index + 1;
|
||||
int i = gear_index + 1;
|
||||
if (i == DriveConstants.GEARS.length)
|
||||
i = DriveConstants.GEARS.length - 1;
|
||||
setPercentOutput(DriveConstants.GEARS[i]);
|
||||
state.gear_index = i;
|
||||
gear_index = i;
|
||||
}
|
||||
|
||||
public void setPercentOutput(double speed) {
|
||||
state.speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
state.gear_index = -1;
|
||||
speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
|
||||
gear_index = -1;
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
setPercentOutput(DriveConstants.SLOW_SPEED);
|
||||
state.gear_index = 0;
|
||||
gear_index = 0;
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
setPercentOutput(DriveConstants.FAST_SPEED);
|
||||
state.gear_index = 1;
|
||||
gear_index = 1;
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
setPercentOutput(DriveConstants.TURBO_SPEED);
|
||||
state.gear_index = 2;
|
||||
gear_index = 2;
|
||||
}
|
||||
|
||||
public void shiftUpRot() {
|
||||
state.rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
|
||||
rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
|
||||
}
|
||||
|
||||
public void shiftDownRot() {
|
||||
state.rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
|
||||
rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
|
||||
}
|
||||
|
||||
private int tmp_gear_index = DriveConstants.STARTING_GEAR;
|
||||
|
||||
public void startSlowPeriod() {
|
||||
tmp_gear_index = state.gear_index;
|
||||
tmp_gear_index = gear_index;
|
||||
setToSlow();
|
||||
}
|
||||
|
||||
public void startTurboPeriod() {
|
||||
tmp_gear_index = state.gear_index;
|
||||
tmp_gear_index = gear_index;
|
||||
setToTurbo();
|
||||
}
|
||||
|
||||
public void endSlowPeriod() {
|
||||
setPercentOutput(DriveConstants.GEARS[tmp_gear_index]);
|
||||
state.gear_index = tmp_gear_index;
|
||||
gear_index = tmp_gear_index;
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
|
||||
if(curPose.isPresent() && lastPose.isPresent()){
|
||||
state.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
|
||||
lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,281 +0,0 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
import org.photonvision.EstimatedRobotPose;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonPoseEstimator;
|
||||
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.FieldConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
|
||||
public class Vision extends SubsystemBase implements Queryable {
|
||||
private PhotonCamera[] cameras;
|
||||
private PhotonPoseEstimator[] estimators;
|
||||
|
||||
@AutoLog
|
||||
public class VisionState {
|
||||
public boolean isTagDetected = false;
|
||||
public boolean isTagProcessed = false;
|
||||
public List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
public double latency = 0;
|
||||
public Pose2d lastVisionPose = new Pose2d();
|
||||
public Pose2d lastPhysOdomPose = new Pose2d();
|
||||
}
|
||||
|
||||
private VisionState state = new VisionState();
|
||||
|
||||
private Field2d field = new Field2d();
|
||||
public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){
|
||||
FaultReporter.register(this);
|
||||
SmartDashboard.putData(field);
|
||||
|
||||
this.cameras = new PhotonCamera[]{leftCamera, rightCamera};
|
||||
|
||||
PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS);
|
||||
PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS);
|
||||
|
||||
photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
|
||||
this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight};
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
update();
|
||||
field.setRobotPose(getPose2d());
|
||||
}
|
||||
|
||||
// private Instant lastVisionTime = null;
|
||||
|
||||
|
||||
private void update() {
|
||||
state.isTagProcessed = false;
|
||||
state.isTagDetected = false;
|
||||
|
||||
// Instant now = Instant.now();
|
||||
|
||||
// int cams = 0;
|
||||
|
||||
double latency = 0;
|
||||
|
||||
// Pose2d pose = null;
|
||||
state.poses.clear();
|
||||
|
||||
for(int i = 0; i < cameras.length; i++){
|
||||
PhotonCamera camera = cameras[i];
|
||||
PhotonPoseEstimator estimator = estimators[i];
|
||||
|
||||
var results = camera.getAllUnreadResults();
|
||||
|
||||
// If there are no more updates from the camera
|
||||
if (results.size() == 0)
|
||||
continue;
|
||||
|
||||
|
||||
var result = results.get(results.size()-1);
|
||||
latency += result.getTimestampSeconds();
|
||||
|
||||
state.isTagDetected = state.isTagDetected | result.hasTargets();
|
||||
|
||||
// If there are no tags
|
||||
if(!result.hasTargets())
|
||||
continue;
|
||||
|
||||
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
|
||||
|
||||
// If the tag was failed to be processed
|
||||
if(estimatedRobotPose.isEmpty())
|
||||
continue;
|
||||
|
||||
state.poses.add(estimatedRobotPose.get());
|
||||
|
||||
// if(pose == null)
|
||||
// pose = estimatedRobotPose.get().estimatedPose.toPose2d();
|
||||
// else
|
||||
// pose = pose.interpolate(pose, 0.5);
|
||||
// X += pose.getX();
|
||||
// Y += pose.getY();
|
||||
|
||||
// if(X > 6)
|
||||
|
||||
// Yaw += (pose.getRotation().getDegrees() + 180) % 360;
|
||||
// cams++;
|
||||
|
||||
state.isTagProcessed = true;
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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 getEstimationStdDevs}
|
||||
*
|
||||
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
|
||||
* used for estimation.
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
|
||||
Optional<EstimatedRobotPose> visionEst = Optional.empty();
|
||||
|
||||
var targets = change.getTargets();
|
||||
for(int i = targets.size()-1; i >= 0; i--){
|
||||
Transform3d pos = targets.get(i).getBestCameraToTarget();
|
||||
double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
|
||||
if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
change.targets.remove(i);
|
||||
}
|
||||
}
|
||||
|
||||
if(targets.size() <= 0)
|
||||
return visionEst; // Will be empty
|
||||
|
||||
visionEst = estimator.update(change);
|
||||
// updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
|
||||
|
||||
return 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,
|
||||
// PhotonPoseEstimator estimator) {
|
||||
// if (estimatedPose.isEmpty()) {
|
||||
// // No pose input. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
|
||||
// } else {
|
||||
// // Pose present. Start running Heuristic
|
||||
// var estStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// 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 = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
// if (tagPose.isEmpty()) continue;
|
||||
|
||||
// double distance = tagPose
|
||||
// .get()
|
||||
// .toPose2d()
|
||||
// .getTranslation()
|
||||
// .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
|
||||
// if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
// numTags++;
|
||||
// avgDist += distance;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (numTags == 0) {
|
||||
// // No tags visible. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// } else {
|
||||
// // One or more tags visible, run the full heuristic.
|
||||
// avgDist /= numTags;
|
||||
// // Decrease std devs if multiple targets are visible
|
||||
// if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
|
||||
// // 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;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
public void setLastOdomPose(Optional<Pose2d> pose){
|
||||
if(pose.isPresent())
|
||||
state.lastPhysOdomPose = pose.get();
|
||||
}
|
||||
|
||||
// public double getLastOdomSpeed(){
|
||||
// return lastOdomSpeed;
|
||||
// }
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
if(state.lastPhysOdomPose != null)
|
||||
return state.lastPhysOdomPose;
|
||||
|
||||
// if(lastVisionPose != null)
|
||||
// return lastVisionPose;
|
||||
return new Pose2d();
|
||||
|
||||
}
|
||||
|
||||
public static double getTime() {
|
||||
return Utils.getCurrentTimeSeconds();
|
||||
}
|
||||
|
||||
public boolean isTag(){
|
||||
return state.isTagDetected && state.isTagProcessed;
|
||||
}
|
||||
|
||||
|
||||
public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
|
||||
for(EstimatedRobotPose pose : state.poses){
|
||||
drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Vision";
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
return new Status();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class LiDAR extends SubsystemBase implements Queryable {
|
||||
LidarIO io;
|
||||
LidarStateAutoLogged state = new LidarStateAutoLogged();
|
||||
|
||||
private String name = "Lidar";
|
||||
|
||||
public LiDAR(LidarIO device, String name) {
|
||||
FaultReporter.register(this);
|
||||
|
||||
this.io = device;
|
||||
this.name = name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
io.updateInputs(state);
|
||||
}
|
||||
|
||||
// @AutoLogOutput(key = "Lidar/{name}")
|
||||
public double getDistance(){
|
||||
return state.distance;
|
||||
}
|
||||
|
||||
public boolean withinDistance(){
|
||||
if(state.distance == -1) return false;
|
||||
return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Lidar " + name;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
if(state.distance == -1)
|
||||
s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
|
||||
|
||||
s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance);
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
public interface LidarIO {
|
||||
@AutoLog
|
||||
public class LidarState {
|
||||
public boolean connected;
|
||||
public double distance;
|
||||
}
|
||||
|
||||
public default void updateInputs(LidarState state) {}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package frc4388.robot.subsystems.lidar;
|
||||
|
||||
import edu.wpi.first.wpilibj.Counter;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
|
||||
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
|
||||
public class LidarLiteV2 implements LidarIO {
|
||||
|
||||
|
||||
private Counter LidarPWM;
|
||||
|
||||
public LidarLiteV2(int port) {
|
||||
LidarPWM = new Counter(port);
|
||||
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
|
||||
LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
|
||||
LidarPWM.reset();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(LidarState state) {
|
||||
|
||||
if(LidarPWM.get() < 1)
|
||||
state.distance = -1;
|
||||
else
|
||||
state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLogOutput;
|
||||
import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status;
|
||||
|
||||
public class Vision extends SubsystemBase implements Queryable {
|
||||
VisionIO[] io;
|
||||
VisionStateAutoLogged[] state;
|
||||
|
||||
|
||||
public Pose2d lastVisionPose = new Pose2d();
|
||||
public Pose2d lastPhysOdomPose = new Pose2d();
|
||||
|
||||
public Vision(VisionIO... devices) {
|
||||
FaultReporter.register(this);
|
||||
io = devices;
|
||||
state = new VisionStateAutoLogged[io.length];
|
||||
|
||||
for(int i = 0; i < io.length; i++) {
|
||||
state[i] = new VisionStateAutoLogged();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
for(int i = 0; i < io.length; i++) {
|
||||
io[i].updateInputs(state[i]);
|
||||
Logger.processInputs("Vision/Camera" + i , state[i]);
|
||||
}
|
||||
}
|
||||
|
||||
public void addVisionMeasurement(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
|
||||
// for(EstimatedRobotPose pose : poses){
|
||||
//
|
||||
// }
|
||||
for(int i = 0; i < state.length; i++) {
|
||||
if(state[i].lastEstimatedPose != null) {
|
||||
PoseObservation pose = state[i].lastEstimatedPose;
|
||||
drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void setLastOdomPose(Optional<Pose2d> pose){
|
||||
if(pose.isPresent())
|
||||
lastPhysOdomPose = pose.get();
|
||||
}
|
||||
|
||||
public boolean isTag(){
|
||||
for(int i = 0; i < state.length; i++){
|
||||
if(state[i].isTagDetected && state[i].isTagProcessed)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@AutoLogOutput
|
||||
public Pose2d getPose2d() {
|
||||
if(lastPhysOdomPose != null)
|
||||
return lastPhysOdomPose;
|
||||
|
||||
// if(lastVisionPose != null)
|
||||
// return lastVisionPose;
|
||||
return new Pose2d();
|
||||
|
||||
}
|
||||
|
||||
public static double getTime() {
|
||||
return Utils.getCurrentTimeSeconds();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
return new Status();
|
||||
// // TODO Auto-generated method stub
|
||||
// throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'");
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import org.littletonrobotics.junction.AutoLog;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
|
||||
public interface VisionIO {
|
||||
@AutoLog
|
||||
public class VisionState {
|
||||
public boolean isTagDetected = false;
|
||||
public boolean isTagProcessed = false;
|
||||
// public double latency = 0;
|
||||
public PoseObservation lastEstimatedPose = null;
|
||||
}
|
||||
|
||||
public static record PoseObservation(
|
||||
Pose2d pose,
|
||||
double timestamp
|
||||
) {}
|
||||
|
||||
public default void updateInputs(VisionState state) {}
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
package frc4388.robot.subsystems.vision;
|
||||
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
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.PhotonPipelineResult;
|
||||
import frc4388.robot.constants.Constants.FieldConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
|
||||
public class VisionPhotonvision implements VisionIO {
|
||||
// private PhotonCamera[] cameras;
|
||||
// private PhotonPoseEstimator[] estimators;
|
||||
|
||||
private PhotonCamera camera;
|
||||
private PhotonPoseEstimator estimator;
|
||||
|
||||
// public List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
|
||||
|
||||
public VisionPhotonvision(PhotonCamera camera, Transform3d position){
|
||||
this.camera = camera;
|
||||
estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
|
||||
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
}
|
||||
|
||||
// private Instant lastVisionTime = null;
|
||||
|
||||
|
||||
public void updateInputs(VisionState state) {
|
||||
state.isTagProcessed = false;
|
||||
state.isTagDetected = false;
|
||||
|
||||
state.lastEstimatedPose = null;
|
||||
|
||||
var results = camera.getAllUnreadResults();
|
||||
|
||||
// If there are no more updates from the camera
|
||||
if (results.size() == 0)
|
||||
return;
|
||||
|
||||
|
||||
var result = results.get(results.size()-1);
|
||||
|
||||
state.isTagDetected = state.isTagDetected | result.hasTargets();
|
||||
|
||||
// If there are no tags
|
||||
if(!result.hasTargets())
|
||||
return;
|
||||
|
||||
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
|
||||
|
||||
// If the tag was failed to be processed
|
||||
if(estimatedRobotPose.isEmpty())
|
||||
return;
|
||||
|
||||
EstimatedRobotPose pose = estimatedRobotPose.get();
|
||||
|
||||
state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds);
|
||||
|
||||
state.isTagProcessed = true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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 getEstimationStdDevs}
|
||||
*
|
||||
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
|
||||
* used for estimation.
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
|
||||
Optional<EstimatedRobotPose> visionEst = Optional.empty();
|
||||
|
||||
var targets = change.getTargets();
|
||||
for(int i = targets.size()-1; i >= 0; i--){
|
||||
Transform3d pos = targets.get(i).getBestCameraToTarget();
|
||||
double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
|
||||
if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
change.targets.remove(i);
|
||||
}
|
||||
}
|
||||
|
||||
if(targets.size() <= 0)
|
||||
return visionEst; // Will be empty
|
||||
|
||||
visionEst = estimator.update(change);
|
||||
// updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
|
||||
|
||||
return visionEst;
|
||||
}
|
||||
|
||||
public String getName() {
|
||||
return camera.getName();
|
||||
}
|
||||
|
||||
|
||||
// /**
|
||||
// * 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,
|
||||
// PhotonPoseEstimator estimator) {
|
||||
// if (estimatedPose.isEmpty()) {
|
||||
// // No pose input. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
|
||||
// } else {
|
||||
// // Pose present. Start running Heuristic
|
||||
// var estStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// 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 = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
// if (tagPose.isEmpty()) continue;
|
||||
|
||||
// double distance = tagPose
|
||||
// .get()
|
||||
// .toPose2d()
|
||||
// .getTranslation()
|
||||
// .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
|
||||
// if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
// numTags++;
|
||||
// avgDist += distance;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (numTags == 0) {
|
||||
// // No tags visible. Default to single-tag std devs
|
||||
// curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// } else {
|
||||
// // One or more tags visible, run the full heuristic.
|
||||
// avgDist /= numTags;
|
||||
// // Decrease std devs if multiple targets are visible
|
||||
// if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
|
||||
// // 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;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
}
|
||||
@@ -6,9 +6,5 @@ import frc4388.robot.RobotContainer;
|
||||
|
||||
// Class to update a series of WPILIB Alerts
|
||||
public class Alerts {
|
||||
private static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError);
|
||||
|
||||
public static void UpdateAlerts(RobotContainer m_RobotContainer) {
|
||||
no_auto.set(!m_RobotContainer.autoChooserUpdated);
|
||||
}
|
||||
public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user