Skip to content
This repository has been archived by the owner on Jan 9, 2025. It is now read-only.

Commit

Permalink
clean swerve
Browse files Browse the repository at this point in the history
  • Loading branch information
@chaoticthegreat
chaoticthegreat committed Nov 6, 2024
1 parent 00378c3 commit 9063506
Showing 1 changed file with 0 additions and 137 deletions.
137 changes: 0 additions & 137 deletions src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,9 @@
import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.mechanisms.swerve.*;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.ReplanningConfig;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
Expand All @@ -36,9 +29,6 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
import frc.robot.subsystems.vision.Vision;
import frc.robot.utils.LimelightHelpers;
import java.util.function.Supplier;

/**
Expand Down Expand Up @@ -124,7 +114,6 @@ public CommandSwerveDrivetrain(
SwerveModuleConstants... modules) {
super(driveTrainConstants, OdometryUpdateFrequency, modules);
this.enabled = enabled;
configurePathPlanner();
if (Utils.isSimulation()) {
startSimThread();
}
Expand All @@ -136,7 +125,6 @@ public CommandSwerveDrivetrain(
SwerveModuleConstants... modules) {
super(driveTrainConstants, modules);
this.enabled = enabled;
configurePathPlanner();
if (Utils.isSimulation()) {
startSimThread();
}
Expand All @@ -159,124 +147,6 @@ public ChassisSpeeds choreoController(Pose2d currentPose, SwerveSample sample) {
currentPose.getRotation());
}

public void updateVision() {
boolean useMegaTag2 = true; // set to false to use MegaTag1
boolean doRejectUpdate = false;
if (useMegaTag2 == false) {
LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");

if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) {
if (mt1.rawFiducials[0].ambiguity > .7) {
doRejectUpdate = true;
}
if (mt1.rawFiducials[0].distToCamera > 3) {
doRejectUpdate = true;
}
}
if (mt1.tagCount == 0) {
doRejectUpdate = true;
}

if (!doRejectUpdate) {
this.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999));
this.addVisionMeasurement(mt1.pose, mt1.timestampSeconds);
}
} else if (useMegaTag2 == true) {
LimelightHelpers.SetRobotOrientation(
"limelight",
this.m_odometry.getEstimatedPosition().getRotation().getDegrees(),
0,
0,
0,
0,
0);
LimelightHelpers.PoseEstimate mt2 =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
if (Math.abs(this.getPigeon2().getRate())
> 720) // if our angular velocity is greater than 720 degrees per
// second, ignore vision
// updates
{
doRejectUpdate = true;
}
if (mt2.tagCount == 0) {
doRejectUpdate = true;
}
if (!doRejectUpdate) {
this.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999));
this.addVisionMeasurement(mt2.pose, mt2.timestampSeconds);
}
}
}

private void configurePathPlanner() {
double driveBaseRadius = 0;
for (var moduleLocation : m_moduleLocations) {
driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm());
}

AutoBuilder.configureHolonomic(
() -> this.getState().Pose, // Supplier of current robot pose
this::seedFieldRelative, // Consumer for seeding pose against auto
this::getCurrentRobotChassisSpeeds,
(speeds) ->
this.setControl(
AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot
new HolonomicPathFollowerConfig(
SwerveConstants.autoTranslationalController,
SwerveConstants.autoRotationalController,
TunerConstants.kSpeedAt12VoltsMps,
driveBaseRadius,
new ReplanningConfig()),
() ->
DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue)
== DriverStation.Alliance.Red, // Assume
// the path
// needs to
// be
// flipped
// for Red
// vs Blue,
// this is
// normally
// the case
this); // Subsystem for requirements
}

public Command pathfindToNote(Vision vision) {
PathConstraints constraints =
new PathConstraints(
TunerConstants.kSpeedAt12VoltsMps - 1, // max speed (4.96)
4, // max acceleration (4 m/s^2)
edu.wpi.first.math.util.Units.degreesToRadians(450), // max angular velocity (450 deg/s)
edu.wpi.first.math.util.Units.degreesToRadians(
540)); // max angular acceleration (540 deg/s^2)
return AutoBuilder.pathfindToPose(
// current pose, path constraints (see above), "goal end velocity", rotation
// delay distance (how long to travel before rotating)
vision.getNotePose(this.getState().Pose), constraints, 1, 0.0);
}

public Command rotationTest() {
PathConstraints constraints =
new PathConstraints(
TunerConstants.kSpeedAt12VoltsMps - 1, // max speed (4.96)
4, // max acceleration (4 m/s^2)
edu.wpi.first.math.util.Units.degreesToRadians(450), // max angular velocity (450 deg/s)
edu.wpi.first.math.util.Units.degreesToRadians(
540)); // max angular acceleration (540 deg/s^2)
return AutoBuilder.pathfindToPose(
// current pose, path constraints (see above), "goal end velocity", rotation
// delay distance (how long to travel before rotating)
this.getState()
.Pose
.rotateBy(Rotation2d.fromDegrees(90))
.plus(new Transform2d(new Translation2d(2, 1), Rotation2d.fromDegrees(0))),
constraints,
0,
0.0);
}

public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
return run(() -> this.setControl(requestSupplier.get()));
}
Expand All @@ -286,10 +156,6 @@ public void resetOdometry(AutoTrajectory traj) {
traj.getInitialPose().get().getRotation(), m_modulePositions, traj.getInitialPose().get());
}

public Command getAutoPath(String pathName) {
return new PathPlannerAuto(pathName);
}

/*
* Both the sysid commands are specific to one particular sysid routine, change
* which one you're trying to characterize
Expand Down Expand Up @@ -438,9 +304,6 @@ public void periodic() {
: BlueAlliancePerspectiveRotation);
hasAppliedOperatorPerspective = true;
});
if (Constants.FeatureFlags.kVisionEnabled) {
updateVision();
}
}
}
}

0 comments on commit 9063506

Please sign in to comment.