Skip to content

Commit

Permalink
Use CTR swerve for localization
Browse files Browse the repository at this point in the history
  • Loading branch information
@jonahsnider
jonahsnider committed Nov 20, 2024
1 parent 5669cd4 commit 5eeb752
Show file tree
Hide file tree
Showing 3 changed files with 47 additions and 147 deletions.
51 changes: 6 additions & 45 deletions src/main/java/frc/robot/localization/LocalizationSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,9 @@
import dev.doglog.DogLog;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
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.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Timer;
Expand All @@ -35,8 +32,6 @@ public class LocalizationSubsystem extends StateMachine<LocalizationState> {
private final ImuSubsystem imu;
private final VisionSubsystem vision;
private final SwerveSubsystem swerve;
private final SwerveDrivePoseEstimator poseEstimator;
private final SwerveDriveOdometry odometry;
private final TimeInterpolatableBuffer<Pose2d> poseHistory =
TimeInterpolatableBuffer.createBuffer(1.5);
private double lastAddedVisionTimestamp = 0;
Expand All @@ -47,17 +42,6 @@ public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision, SwerveSub
this.swerve = swerve;
this.imu = imu;
this.vision = vision;
poseEstimator =
new SwerveDrivePoseEstimator(
SwerveSubsystem.KINEMATICS,
Rotation2d.fromDegrees(imu.getRobotHeading()),
swerve.getModulePositions().toArray(new SwerveModulePosition[4]),
new Pose2d());
odometry =
new SwerveDriveOdometry(
SwerveSubsystem.KINEMATICS,
Rotation2d.fromDegrees(imu.getRobotHeading()),
swerve.getModulePositions().toArray(new SwerveModulePosition[4]));
}

@Override
Expand All @@ -66,24 +50,12 @@ protected void collectInputs() {
}

public Pose2d getPose() {
return poseEstimator.getEstimatedPosition();
}

public Pose2d getOdometryPose() {
return odometry.getPoseMeters();
}

public void resetPose(Pose2d pose) {
resetPose(pose, pose);
return swerve.drivetrain.getState().Pose;
}

@Override
public void robotPeriodic() {
super.robotPeriodic();
SwerveModulePosition[] modulePositions =
swerve.getModulePositions().toArray(new SwerveModulePosition[4]);
odometry.update(Rotation2d.fromDegrees(imu.getRobotHeading()), modulePositions);
poseEstimator.update(Rotation2d.fromDegrees(imu.getRobotHeading()), modulePositions);

for (var results : latestResult) {
Pose2d visionPose = results.pose();
Expand All @@ -93,35 +65,25 @@ public void robotPeriodic() {
if (visionTimestamp == lastAddedVisionTimestamp) {
// Don't add the same vision pose over and over
} else {
poseEstimator.addVisionMeasurement(visionPose, visionTimestamp, VISION_STD_DEVS);
swerve.drivetrain.addVisionMeasurement(visionPose, visionTimestamp, VISION_STD_DEVS);
lastAddedVisionTimestamp = visionTimestamp;
}

poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
poseHistory.addSample(Timer.getFPGATimestamp(), getPose());
}

DogLog.log("Localization/EstimatedPose", getPose());
DogLog.log("Localization/OdometryPose", getOdometryPose());
}

private void resetGyro(double gyroAngle) {
Pose2d estimatedPose =
new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(gyroAngle));
Pose2d odometryPose =
new Pose2d(getOdometryPose().getTranslation(), Rotation2d.fromDegrees(gyroAngle));
resetPose(estimatedPose, odometryPose);
resetPose(estimatedPose);
}

