Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 08:48:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into Elevator

Browse Source
This commit is contained in:
C4llSiqn
2025-01-20 14:47:58 -07:00
parent d341b6a7d7 adadac3d8b
commit 5a57da2698
9 changed files with 542 additions and 264 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Lidar.java
+80
View File
@@ -0,0 +1,80 @@
package frc4388.robot.subsystems;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.PWM;
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 frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
public class Lidar extends Subsystem {
private double distance = -1;
Counter LidarPWM = new Counter(AutoConstants.LIDAR_DIO_CHANNEL);
public Lidar() {
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() * AutoConstants.SECONDS_TO_MICROS) / AutoConstants.LIDAR_MICROS_TO_CM;
}
public double getDistance(){
return distance;
}
public boolean withinDistance(){
if(distance == -1) return false;
return distance < AutoConstants.LIDAR_DETECT_DISTANCE;
}
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbDistance = subsystemLayout
.add("Distance", 0)
.withWidget(BuiltInWidgets.kGraph)
.getEntry();
GenericEntry sbWithinDistance = subsystemLayout
.add("Within Distance", 0)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
@Override
public String getSubsystemName() {
return "Lidar";
}
@Override
public void queryStatus() {
sbDistance.setDouble(distance);
sbWithinDistance.setBoolean(withinDistance());
}
@Override
public Status diagnosticStatus() {
Status s = new Status();
if(distance == -1){
s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED");
}else{
s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED");
}
return s;
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+206 -184
View File
@@ -43,292 +43,314 @@ public class SwerveDrive extends Subsystem {
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
private Vision vision;
private int gear_index = SwerveDriveConstants.STARTING_GEAR;
private boolean stopped = false;
public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25%
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
// 25%
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
super();
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
// swerveDriveTrain) {
super();
this.swerveDriveTrain = swerveDriveTrain;
this.vision = vision;
this.swerveDriveTrain = swerveDriveTrain;
this.vision = vision;
RobotConfig config;
try{
config = RobotConfig.fromGUISettings();
} catch (Exception e) {
// Handle exception as needed
config = null;
}
RobotConfig config;
try {
config = RobotConfig.fromGUISettings();
} catch (Exception e) {
// Handle exception as needed
config = null;
}
// DoubleSupplier a = () -> 1.d;
AutoBuilder.configure(
() -> {
() -> {
var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d());
// pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1));
return pose;//getRotation().times(-1)
}, // Robot pose supplier
swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
.withSpeeds(speeds)
), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
),
config, // The robot configuration
() -> {
// Boolean supplier that controls when the path will be mirrored for the red alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
return pose;// getRotation().times(-1)
}, // Robot pose supplier
swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting
// pose)
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
// Also optionally outputs individual module feedforwards
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
// holonomic drive trains
new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
),
config, // The robot configuration
() -> {
// Boolean supplier that controls when the path will be mirrored for the red
// alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this // Reference to this subsystem to set requirements
);
var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this // Reference to this subsystem to set requirements
);
}
}
// public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
// // rightStick.getAngle()
// double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
// // System.out.println(ang);
// // module.go(ang);
// // Rotation2d rot = Rotation2d.fromRadians(ang);
// Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
// SwerveModuleState state = new SwerveModuleState(speed, rot);
// module.setDesiredState(state);
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
// Translation2d rightStick){
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
// // rightStick.getAngle()
// double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) +
// Math.pow(leftStick.getY(), 2));
// // System.out.println(ang);
// // module.go(ang);
// // Rotation2d rot = Rotation2d.fromRadians(ang);
// Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
// SwerveModuleState state = new SwerveModuleState(speed, rot);
// module.setDesiredState(state);
// }
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
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
return; // don't bother doing swerve drive math and return early.
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
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
if (fieldRelative) {
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(-leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
);
// double rot = 0;
// ! drift correction
// dependant on if the new odomitry system acounts for rotational drift, this may not be needed.
// if (rightStick.getNorm() > 0.05) {
// rotTarget = swerveDriveTrain.getRotation3d().toRotation2d().getDegrees();
// swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
// .withVelocityX(leftStick.getX()*speedAdjust)
// .withVelocityY(leftStick.getY()*speedAdjust)
// .withRotationalRate(rightStick.getY()*rotSpeedAdjust)
// );
// // SmartDashboard.putBoolean("drift correction", false);
// stopped = false;
// } else if(leftStick.getNorm() > 0.05) {
// if (!stopped) {
// stopModules();
// stopped = true;
// }
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early.
// // SmartDashboard.putBoolean("drift correction", true);
// // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
if (SwerveDriveConstants.INVERT_X) leftStick = new Translation2d(-leftStick.getX(), leftStick.getY());
if (SwerveDriveConstants.INVERT_Y) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY());
if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1);
stopped = false;
if (fieldRelative) {
// ! drift correction
if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
SmartDashboard.putBoolean("drift correction", false);
} else {
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(Rotation2d.fromDegrees(rotTarget));
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
);
swerveDriveTrain.setControl(ctrl);
SmartDashboard.putBoolean("drift correction", true);
}
// }
// // SmartDashboard.putBoolean("drift correction", true);
// // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// // rot = ((rotTarget - gyro.getAngle()) / 360) *
// SwerveDriveConstants.ROT_CORRECTION_SPEED;
// // Convert field-relative speeds to robot-relative speeds.
// // chassisSpeeds = chassisSpeeds.
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
} else { // Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(-leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
);
}
// }
// // Use the left joystick to set speed. Apply a cubic curve and the set max
// speed.
// Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00),
// Math.pow(speed.getY(), 3.00));
// // Convert field-relative speeds to robot-relative speeds.
// // chassisSpeeds = chassisSpeeds.
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 *
// speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction,
// gyro.getRotation2d().times(-1));
} else { // Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(-leftStick.getY() * speedAdjust)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
}
}
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version
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
return; // don't bother doing swerve drive math and return early.
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
// reason to have a robot
// relitive version of
// this, and no pre
// provided version
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
leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withTargetDirection(rightStick.getAngle())
);
if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
return; // don't bother doing swerve drive math and return early.
leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rightStick.getAngle()));
}
public boolean rotateToTarget(double angle) {
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(0)
.withVelocityY(0)
.withTargetDirection(Rotation2d.fromDegrees(angle))
);
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(0)
.withVelocityY(0)
.withTargetDirection(Rotation2d.fromDegrees(angle)));
if (Math.abs(angle - getGyroAngle()) < 5.0) {
return true;
}
if (Math.abs(angle - getGyroAngle()) < 5.0) {
return true;
}
return false;
return false;
}
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
// stopModules(); // stop the swerve
// if (leftStick.getNorm() < 0.05) //if no imput
// return; // don't bother doing swerve drive math and return early.
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
// swerve drive is still going:
// stopModules(); // stop the swerve
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX()*-speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withTargetDirection(rot)
);
// double
// if (leftStick.getNorm() < 0.05) //if no imput
// return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * -speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(rot));
// double
}
public double getGyroAngle() {
return swerveDriveTrain.getRotation3d().getAngle();
return swerveDriveTrain.getRotation3d().getAngle();
}
public void resetGyro() {
swerveDriveTrain.tareEverything();
swerveDriveTrain.tareEverything();
}
public void stopModules() {
swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
stopped = true;
swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
}
@Override
public void periodic() {
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
SmartDashboard.putNumber("RotTartget", rotTarget);
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
SmartDashboard.putNumber("RotTartget", rotTarget);
double time = Vision.getTime();
double time = Vision.getTime();
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
if(vision.isTag()){
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
}
if (vision.isTag()) {
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
}
// if(e.isPresent())
// if(e.isPresent())
}
private void reset_index() {
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?)
gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
// robot start in?)
}
public void shiftDown() {
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
int i = gear_index - 1;
if (i == -1) i = 0;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
reset_index(); // If outof bounds: reset index
int i = gear_index - 1;
if (i == -1)
i = 0;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
}
public void shiftUp() {
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
int i = gear_index + 1;
if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
reset_index(); // If outof bounds: reset index
int i = gear_index + 1;
if (i == SwerveDriveConstants.GEARS.length)
i = SwerveDriveConstants.GEARS.length - 1;
setPercentOutput(SwerveDriveConstants.GEARS[i]);
gear_index = i;
}
public void setPercentOutput(double speed) {
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
gear_index = -1;
speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
gear_index = -1;
}
public void setToSlow() {
setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
gear_index = 0;
setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
gear_index = 0;
}
public void setToFast() {
setPercentOutput(SwerveDriveConstants.FAST_SPEED);
gear_index = 1;
setPercentOutput(SwerveDriveConstants.FAST_SPEED);
gear_index = 1;
}
public void setToTurbo() {
setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
gear_index = 2;
setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
gear_index = 2;
}
public void shiftUpRot() {
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
}
public void shiftDownRot() {
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
}
@Override
public String getSubsystemName() {
return "Swerve Drive Controller";
return "Swerve Drive Controller";
}
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbGyro = subsystemLayout
.add("Gyro angle", 0)
.withWidget(BuiltInWidgets.kGyro)
.getEntry();
.add("Gyro angle", 0)
.withWidget(BuiltInWidgets.kGyro)
.getEntry();
GenericEntry sbShiftState = subsystemLayout
.add("Shift State", 0)
.withWidget(BuiltInWidgets.kNumberBar)
.getEntry();
.add("Shift State", 0)
.withWidget(BuiltInWidgets.kNumberBar)
.getEntry();
@Override
public void queryStatus() {
sbGyro.setDouble(getGyroAngle());
sbShiftState.setDouble(this.speedAdjust);
//TODO: Add more status things
sbGyro.setDouble(getGyroAngle());
sbShiftState.setDouble(this.speedAdjust);
// TODO: Add more status things
}
@Override
public Status diagnosticStatus() {
Status status = new Status();
Status status = new Status();
status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
return status;
status.addReport(ReportLevel.ERROR,
"Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
return status;
}
}
src/main/java/frc4388/robot/subsystems/Vision.java
+56 -42
View File
@@ -3,6 +3,7 @@ package frc4388.robot.subsystems;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.numbers.N1;
@@ -19,6 +20,7 @@ import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import com.ctre.phoenix6.Utils;
@@ -30,6 +32,7 @@ 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 frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
@@ -38,7 +41,8 @@ public class Vision extends Subsystem {
private PhotonCamera camera;
private boolean isTag = false;
private boolean isTagDetected = false;
private boolean isTagProcessed = false;
private Pose2d lastVisionPose = new Pose2d();
private Pose2d lastPhysOdomPose = new Pose2d();
@@ -52,10 +56,15 @@ public class Vision extends Subsystem {
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbTag = subsystemLayout
GenericEntry sbTagDetected = subsystemLayout
.add("Tag Detected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbTagProcessed = subsystemLayout
.add("Tag Processed", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbCamConnected = subsystemLayout
.add("Camera Connnected", false)
@@ -66,47 +75,39 @@ public class Vision extends Subsystem {
this.camera = camera;
SmartDashboard.putData(field);
photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS);
photonEstimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS);
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
}
@Override
public void periodic() {
var result = camera.getLatestResult();
isTag = result.hasTargets();
// var result = camera.getLatestResult();
var results = camera.getAllUnreadResults();
if (results.size() == 0) return;
var result = results.get(results.size()-1);
isTagDetected = result.hasTargets();
isTagProcessed = false;
// Optional<MultiTargetPNPResult> multitag = result.getMultiTagResult();
// if (multitag.isEmpty()) {
// Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best;
// }else if()
// sbTag.setBoolean(isTag);
// field.setRobotPose(getPose2d());
// sbCamConnected.setBoolean(camera);
// System.out.println(isTag);
if(!isTag){
sbTag.setBoolean(isTag);
if(!isTagDetected){
// sbTagDetected.setBoolean(isTagDetected);
field.setRobotPose(getPose2d());
return;
}
var EstimatedRobotPose = getEstimatedGlobalPose();
var EstimatedRobotPose = getEstimatedGlobalPose(result);
// In case the pose estimator fails to estimate the pose, fallback to physical odometry.
if(EstimatedRobotPose.isEmpty()){
isTag = false;
sbTag.setBoolean(isTag);
field.setRobotPose(getPose2d());
return;
}
isTagProcessed = true;
lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
// lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation()));
// lastVisionPose.rotateBy(Rotation2d.k180deg);
// lastVisionPose = new Pose2d(
// lastVisionPose.getTranslation(),
// lastPhysOdomPose.getRotation()
@@ -130,9 +131,22 @@ public class Vision extends Subsystem {
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
* used for estimation.
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change) {
Optional<EstimatedRobotPose> visionEst = Optional.empty();
for (var change : camera.getAllUnreadResults()) {
// for (var change : camera.getAllUnreadResults()) {
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 = photonEstimator.update(change);
updateEstimationStdDevs(visionEst, change.getTargets());
@@ -146,16 +160,11 @@ public class Vision extends Subsystem {
// getSimDebugField().getObject("VisionEstimation").setPoses();
// });
// }
}
// }
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.
@@ -179,13 +188,17 @@ public class Vision extends Subsystem {
for (var tgt : targets) {
var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
if (tagPose.isEmpty()) continue;
numTags++;
avgDist +=
tagPose
.get()
.toPose2d()
.getTranslation()
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
double distance = tagPose
.get()
.toPose2d()
.getTranslation()
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
numTags++;
avgDist += distance;
}
}
if (numTags == 0) {
@@ -229,7 +242,7 @@ public class Vision extends Subsystem {
}
public Pose2d getPose2d() {
if(isTag)
if(isTagDetected && isTagProcessed)
return lastVisionPose;
else
return lastPhysOdomPose;
@@ -240,7 +253,7 @@ public class Vision extends Subsystem {
}
public boolean isTag(){
return isTag;
return isTagDetected && isTagProcessed;
}
@@ -266,7 +279,8 @@ public class Vision extends Subsystem {
@Override
public void queryStatus() {
sbTag.setBoolean(isTag);
sbTagDetected.setBoolean(isTagDetected);
sbTagProcessed.setBoolean(isTagProcessed);
sbCamConnected.setBoolean(camera.isConnected());
// field.setRobotPose(getPose2d());
}