Skip to content

attempt to adapt vision to new drivetrain code #15

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Eddie-Edward merged 2 commits into from
Mar 5, 2025
Merged

attempt to adapt vision to new drivetrain code #15

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2bcc96b
attempt at some VIsion changes
joelzoto Mar 5, 2025
c6527f1
Merge branch 'main' of https://github.com/joelzoto/2025RobotCode
joelzoto Mar 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest;
Expand Down Expand Up @@ -36,6 +37,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su

public static CommandSwerveDrivetrain mInstance;

public static Pigeon2 mGyro = new Pigeon2(TunerConstants.kPigeonId, "Canivore");

/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
Expand Down
9 changes: 8 additions & 1 deletion src/main/java/frc/robot/drivetrain/TunerConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@

import static edu.wpi.first.units.Units.*;

import java.util.function.Supplier;

import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.configs.*;
import com.ctre.phoenix6.hardware.*;
Expand All @@ -10,11 +12,15 @@
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.measure.*;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import frc.robot.drivetrain.CommandSwerveDrivetrain;
import frc.robot.vision.Vision;
import frc.robot.vision.VisionConfig;

import com.ctre.phoenix6.swerve.SwerveDrivetrain.DeviceConstructor.*;

// Generated by the Tuner X Swerve Project Generator
Expand Down Expand Up @@ -99,7 +105,8 @@ public class TunerConstants {
private static final boolean kInvertLeftSide = false;
private static final boolean kInvertRightSide = true;

private static final int kPigeonId = 18;
public static final int kPigeonId = 18;


// These are only used for simulation
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
Expand Down
121 changes: 64 additions & 57 deletions src/main/java/frc/robot/vision/SwerveVisionEstimator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,84 +6,91 @@
import java.util.Arrays;
import java.util.function.Supplier;

import com.ctre.phoenix6.swerve.SwerveRequest;

import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.drivetrain.CommandSwerveDrivetrain;


public class SwerveVisionEstimator extends SubsystemBase{

// private final Vision[] visions;
// private static SwerveVisionEstimator mInstance;
// private final Supplier<SwerveModulePosition[]> modSupplier;
// private final Supplier<Rotation2d> rotationSupplier;
// private final SwerveDrivePoseEstimator poseEstimator;
// private final Field2d field2d = new Field2d();
private final Vision[] visions;
private static SwerveVisionEstimator mInstance;
private final Supplier<SwerveModulePosition[]> modSupplier;
private final Supplier<Rotation2d> rotationSupplier;
private final SwerveDrivePoseEstimator poseEstimator;
private final Field2d field2d = new Field2d();


// public SwerveVisionEstimator(
// Supplier<Rotation2d> rotSup, Supplier<SwerveModulePosition[]> modSup){
// this.modSupplier = modSup;
// this.rotationSupplier = rotSup;
public SwerveVisionEstimator(
Supplier<Rotation2d> rotSup, Supplier<SwerveModulePosition[]> modSup){
this.modSupplier = modSup;
this.rotationSupplier = rotSup;

// poseEstimator = new SwerveDrivePoseEstimator(
// DriveConstants.swerveKinematics,
// rotationSupplier.get(),
// modSupplier.get(),
// new Pose2d());
poseEstimator = new SwerveDrivePoseEstimator(
//DriveConstants.swerveKinematics
null,
rotationSupplier.get(),
modSupplier.get(),
new Pose2d());

// visions = new Vision[camNames.length];
// for (int i = 0; i < camNames.length; i++){
// visions[i] = new Vision(camNames[i], robotToCamTransforms[i]);
// }
// }
visions = new Vision[camNames.length];
for (int i = 0; i < camNames.length; i++){
visions[i] = new Vision(camNames[i], robotToCamTransforms[i]);
}
}

// public static SwerveVisionEstimator getInstance(){
// if (mInstance == null){
// final var drivetrain = Drivetrain.getInstance();
// mInstance = new SwerveVisionEstimator(drivetrain::getGyroYaw, drivetrain::getModulePositions);
// }
// return mInstance;
// }
public static SwerveVisionEstimator getInstance(){
if (mInstance == null){
final var drivetrain = CommandSwerveDrivetrain.getInstance();
//mInstance = new SwerveVisionEstimator(drivetrain::getGyroYaw, drivetrain::getModulePositions);
mInstance = new SwerveVisionEstimator(() -> Rotation2d.fromDegrees(CommandSwerveDrivetrain.mGyro.getYaw().getValueAsDouble()), null);
}
return mInstance;
}

// public Pose2d getCurrrentPose(){
// return poseEstimator.getEstimatedPosition();
// }
public Pose2d getCurrrentPose(){
return poseEstimator.getEstimatedPosition();
}

// public void estimatorUpdate(){
// Arrays.stream(visions).forEach(vision -> {
// var visionEst = vision.getEstimatedGlobalPose();
public void estimatorUpdate(){
Arrays.stream(visions).forEach(vision -> {
var visionEst = vision.getEstimatedGlobalPose();

// visionEst.ifPresent(est -> {
// var estStdDevs = vision.getEstimationStdDevs();
visionEst.ifPresent(est -> {
var estStdDevs = vision.getEstimationStdDevs();

// poseEstimator.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
// });
// });
// }
poseEstimator.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
});
});
}

// private String getFormattedPose(){
// var pose = getCurrrentPose();
// return String.format("(%.3f, %.3f) %.2f degrees",
// pose.getX(),
// pose.getY(),
// pose.getRotation().getDegrees());
// }
private String getFormattedPose(){
var pose = getCurrrentPose();
return String.format("(%.3f, %.3f) %.2f degrees",
pose.getX(),
pose.getY(),
pose.getRotation().getDegrees());
}

// @Override
// public void periodic(){
// // //Update swerve pose estimator using drivetrain
// // poseEstimator.update(rotationSupplier.get(), modSupplier.get());
// // //Add vision measurements to swerve pose estimator
// // estimatorUpdate();
@Override
public void periodic(){
// //Update swerve pose estimator using drivetrain
poseEstimator.update(rotationSupplier.get(), modSupplier.get());
//Add vision measurements to swerve pose estimator
estimatorUpdate();

// // //log to dashboard
// // var dashboardPose = poseEstimator.getEstimatedPosition();
// // SmartDashboard.putString("Estimated Pose", getFormattedPose());
// // SmartDashboard.putData(field2d);
// // field2d.setRobotPose(getCurrrentPose());
//log to dashboard
var dashboardPose = poseEstimator.getEstimatedPosition();
SmartDashboard.putString("Estimated Pose", getFormattedPose());
SmartDashboard.putData(field2d);
field2d.setRobotPose(getCurrrentPose());
// }
}
}