public void resetPose(Pose2d estimatedPose, Pose2d odometryPose) {
public void resetPose(Pose2d estimatedPose) {
imu.setAngle(estimatedPose.getRotation().getDegrees());
poseEstimator.resetPosition(
estimatedPose.getRotation(),
swerve.getModulePositions().toArray(new SwerveModulePosition[4]),
estimatedPose);
odometry.resetPosition(
odometryPose.getRotation(),
swerve.getModulePositions().toArray(new SwerveModulePosition[4]),
odometryPose);
swerve.drivetrain.seedFieldRelative(estimatedPose);
}

public Command getZeroCommand() {
Expand All @@ -133,7 +95,6 @@ public Command getZeroCommand() {
* target pose.
*/
public DistanceAngle getFieldRelativeDistanceAngleToPose(Pose2d target) {

DistanceAngle distanceAngleToSpeaker = distanceAngleToTarget(target, getPose());

return distanceAngleToSpeaker;
Expand Down
72 changes: 0 additions & 72 deletions src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java
View file

This file was deleted.

71 changes: 41 additions & 30 deletions src/main/java/frc/robot/swerve/SwerveSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,23 @@
package frc.robot.swerve;

import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.TorqueCurrentConfigs;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import com.ctre.phoenix6.signals.InvertedValue;
import dev.doglog.DogLog;
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.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import frc.robot.config.RobotConfig;
import frc.robot.fms.FmsSubsystem;
import frc.robot.generated.TunerConstants;
Expand All @@ -40,25 +42,7 @@ public class SwerveSubsystem extends StateMachine<SwerveState> {
private static final double rightXDeadband = 0.15;
private static final double leftYDeadband = 0.05;

public static final Translation2d FRONT_LEFT_LOCATION =
new Translation2d(
CommandSwerveDrivetrain.FrontLeft.LocationX, CommandSwerveDrivetrain.FrontLeft.LocationY);
public static final Translation2d FRONT_RIGHT_LOCATION =
new Translation2d(
CommandSwerveDrivetrain.FrontRight.LocationX,
CommandSwerveDrivetrain.FrontRight.LocationY);
public static final Translation2d BACK_LEFT_LOCATION =
new Translation2d(
CommandSwerveDrivetrain.BackLeft.LocationX, CommandSwerveDrivetrain.BackLeft.LocationY);
public static final Translation2d BACK_RIGHT_LOCATION =
new Translation2d(
CommandSwerveDrivetrain.BackRight.LocationX, CommandSwerveDrivetrain.BackRight.LocationY);
public static final Translation2d[] MODULE_LOCATIONS =
new Translation2d[] {
FRONT_LEFT_LOCATION, FRONT_RIGHT_LOCATION, BACK_LEFT_LOCATION, BACK_RIGHT_LOCATION
};
public static final SwerveDriveKinematics KINEMATICS =
new SwerveDriveKinematics(MODULE_LOCATIONS);
private static final double SIM_LOOP_PERIOD = 0.005; // 5 ms

private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber) {
return switch (moduleNumber) {
Expand All @@ -70,7 +54,13 @@ private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber)
};
}

private final CommandSwerveDrivetrain drivetrain = new CommandSwerveDrivetrain();
public final SwerveDrivetrain drivetrain =
new SwerveDrivetrain(
TunerConstants.DrivetrainConstants,
TunerConstants.FrontLeft,
TunerConstants.FrontRight,
TunerConstants.BackLeft,
TunerConstants.BackRight);

public final Pigeon2 drivetrainPigeon = drivetrain.getPigeon2();

Expand All @@ -90,6 +80,9 @@ private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber)
private final SwerveModule backLeft = drivetrain.getModule(2);
private final SwerveModule backRight = drivetrain.getModule(3);

private double lastSimTime;
private Notifier simNotifier = null;

private boolean slowEnoughToShoot = false;
private List<SwerveModulePosition> modulePositions;
private ChassisSpeeds robotRelativeSpeeds;
Expand All @@ -103,10 +96,6 @@ private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber)

private ChassisSpeeds autoSpeeds = new ChassisSpeeds();

private ChassisSpeeds intakeAssistTeleopSpeeds = new ChassisSpeeds();

private ChassisSpeeds intakeAssistAutoSpeeds = new ChassisSpeeds();

public ChassisSpeeds getRobotRelativeSpeeds() {
return robotRelativeSpeeds;
}
Expand Down Expand Up @@ -183,6 +172,10 @@ public SwerveSubsystem() {
: InvertedValue.CounterClockwise_Positive;
steerMotorConfigurator.apply(steerMotorOutput);
}

if (Utils.isSimulation()) {
startSimThread();
}
}

public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) {
Expand Down Expand Up @@ -263,12 +256,12 @@ private List<SwerveModulePosition> calculateModulePositions() {
}

private ChassisSpeeds calculateRobotRelativeSpeeds() {
return KINEMATICS.toChassisSpeeds(drivetrain.getState().ModuleStates);
return drivetrain.getState().speeds;
}

private ChassisSpeeds calculateFieldRelativeSpeeds() {
return ChassisSpeeds.fromRobotRelativeSpeeds(
robotRelativeSpeeds, Rotation2d.fromDegrees(drivetrainPigeon.getYaw().getValueAsDouble()));
robotRelativeSpeeds, drivetrain.getState().Pose.getRotation());
}

private static boolean calculateMovingSlowEnoughForSpeakerShot(ChassisSpeeds speeds) {
Expand Down Expand Up @@ -313,7 +306,7 @@ private void sendSwerveRequest() {
}
}
case INTAKE_ASSIST_TELEOP -> {
intakeAssistTeleopSpeeds =
var intakeAssistTeleopSpeeds =
IntakeAssistManager.getRobotRelativeAssistSpeeds(0, teleopSpeeds);
/// fix robotHeading

Expand All @@ -340,7 +333,8 @@ private void sendSwerveRequest() {
.withDriveRequestType(DriveRequestType.Velocity));

case INTAKE_ASSIST_AUTO -> {
intakeAssistAutoSpeeds = IntakeAssistManager.getRobotRelativeAssistSpeeds(0, autoSpeeds);
var intakeAssistAutoSpeeds =
IntakeAssistManager.getRobotRelativeAssistSpeeds(0, autoSpeeds);
/// fix robotHeading
drivetrain.setControl(
drive
Expand Down Expand Up @@ -371,4 +365,21 @@ public void robotPeriodic() {

DogLog.log("Swerve/SnapAngle", goalSnapAngle);
}

private void startSimThread() {
lastSimTime = Utils.getCurrentTimeSeconds();

/* Run simulation at a faster rate so PID gains behave more reasonably */
simNotifier =
new Notifier(
() -> {
final double currentTime = Utils.getCurrentTimeSeconds();
double deltaTime = currentTime - lastSimTime;
lastSimTime = currentTime;

/* use the measured time delta, get battery voltage from WPILib */
drivetrain.updateSimState(deltaTime, RobotController.getBatteryVoltage());
});
simNotifier.startPeriodic(SIM_LOOP_PERIOD);
}
}

0 comments on commit 5eeb752

Please sign in to comment.