Skip to content

Swerve Vision Estimator Integration Attempt #16

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 6, 2025
Merged

Swerve Vision Estimator Integration Attempt #16

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
72499af
Swerve Vision Estimator Integration Attempt
joelzoto Mar 6, 2025
1d13faf
Merge branch 'CrevolutionRoboticsProgramming:main' into main
joelzoto Mar 6, 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
8 changes: 8 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 @@ -7,6 +7,7 @@
import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest;
Expand All @@ -19,6 +20,7 @@
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -40,6 +42,12 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su

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

public static TalonFX FrontLeftSteerMotor = new TalonFX(TunerConstants.kFrontLeftSteerMotorId, "Canbus");
public static TalonFX FrontRightSteerMotor = new TalonFX(TunerConstants.kFrontRightSteerMotorId, "Canbus");
public static TalonFX BackLeftSteerMotor = new TalonFX(TunerConstants.kBackLeftSteerMotorId, "Canbus");
public static TalonFX BackRightSteerMotor = new TalonFX(TunerConstants.kBackRightSteerMotorId, "Canbus");


/* 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
18 changes: 13 additions & 5 deletions 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 @@ -12,11 +12,13 @@
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Rotation2d;
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.crevolib.math.Conversions;
import frc.robot.drivetrain.CommandSwerveDrivetrain;
import frc.robot.vision.Vision;
import frc.robot.vision.VisionConfig;
Expand Down Expand Up @@ -107,7 +109,6 @@ public class TunerConstants {

public static final int kPigeonId = 18;


// These are only used for simulation
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
Expand Down Expand Up @@ -146,7 +147,7 @@ public class TunerConstants {

// Front Left
private static final int kFrontLeftDriveMotorId = 1;
private static final int kFrontLeftSteerMotorId = 5;
public static final int kFrontLeftSteerMotorId = 5;
private static final int kFrontLeftEncoderId = 19;
private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.384765625);
private static final boolean kFrontLeftSteerMotorInverted = false;
Expand All @@ -157,7 +158,7 @@ public class TunerConstants {

// Front Right
private static final int kFrontRightDriveMotorId = 2;
private static final int kFrontRightSteerMotorId = 6;
public static final int kFrontRightSteerMotorId = 6;
private static final int kFrontRightEncoderId = 20;
private static final Angle kFrontRightEncoderOffset = Rotations.of(0.110107421875);
private static final boolean kFrontRightSteerMotorInverted = false;
Expand All @@ -168,7 +169,7 @@ public class TunerConstants {

// Back Left
private static final int kBackLeftDriveMotorId = 3;
private static final int kBackLeftSteerMotorId = 7;
public static final int kBackLeftSteerMotorId = 7;
private static final int kBackLeftEncoderId = 21;
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.086181640625);
private static final boolean kBackLeftSteerMotorInverted = false;
Expand All @@ -179,7 +180,7 @@ public class TunerConstants {

// Back Right
private static final int kBackRightDriveMotorId = 4;
private static final int kBackRightSteerMotorId = 8;
public static final int kBackRightSteerMotorId = 8;
private static final int kBackRightEncoderId = 22;
private static final Angle kBackRightEncoderOffset = Rotations.of(0.274658203125);
private static final boolean kBackRightSteerMotorInverted = false;
Expand Down Expand Up @@ -210,6 +211,7 @@ public class TunerConstants {
kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted
);


/**
* Creates a CommandSwerveDrivetrain instance.
* This should only be called once in your robot program,.
Expand All @@ -220,6 +222,12 @@ public static CommandSwerveDrivetrain createDrivetrain() {
);
}

public static final SwerveModulePosition[] K_SWERVE_MODULE_POSITIONS = new SwerveModulePosition[]{
new SwerveModulePosition(kFrontLeftXPos, Rotation2d.fromDegrees(CommandSwerveDrivetrain.FrontLeftSteerMotor.getPosition().getValueAsDouble())),
new SwerveModulePosition(kFrontRightXPos, Rotation2d.fromDegrees(CommandSwerveDrivetrain.FrontRightSteerMotor.getPosition().getValueAsDouble())),
new SwerveModulePosition(kBackLeftXPos, Rotation2d.fromDegrees(CommandSwerveDrivetrain.BackLeftSteerMotor.getPosition().getValueAsDouble())),
new SwerveModulePosition(kBackRightXPos, Rotation2d.fromDegrees(CommandSwerveDrivetrain.BackRightSteerMotor.getPosition().getValueAsDouble()))
};

/**
* Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
Expand Down
5 changes: 4 additions & 1 deletion 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 @@ -16,6 +16,7 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.drivetrain.CommandSwerveDrivetrain;
import frc.robot.drivetrain.TunerConstants;


public class SwerveVisionEstimator extends SubsystemBase{
Expand Down Expand Up @@ -50,7 +51,9 @@ 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);
mInstance = new SwerveVisionEstimator(() ->
Rotation2d.fromDegrees(drivetrain.mGyro.getYaw().getValueAsDouble()),
()-> TunerConstants.K_SWERVE_MODULE_POSITIONS);
}
return mInstance;
}
Expand Down