From 2b8650ad3c14b3c9e71b446d30118173d97fee5b Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Tue, 23 Jan 2024 08:59:18 -0500 Subject: [PATCH 01/15] initial drivetrain implementation. includes swerve teleop control and static path following --- gradlew | 0 .../org/robolancers321/BuildConstants.java | 12 +- .../org/robolancers321/RobotContainer.java | 42 ++- .../commands/drivetrain/DriveToTarget.java | 111 ++++++ .../commands/drivetrain/FeedForwardDrive.java | 60 ++++ .../commands/drivetrain/TeleopDrive.java | 22 ++ .../drivetrain/TuneDriveToTarget.java | 150 ++++++++ .../commands/drivetrain/TuneModules.java | 48 +++ .../subsystems/drivetrain/Drivetrain.java | 250 +++++++++++++ .../subsystems/drivetrain/SwerveModule.java | 237 ++++++++++++ vendordeps/NavX.json | 40 +++ vendordeps/PathplannerLib.json | 38 ++ vendordeps/Phoenix5.json | 151 ++++++++ vendordeps/Phoenix6.json | 339 ++++++++++++++++++ vendordeps/REVLib.json | 74 ++++ vendordeps/photonlib.json | 57 +++ 16 files changed, 1621 insertions(+), 10 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/java/org/robolancers321/commands/drivetrain/DriveToTarget.java create mode 100644 src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java create mode 100644 src/main/java/org/robolancers321/commands/drivetrain/TeleopDrive.java create mode 100644 src/main/java/org/robolancers321/commands/drivetrain/TuneDriveToTarget.java create mode 100644 src/main/java/org/robolancers321/commands/drivetrain/TuneModules.java create mode 100644 src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java create mode 100644 src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java create mode 100644 vendordeps/NavX.json create mode 100644 vendordeps/PathplannerLib.json create mode 100644 vendordeps/Phoenix5.json create mode 100644 vendordeps/Phoenix6.json create mode 100644 vendordeps/REVLib.json create mode 100644 vendordeps/photonlib.json diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index c52be06..66ad716 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 4; - public static final String GIT_SHA = "890b1f13f053a4a882dc48c8de3887bfe2f9294f"; - public static final String GIT_DATE = "2024-01-18 19:40:03 EST"; - public static final String GIT_BRANCH = "dev"; - public static final String BUILD_DATE = "2024-01-18 19:43:26 EST"; - public static final long BUILD_UNIX_TIME = 1705625006821L; + public static final int GIT_REVISION = 6; + public static final String GIT_SHA = "6f27170a2cbfcc3e8a09f9dca94b598405cb30ef"; + public static final String GIT_DATE = "2024-01-18 19:44:33 EST"; + public static final String GIT_BRANCH = "dev-drivetrain"; + public static final String BUILD_DATE = "2024-01-23 08:57:30 EST"; + public static final long BUILD_UNIX_TIME = 1706018250153L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 55b784c..ddd92ae 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -1,17 +1,51 @@ /* (C) Robolancers 2024 */ package org.robolancers321; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import org.robolancers321.commands.drivetrain.TeleopDrive; +import org.robolancers321.subsystems.drivetrain.Drivetrain; public class RobotContainer { + Drivetrain drivetrain; + + XboxController controller; + + SendableChooser autoChooser; + public RobotContainer() { - configureBindings(); + this.drivetrain = Drivetrain.getInstance(); + + this.controller = new XboxController(0); + + this.autoChooser = AutoBuilder.buildAutoChooser(); + + this.configureBindings(); + this.configureAutoChooser(); } - private void configureBindings() {} + private void configureBindings() { + this.drivetrain.setDefaultCommand(new TeleopDrive(this.controller)); + + new Trigger(this.controller::getAButton).onTrue(new InstantCommand(this.drivetrain::zeroYaw)); + } + + private void configureAutoChooser() { + NamedCommands.registerCommand("Say Hello", new PrintCommand("Hello")); + + this.autoChooser.addOption("Do Nothing", new InstantCommand()); + } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return new PathPlannerAuto("Example Auto"); + + // return this.autoChooser.getSelected(); } } diff --git a/src/main/java/org/robolancers321/commands/drivetrain/DriveToTarget.java b/src/main/java/org/robolancers321/commands/drivetrain/DriveToTarget.java new file mode 100644 index 0000000..cf9bacc --- /dev/null +++ b/src/main/java/org/robolancers321/commands/drivetrain/DriveToTarget.java @@ -0,0 +1,111 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.drivetrain; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.robolancers321.subsystems.drivetrain.Drivetrain; + +public class DriveToTarget extends Command { + /* + * Constants + */ + + private static final double kTranslationP = 0.0; + private static final double kTranslationI = 0.0; + private static final double kTranslationD = 0.0; + + private static final double kRotationP = 0.0; + private static final double kRotationI = 0.0; + private static final double kRotationD = 0.0; + + /* + * Implementation + */ + + Drivetrain drivetrain; + PhotonCamera camera; + + DoubleSupplier desiredDxSupplier; + DoubleSupplier desiredDzSupplier; + DoubleSupplier desiredDThetaSupplier; + + PIDController xController; + PIDController zController; + PIDController thetaController; + + public DriveToTarget( + DoubleSupplier desiredDxSupplier, + DoubleSupplier desiredDzSupplier, + DoubleSupplier desiredDThetaSupplier) { + this.drivetrain = Drivetrain.getInstance(); + this.camera = new PhotonCamera("photonvision"); + + this.desiredDxSupplier = desiredDxSupplier; + this.desiredDzSupplier = desiredDzSupplier; + this.desiredDThetaSupplier = desiredDThetaSupplier; + + this.xController = new PIDController(kTranslationP, kTranslationI, kTranslationD); + this.zController = new PIDController(kTranslationP, kTranslationI, kTranslationD); + this.thetaController = new PIDController(kRotationP, kRotationI, kRotationD); + + this.addRequirements(this.drivetrain); + } + + public DriveToTarget(double desiredDx, double desiredDz, double desiredTheta) { + this(() -> desiredDx, () -> desiredDz, () -> desiredTheta); + } + + @Override + public void execute() { + double desiredDx = this.desiredDxSupplier.getAsDouble(); + double desiredDz = this.desiredDzSupplier.getAsDouble(); + double desiredDTheta = this.desiredDThetaSupplier.getAsDouble(); + + this.xController.setSetpoint(desiredDx); + this.zController.setSetpoint(desiredDz); + this.thetaController.setSetpoint(desiredDTheta); + + double actualDx; + double actualDz; + double actualDTheta; + + PhotonPipelineResult visionResults = this.camera.getLatestResult(); + + SmartDashboard.putBoolean("has vision target", visionResults.hasTargets()); + + if (visionResults.hasTargets()) { + PhotonTrackedTarget bestTarget = visionResults.getBestTarget(); + + Transform3d relativeCameraPosition = bestTarget.getBestCameraToTarget(); + + actualDx = relativeCameraPosition.getX(); + actualDz = relativeCameraPosition.getZ(); + actualDTheta = relativeCameraPosition.getRotation().getY(); + } else { + actualDx = desiredDx; + actualDz = desiredDz; + actualDTheta = desiredDTheta; + } + + SmartDashboard.putNumber("actual target dx", actualDx); + SmartDashboard.putNumber("actual target dz", actualDz); + SmartDashboard.putNumber("actual target dtheta", actualDTheta); + + double xControllerOutput = this.xController.calculate(actualDx); + double zControllerOutput = this.zController.calculate(actualDz); + double thetaControllerOutput = this.thetaController.calculate(actualDTheta); + + SmartDashboard.putNumber("x controller output", xControllerOutput); + SmartDashboard.putNumber("z controller output", zControllerOutput); + SmartDashboard.putNumber("theta controller output", thetaControllerOutput); + + // TODO: check vision outputs before actually calling drive + // this.drivetrain.drive(zControllerOutput, xControllerOutput, thetaControllerOutput, false); + } +} diff --git a/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java b/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java new file mode 100644 index 0000000..3054e08 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java @@ -0,0 +1,60 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.drivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import org.robolancers321.subsystems.drivetrain.Drivetrain; + +public class FeedForwardDrive extends Command { + private Drivetrain drivetrain; + + private DoubleSupplier throttleSupplier; + private DoubleSupplier strafeSupplier; + private DoubleSupplier omegaSupplier; + private BooleanSupplier fieldCentricSupplier; + + public FeedForwardDrive( + DoubleSupplier throttleSupplier, + DoubleSupplier strafeSupplier, + DoubleSupplier omegaSupplier, + BooleanSupplier fieldCentricSupplier) { + this.drivetrain = Drivetrain.getInstance(); + + this.throttleSupplier = throttleSupplier; + this.strafeSupplier = strafeSupplier; + this.omegaSupplier = omegaSupplier; + this.fieldCentricSupplier = fieldCentricSupplier; + + this.addRequirements(this.drivetrain); + } + + public FeedForwardDrive(double throttle, double strafe, double omega, boolean fieldCentric) { + this(() -> throttle, () -> strafe, () -> omega, () -> fieldCentric); + } + + @Override + public void execute() { + double desiredThrottle = this.throttleSupplier.getAsDouble(); + double desiredStrafe = this.strafeSupplier.getAsDouble(); + double desiredOmega = this.omegaSupplier.getAsDouble(); + boolean desiredFieldCentic = this.fieldCentricSupplier.getAsBoolean(); + + // TODO: any preprocessing for desired input should be done here + desiredThrottle *= 1.2; + desiredStrafe *= 1.2; + desiredOmega *= -3.0; + + this.drivetrain.drive(desiredThrottle, desiredStrafe, desiredOmega, desiredFieldCentic); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + this.drivetrain.drive(0, 0, 0, false); + } +} diff --git a/src/main/java/org/robolancers321/commands/drivetrain/TeleopDrive.java b/src/main/java/org/robolancers321/commands/drivetrain/TeleopDrive.java new file mode 100644 index 0000000..6b059c4 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/drivetrain/TeleopDrive.java @@ -0,0 +1,22 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.XboxController; + +public class TeleopDrive extends FeedForwardDrive { + private static final double kTeleopMaxTranslationalSpeed = 2.5; + private static final double kTeleopMaxRotationalSpeed = 1.0; + + public TeleopDrive(XboxController controller, boolean fieldCentric) { + super( + () -> kTeleopMaxTranslationalSpeed * MathUtil.applyDeadband(controller.getLeftY(), 0.2), + () -> -kTeleopMaxTranslationalSpeed * MathUtil.applyDeadband(controller.getLeftX(), 0.2), + () -> kTeleopMaxRotationalSpeed * MathUtil.applyDeadband(controller.getRightX(), 0.2), + () -> fieldCentric); + } + + public TeleopDrive(XboxController controller) { + this(controller, true); + } +} diff --git a/src/main/java/org/robolancers321/commands/drivetrain/TuneDriveToTarget.java b/src/main/java/org/robolancers321/commands/drivetrain/TuneDriveToTarget.java new file mode 100644 index 0000000..9375400 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/drivetrain/TuneDriveToTarget.java @@ -0,0 +1,150 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.robolancers321.subsystems.drivetrain.Drivetrain; + +public class TuneDriveToTarget extends Command { + /* + * Constants + */ + + private static final double kTranslationP = 0.4; + private static final double kTranslationI = 0.0; + private static final double kTranslationD = 0.0; + + private static final double kRotationP = 0.3; + private static final double kRotationI = 0.0; + private static final double kRotationD = 0.0; + + /* + * Implementation + */ + + Drivetrain drivetrain; + PhotonCamera camera; + + DoubleSupplier desiredDxSupplier; + DoubleSupplier desiredDzSupplier; + DoubleSupplier desiredDThetaSupplier; + + PIDController positionController; + PIDController thetaController; + + public TuneDriveToTarget( + DoubleSupplier desiredDxSupplier, + DoubleSupplier desiredDzSupplier, + DoubleSupplier desiredDThetaSupplier) { + this.drivetrain = Drivetrain.getInstance(); + this.camera = new PhotonCamera("camera"); + + this.desiredDxSupplier = desiredDxSupplier; + this.desiredDzSupplier = desiredDzSupplier; + this.desiredDThetaSupplier = desiredDThetaSupplier; + + this.positionController = new PIDController(kTranslationP, kTranslationI, kTranslationD); + this.thetaController = new PIDController(kRotationP, kRotationI, kRotationD); + + SmartDashboard.putNumber( + "translation controller kp", + SmartDashboard.getNumber("translation controller kp", kTranslationP)); + SmartDashboard.putNumber( + "translation controller ki", + SmartDashboard.getNumber("translation controller ki", kTranslationI)); + SmartDashboard.putNumber( + "translation controller kd", + SmartDashboard.getNumber("translation controller kd", kTranslationD)); + + SmartDashboard.putNumber( + "rotation controller kp", SmartDashboard.getNumber("rotation controller kp", kRotationP)); + SmartDashboard.putNumber( + "rotation controller ki", SmartDashboard.getNumber("rotation controller ki", kRotationI)); + SmartDashboard.putNumber( + "rotation controller kd", SmartDashboard.getNumber("rotation controller kd", kRotationD)); + + this.addRequirements(this.drivetrain); + } + + public TuneDriveToTarget(double desiredDx, double desiredDz, double desiredTheta) { + this(() -> desiredDx, () -> desiredDz, () -> desiredTheta); + } + + @Override + public void execute() { + double tunedTranslationP = SmartDashboard.getNumber("translation controller kp", kTranslationP); + double tunedTranslationI = SmartDashboard.getNumber("translation controller ki", kTranslationI); + double tunedTranslationD = SmartDashboard.getNumber("translation controller kd", kTranslationD); + + this.positionController.setPID(tunedTranslationP, tunedTranslationI, tunedTranslationD); + + double tunedRotationP = SmartDashboard.getNumber("rotation controller kp", kRotationP); + double tunedRotationI = SmartDashboard.getNumber("rotation controller ki", kRotationI); + double tunedRotationD = SmartDashboard.getNumber("rotation controller kd", kRotationD); + + this.thetaController.setPID(tunedRotationP, tunedRotationI, tunedRotationD); + + double desiredDx = this.desiredDxSupplier.getAsDouble(); + double desiredDz = this.desiredDzSupplier.getAsDouble(); + double desiredDTheta = this.desiredDThetaSupplier.getAsDouble(); + + this.positionController.setSetpoint(0.0); + this.thetaController.setSetpoint(desiredDTheta); + + double actualDx; + double actualDz; + double actualDTheta; + + PhotonPipelineResult visionResults = this.camera.getLatestResult(); + + SmartDashboard.putBoolean("has vision target", visionResults.hasTargets()); + + if (visionResults.hasTargets()) { + PhotonTrackedTarget bestTarget = visionResults.getBestTarget(); + + Transform3d relativeCameraPosition = bestTarget.getBestCameraToTarget(); + + // goofy ah coordinate system + // these are from robot looking at tag + actualDx = -relativeCameraPosition.getY(); + actualDz = relativeCameraPosition.getX(); + actualDTheta = relativeCameraPosition.getRotation().getX() * 180.0 / Math.PI; + + } else { + actualDx = desiredDx; + actualDz = desiredDz; + actualDTheta = desiredDTheta; + } + + SmartDashboard.putNumber("actual target dx", actualDx); + SmartDashboard.putNumber("actual target dz", actualDz); + SmartDashboard.putNumber("actual target dtheta", actualDTheta); + + double errorX = desiredDx - actualDx; + double errorZ = desiredDz - actualDz; + double errorPosition = Math.hypot(errorX, errorZ); + + double positionControllerOutput = + MathUtil.clamp(this.positionController.calculate(errorPosition), -0.3, 0.3); + double thetaControllerOutput = + MathUtil.clamp(this.thetaController.calculate(actualDTheta), -0.3, 0.3); + + SmartDashboard.putNumber("position controller output", positionControllerOutput); + SmartDashboard.putNumber("theta controller output", thetaControllerOutput); + + double xComponentOfError = errorX / errorPosition; + double zComponentOfError = errorZ / errorPosition; + + double outputX = MathUtil.clamp(xComponentOfError * positionControllerOutput, -0.3, 0.3); + double outputZ = MathUtil.clamp(zComponentOfError * positionControllerOutput, -0.3, 0.3); + + this.drivetrain.drive(outputX, outputZ, thetaControllerOutput, false); + } +} diff --git a/src/main/java/org/robolancers321/commands/drivetrain/TuneModules.java b/src/main/java/org/robolancers321/commands/drivetrain/TuneModules.java new file mode 100644 index 0000000..8a60fef --- /dev/null +++ b/src/main/java/org/robolancers321/commands/drivetrain/TuneModules.java @@ -0,0 +1,48 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.drivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.drivetrain.SwerveModule; + +public class TuneModules extends Command { + private Drivetrain drivetrain; + + public TuneModules() { + this.drivetrain = Drivetrain.getInstance(); + + SwerveModule.initTuning(); + + SmartDashboard.putNumber("target module angle (deg)", 0.0); + SmartDashboard.putNumber("target module vel (m/s)", 0.0); + + this.addRequirements(this.drivetrain); + } + + @Override + public void execute() { + double theta = SmartDashboard.getNumber("target module angle (deg)", 0.0); + double vel = SmartDashboard.getNumber("target module vel (m/s)", 0.0); + + SwerveModuleState[] states = new SwerveModuleState[4]; + + for (int i = 0; i < 4; i++) { + states[i] = new SwerveModuleState(vel, Rotation2d.fromDegrees(theta)); + } + + this.drivetrain.tuneDrive(states); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + this.drivetrain.drive(0.0, 0.0, 0.0, false); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java new file mode 100644 index 0000000..088b403 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -0,0 +1,250 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.drivetrain; + +import com.kauailabs.navx.frc.AHRS; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +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.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Drivetrain extends SubsystemBase { + /* + * Singleton + */ + + private static Drivetrain instance = null; + + public static Drivetrain getInstance() { + if (instance == null) instance = new Drivetrain(); + + return instance; + } + + /* + * Constants + */ + + private static final double kTrackWidthMeters = Units.inchesToMeters(17.5); + private static final double kWheelBaseMeters = Units.inchesToMeters(17.5); + + private static final double kMaxSpeedMetersPerSecond = 4.0; + private static final double kMaxOmegaRadiansPerSecond = 1.5 * Math.PI; + + private static final PathConstraints kAutoConstraints = + new PathConstraints(3.0, 3.0, 540 * Math.PI / 180, 720 * Math.PI / 180); + + private static final SwerveDriveKinematics kSwerveKinematics = + new SwerveDriveKinematics( + new Translation2d(0.5 * kTrackWidthMeters, 0.5 * kWheelBaseMeters), // front left + new Translation2d(0.5 * kTrackWidthMeters, -0.5 * kWheelBaseMeters), // front right + new Translation2d(-0.5 * kTrackWidthMeters, 0.5 * kWheelBaseMeters), // back left + new Translation2d(-0.5 * kTrackWidthMeters, -0.5 * kWheelBaseMeters) // back right + ); + + private static double kSecondOrderKinematicsDt = 0.2; + + private static final double kTranslationP = 0.0; + private static final double kTranslationI = 0.0; + private static final double kTranslationD = 0.0; + + private static final double kRotationP = 0.0; + private static final double kRotationI = 0.0; + private static final double kRotationD = 0.0; + + /* + * Implementation + */ + + private SwerveModule frontLeft; + private SwerveModule frontRight; + private SwerveModule backLeft; + private SwerveModule backRight; + + private AHRS gyro; + + private SwerveDrivePoseEstimator odometry; + + private Field2d field; + + private Drivetrain() { + this.frontLeft = SwerveModule.getFrontLeft(); + this.frontRight = SwerveModule.getFrontRight(); + this.backLeft = SwerveModule.getBackLeft(); + this.backRight = SwerveModule.getBackRight(); + + this.gyro = new AHRS(SPI.Port.kMXP); + this.zeroYaw(); + + this.odometry = + new SwerveDrivePoseEstimator( + kSwerveKinematics, this.gyro.getRotation2d(), this.getModulePositions(), new Pose2d()); + + this.field = new Field2d(); + + SmartDashboard.putData(this.field); + + AutoBuilder.configureHolonomic( + this::getPose, + this::resetPose, + this::getChassisSpeeds, + this::drive, + new HolonomicPathFollowerConfig( + new PIDConstants(kTranslationP, kTranslationI, kTranslationD), + new PIDConstants(kRotationP, kRotationI, kRotationD), + kMaxSpeedMetersPerSecond, + 0.5 * Math.hypot(kTrackWidthMeters, kWheelBaseMeters), + new ReplanningConfig()), + () -> { + var myAlliance = DriverStation.getAlliance(); + + if (myAlliance.isPresent()) return myAlliance.get() == DriverStation.Alliance.Red; + + return false; + }, + this); + } + + public Pose2d getPose() { + return this.odometry.getEstimatedPosition(); + } + + public void resetPose(Pose2d pose) { + this.odometry.resetPosition(this.gyro.getRotation2d(), this.getModulePositions(), pose); + } + + private SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + this.frontLeft.getState(), + this.frontRight.getState(), + this.backLeft.getState(), + this.backRight.getState() + }; + } + + public ChassisSpeeds getChassisSpeeds() { + return kSwerveKinematics.toChassisSpeeds(this.getModuleStates()); + } + + private SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + this.frontLeft.getPosition(), + this.frontRight.getPosition(), + this.backLeft.getPosition(), + this.backRight.getPosition() + }; + } + + public double getYawDeg() { + return this.gyro.getYaw(); + } + + public Pose2d getOdometryEstimatedPose() { + return this.odometry.getEstimatedPosition(); + } + + public void zeroYaw() { + this.gyro.zeroYaw(); + this.gyro.setAngleAdjustment(90.0); // TODO: change this depending on navx orientation + } + + public void updateModules(SwerveModuleState[] states) { + this.frontLeft.update(states[0]); + this.frontRight.update(states[1]); + this.backLeft.update(states[2]); + this.backRight.update(states[3]); + } + + public void drive( + double desiredThrottleMPS, + double desiredStrafeMPS, + double desiredOmegaRadPerSec, + boolean fieldRelative) { + double correctedOmega = + MathUtil.clamp( + desiredOmegaRadPerSec, -kMaxOmegaRadiansPerSecond, kMaxOmegaRadiansPerSecond); + + // apply corrective pose logarithm + double dt = kSecondOrderKinematicsDt; + double angularDisplacement = + -correctedOmega * dt; // TODO: why is this negative, maybe gyro orientation + + double sin = Math.sin(0.5 * angularDisplacement); + double cos = Math.cos(0.5 * angularDisplacement); + + double correctedThrottle = desiredStrafeMPS * sin + desiredThrottleMPS * cos; + double correctedStrafe = desiredStrafeMPS * cos - desiredThrottleMPS * sin; + + ChassisSpeeds speeds = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + correctedStrafe, correctedThrottle, correctedOmega, this.gyro.getRotation2d()) + : new ChassisSpeeds(correctedStrafe, correctedThrottle, correctedOmega); + + this.drive(speeds); + } + + public void drive(ChassisSpeeds speeds) { + SwerveModuleState[] states = kSwerveKinematics.toSwerveModuleStates(speeds); + + SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeedMetersPerSecond); + + this.updateModules(states); + } + + public void tuneDrive(SwerveModuleState[] states) { + this.frontLeft.tune(); + this.frontRight.tune(); + this.backLeft.tune(); + this.backRight.tune(); + + this.updateModules(states); + } + + public Command followPath(PathPlannerPath path) { + return AutoBuilder.followPath(path); + } + + private void doSendables() { + SmartDashboard.putNumber("Drive Heading", this.getYawDeg()); + + Pose2d odometryEstimatedPose = this.getOdometryEstimatedPose(); + + SmartDashboard.putNumber("Odometry Pos X (m)", odometryEstimatedPose.getX()); + SmartDashboard.putNumber("Odometry Pos Y (m)", odometryEstimatedPose.getY()); + + // TODO: is this angle or omega? + SmartDashboard.putNumber( + "Odometry Angle (deg)", odometryEstimatedPose.getRotation().getDegrees()); + } + + @Override + public void periodic() { + this.odometry.update(this.gyro.getRotation2d(), this.getModulePositions()); + + this.field.setRobotPose(this.getOdometryEstimatedPose()); + + this.doSendables(); + + this.frontLeft.doSendables(); + this.frontRight.doSendables(); + this.backLeft.doSendables(); + this.backRight.doSendables(); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java new file mode 100644 index 0000000..d01f758 --- /dev/null +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -0,0 +1,237 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.subsystems.drivetrain; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SwerveModule { + /* + * Singletons + */ + + // TODO: change these for new robot + + private static SwerveModule frontLeft = null; + + public static SwerveModule getFrontLeft() { + if (frontLeft == null) + frontLeft = new SwerveModule("Front Left", 4, 3, 15, false, false, false, -0.342529); + + return frontLeft; + } + + private static SwerveModule frontRight = null; + + public static SwerveModule getFrontRight() { + if (frontRight == null) + frontRight = new SwerveModule("Front Right", 6, 5, 16, false, false, false, -0.238281); + + return frontRight; + } + + private static SwerveModule backLeft = null; + + public static SwerveModule getBackLeft() { + if (backLeft == null) + backLeft = new SwerveModule("Back Left", 2, 1, 14, true, false, false, 0.325928); + + return backLeft; + } + + private static SwerveModule backRight = null; + + public static SwerveModule getBackRight() { + if (backRight == null) + backRight = new SwerveModule("Back Right", 8, 7, 13, true, false, false, -0.016357); + + return backRight; + } + + /* + * Constants + */ + + // TODO: change these for new robot + + private static final CANcoderConfiguration kCANCoderConfig = new CANcoderConfiguration(); + + private static final double kWheelRadiusMeters = Units.inchesToMeters(2.0); + private static final double kGearRatio = 6.8; + private static final double kRPMToMPS = 2 * Math.PI * kWheelRadiusMeters / (kGearRatio * 60.0); + + private static final double kDriveP = 0.00; + private static final double kDriveI = 0.00; + private static final double kDriveD = 0.00; + private static final double kDriveFF = 0.266; + + private static final double kTurnP = 0.50; + private static final double kTurnI = 0.00; + private static final double kTurnD = 0.005; + + /* + * Implementation + */ + + private String id; + + private CANSparkMax driveMotor; + private CANSparkMax turnMotor; + + private RelativeEncoder driveEncoder; + private CANcoder turnEncoder; + + private SparkPIDController driveController; + private PIDController turnController; + + private SwerveModule( + String id, + int driveMotorPort, + int turnMotorPort, + int turnEncoderPort, + boolean invertDriveMotor, + boolean invertTurnMotor, + boolean invertTurnEncoder, + double turnEncoderOffset) { + this.id = id; + + this.configDrive(driveMotorPort, invertDriveMotor); + this.configTurn( + turnMotorPort, turnEncoderPort, invertTurnMotor, invertTurnEncoder, turnEncoderOffset); + } + + private void configDrive(int driveMotorPort, boolean invertDriveMotor) { + this.driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless); + + this.driveMotor.setInverted(invertDriveMotor); + this.driveMotor.setIdleMode(IdleMode.kBrake); + this.driveMotor.setSmartCurrentLimit(40); + this.driveMotor.enableVoltageCompensation(12); + this.driveMotor.burnFlash(); + + this.driveEncoder = this.driveMotor.getEncoder(); + this.driveEncoder.setVelocityConversionFactor(kRPMToMPS); + + this.driveController = this.driveMotor.getPIDController(); + this.driveController.setP(kDriveP); + this.driveController.setI(kDriveI); + this.driveController.setD(kDriveD); + this.driveController.setFF(kDriveFF); + } + + private void configTurn( + int turnMotorPort, + int turnEncoderPort, + boolean invertTurnMotor, + boolean invertTurnEncoder, + double turnEncoderOffset) { + this.turnMotor = new CANSparkMax(turnMotorPort, MotorType.kBrushless); + + this.turnMotor.setInverted(invertTurnMotor); + this.turnMotor.setIdleMode(IdleMode.kBrake); + this.turnMotor.setSmartCurrentLimit(40); + this.turnMotor.enableVoltageCompensation(12); + this.turnMotor.burnFlash(); + + this.turnEncoder = new CANcoder(turnEncoderPort); + + CANcoderConfiguration config = kCANCoderConfig; + config.MagnetSensor.withMagnetOffset(turnEncoderOffset); + config.MagnetSensor.withSensorDirection( + invertTurnEncoder + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive); + + this.turnEncoder.getConfigurator().apply(config); + + this.turnController = new PIDController(kTurnP, kTurnI, kTurnD); + this.turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + public double getDriveVelocity() { + return this.driveEncoder.getVelocity(); + } + + public double getTurnAngleRad() { + return this.turnEncoder.getAbsolutePosition().getValue() + * 2 + * Math.PI; // TODO: conversion rate? + } + + public double getTurnAngleDeg() { + return this.getTurnAngleRad() * 180.0 / Math.PI; + } + + public SwerveModulePosition getPosition() { + // TODO: should drive encoder position have a conversion factor? + return new SwerveModulePosition( + this.driveEncoder.getPosition(), Rotation2d.fromRadians(this.getTurnAngleRad())); + } + + public SwerveModuleState getState() { + return new SwerveModuleState( + this.getDriveVelocity(), Rotation2d.fromRadians(this.getTurnAngleRad())); + } + + public void update(SwerveModuleState desiredState) { + SwerveModuleState optimized = + SwerveModuleState.optimize(desiredState, Rotation2d.fromRadians(this.getTurnAngleRad())); + + this.driveController.setReference(optimized.speedMetersPerSecond, ControlType.kVelocity); + + this.turnController.setSetpoint(optimized.angle.getRadians()); + this.turnMotor.set( + -MathUtil.clamp(this.turnController.calculate(this.getTurnAngleRad()), -1.0, 1.0)); + } + + public void doSendables() { + SmartDashboard.putNumber(this.id + " Drive Vel (m/s)", this.getDriveVelocity()); + SmartDashboard.putNumber(this.id + " Turn Angle (deg)", this.getTurnAngleDeg()); + } + + public static void initTuning() { + SmartDashboard.putNumber( + "module drive kp", SmartDashboard.getNumber("module drive kp", kDriveP)); + SmartDashboard.putNumber( + "module drive ki", SmartDashboard.getNumber("module drive ki", kDriveI)); + SmartDashboard.putNumber( + "module drive kd", SmartDashboard.getNumber("module drive kd", kDriveD)); + SmartDashboard.putNumber( + "module drive kff", SmartDashboard.getNumber("module drive kff", kDriveFF)); + + SmartDashboard.putNumber("module turn kp", SmartDashboard.getNumber("module turn kp", kTurnP)); + SmartDashboard.putNumber("module turn ki", SmartDashboard.getNumber("module turn ki", kTurnI)); + SmartDashboard.putNumber("module turn kd", SmartDashboard.getNumber("module turn kd", kTurnD)); + } + + public void tune() { + double tunedDriveP = SmartDashboard.getNumber("module drive kp", kDriveP); + double tunedDriveI = SmartDashboard.getNumber("module drive ki", kDriveI); + double tunedDriveD = SmartDashboard.getNumber("module drive kd", kDriveD); + double tunedDriveFF = SmartDashboard.getNumber("module drive kff", kDriveFF); + + this.driveController.setP(tunedDriveP); + this.driveController.setI(tunedDriveI); + this.driveController.setD(tunedDriveD); + this.driveController.setFF(tunedDriveFF); + + double tunedTurnP = SmartDashboard.getNumber("module turn kp", kTurnP); + double tunedTurnI = SmartDashboard.getNumber("module turn ki", kTurnI); + double tunedTurnD = SmartDashboard.getNumber("module turn kd", kTurnD); + + this.turnController.setPID(tunedTurnP, tunedTurnI, tunedTurnD); + } +} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..e978a5f --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..3c74146 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.1.2", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..88a68dd --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..69a4079 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,339 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.1.0", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.1.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.1.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.1.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.1.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.1.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.1.0", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.1.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.1.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.1.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.1.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.1.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.1.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..2d7c546 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.2.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.2.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.2.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.2.1" + } + ] +} \ No newline at end of file From f027017beb7733e6f7f9797d258cc26b1d2b4af9 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Wed, 31 Jan 2024 15:25:07 -0500 Subject: [PATCH 02/15] working changes for tuning the new chassis --- .../org/robolancers321/BuildConstants.java | 10 ++++---- .../org/robolancers321/RobotContainer.java | 11 +++++---- .../subsystems/drivetrain/Drivetrain.java | 14 +++++++++++ .../subsystems/drivetrain/SwerveModule.java | 23 ++++++++++++++----- 4 files changed, 43 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 66ad716..f9959d5 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 6; - public static final String GIT_SHA = "6f27170a2cbfcc3e8a09f9dca94b598405cb30ef"; - public static final String GIT_DATE = "2024-01-18 19:44:33 EST"; + public static final int GIT_REVISION = 7; + public static final String GIT_SHA = "2b8650ad3c14b3c9e71b446d30118173d97fee5b"; + public static final String GIT_DATE = "2024-01-23 08:59:18 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-01-23 08:57:30 EST"; - public static final long BUILD_UNIX_TIME = 1706018250153L; + public static final String BUILD_DATE = "2024-01-30 13:58:29 EST"; + public static final long BUILD_UNIX_TIME = 1706641109688L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index ddd92ae..2d6e7cd 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import org.robolancers321.commands.drivetrain.TeleopDrive; +import org.robolancers321.commands.drivetrain.TuneModules; import org.robolancers321.subsystems.drivetrain.Drivetrain; public class RobotContainer { @@ -32,19 +33,21 @@ public RobotContainer() { } private void configureBindings() { - this.drivetrain.setDefaultCommand(new TeleopDrive(this.controller)); + this.drivetrain.setDefaultCommand(new TuneModules()); new Trigger(this.controller::getAButton).onTrue(new InstantCommand(this.drivetrain::zeroYaw)); } private void configureAutoChooser() { - NamedCommands.registerCommand("Say Hello", new PrintCommand("Hello")); + // NamedCommands.registerCommand("Say Hello", new PrintCommand("Hello")); - this.autoChooser.addOption("Do Nothing", new InstantCommand()); + // this.autoChooser.addOption("Do Nothing", new InstantCommand()); } public Command getAutonomousCommand() { - return new PathPlannerAuto("Example Auto"); + return new InstantCommand(); + + // return new PathPlannerAuto("Example Auto"); // return this.autoChooser.getSelected(); } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 088b403..6050f44 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -217,6 +217,20 @@ public void tuneDrive(SwerveModuleState[] states) { this.updateModules(states); } + public void dangerouslyTurnModules(){ + this.frontLeft.dangerouslyRunTurn(); + this.frontRight.dangerouslyRunTurn(); + this.backLeft.dangerouslyRunTurn(); + this.backRight.dangerouslyRunTurn(); + } + + public void dangerouslyDriveModules(){ + this.frontLeft.dangerouslyRunDrive(); + this.frontRight.dangerouslyRunDrive(); + this.backLeft.dangerouslyRunDrive(); + this.backRight.dangerouslyRunDrive(); + } + public Command followPath(PathPlannerPath path) { return AutoBuilder.followPath(path); } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index d01f758..939ea22 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -29,7 +29,7 @@ public class SwerveModule { public static SwerveModule getFrontLeft() { if (frontLeft == null) - frontLeft = new SwerveModule("Front Left", 4, 3, 15, false, false, false, -0.342529); + frontLeft = new SwerveModule("Front Left", 2, 1, 9, false, true, false, -0.453857); return frontLeft; } @@ -38,7 +38,7 @@ public static SwerveModule getFrontLeft() { public static SwerveModule getFrontRight() { if (frontRight == null) - frontRight = new SwerveModule("Front Right", 6, 5, 16, false, false, false, -0.238281); + frontRight = new SwerveModule("Front Right", 4, 3, 10, true, true, false, -0.359375); return frontRight; } @@ -47,7 +47,7 @@ public static SwerveModule getFrontRight() { public static SwerveModule getBackLeft() { if (backLeft == null) - backLeft = new SwerveModule("Back Left", 2, 1, 14, true, false, false, 0.325928); + backLeft = new SwerveModule("Back Left", 8, 7, 12, false, true, false, -0.084473); return backLeft; } @@ -56,7 +56,7 @@ public static SwerveModule getBackLeft() { public static SwerveModule getBackRight() { if (backRight == null) - backRight = new SwerveModule("Back Right", 8, 7, 13, true, false, false, -0.016357); + backRight = new SwerveModule("Back Right", 6, 5, 11, true, true, false, -0.071045); return backRight; } @@ -76,7 +76,7 @@ public static SwerveModule getBackRight() { private static final double kDriveP = 0.00; private static final double kDriveI = 0.00; private static final double kDriveD = 0.00; - private static final double kDriveFF = 0.266; + private static final double kDriveFF = 0.22; private static final double kTurnP = 0.50; private static final double kTurnI = 0.00; @@ -120,11 +120,12 @@ private void configDrive(int driveMotorPort, boolean invertDriveMotor) { this.driveMotor.setIdleMode(IdleMode.kBrake); this.driveMotor.setSmartCurrentLimit(40); this.driveMotor.enableVoltageCompensation(12); - this.driveMotor.burnFlash(); this.driveEncoder = this.driveMotor.getEncoder(); this.driveEncoder.setVelocityConversionFactor(kRPMToMPS); + this.driveMotor.burnFlash(); + this.driveController = this.driveMotor.getPIDController(); this.driveController.setP(kDriveP); this.driveController.setI(kDriveI); @@ -186,6 +187,16 @@ public SwerveModuleState getState() { this.getDriveVelocity(), Rotation2d.fromRadians(this.getTurnAngleRad())); } + // for determining inversions + public void dangerouslyRunTurn(){ + this.turnMotor.set(0.05); + } + + // for determining inversions + public void dangerouslyRunDrive(){ + this.driveMotor.set(0.05); + } + public void update(SwerveModuleState desiredState) { SwerveModuleState optimized = SwerveModuleState.optimize(desiredState, Rotation2d.fromRadians(this.getTurnAngleRad())); From 557ab0f36810c27abdd9bef8453e6a1592a654c6 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 2 Feb 2024 16:30:14 -0500 Subject: [PATCH 03/15] i love jason yang yay --- src/main/java/org/robolancers321/BuildConstants.java | 10 +++++----- src/main/java/org/robolancers321/RobotContainer.java | 3 +-- .../commands/drivetrain/FeedForwardDrive.java | 6 ------ .../subsystems/drivetrain/SwerveModule.java | 6 +++--- 4 files changed, 9 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index f9959d5..cabd93b 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 7; - public static final String GIT_SHA = "2b8650ad3c14b3c9e71b446d30118173d97fee5b"; - public static final String GIT_DATE = "2024-01-23 08:59:18 EST"; + public static final int GIT_REVISION = 8; + public static final String GIT_SHA = "f027017beb7733e6f7f9797d258cc26b1d2b4af9"; + public static final String GIT_DATE = "2024-01-31 15:25:07 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-01-30 13:58:29 EST"; - public static final long BUILD_UNIX_TIME = 1706641109688L; + public static final String BUILD_DATE = "2024-02-02 16:25:50 EST"; + public static final long BUILD_UNIX_TIME = 1706909150390L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 2d6e7cd..49ba81d 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -23,7 +23,6 @@ public class RobotContainer { public RobotContainer() { this.drivetrain = Drivetrain.getInstance(); - this.controller = new XboxController(0); this.autoChooser = AutoBuilder.buildAutoChooser(); @@ -33,7 +32,7 @@ public RobotContainer() { } private void configureBindings() { - this.drivetrain.setDefaultCommand(new TuneModules()); + this.drivetrain.setDefaultCommand(new TeleopDrive(this.controller)); new Trigger(this.controller::getAButton).onTrue(new InstantCommand(this.drivetrain::zeroYaw)); } diff --git a/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java b/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java index 3054e08..166bc23 100644 --- a/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java +++ b/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java @@ -39,12 +39,6 @@ public void execute() { double desiredStrafe = this.strafeSupplier.getAsDouble(); double desiredOmega = this.omegaSupplier.getAsDouble(); boolean desiredFieldCentic = this.fieldCentricSupplier.getAsBoolean(); - - // TODO: any preprocessing for desired input should be done here - desiredThrottle *= 1.2; - desiredStrafe *= 1.2; - desiredOmega *= -3.0; - this.drivetrain.drive(desiredThrottle, desiredStrafe, desiredOmega, desiredFieldCentic); } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index 939ea22..910337b 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -124,13 +124,13 @@ private void configDrive(int driveMotorPort, boolean invertDriveMotor) { this.driveEncoder = this.driveMotor.getEncoder(); this.driveEncoder.setVelocityConversionFactor(kRPMToMPS); - this.driveMotor.burnFlash(); - this.driveController = this.driveMotor.getPIDController(); this.driveController.setP(kDriveP); this.driveController.setI(kDriveI); this.driveController.setD(kDriveD); this.driveController.setFF(kDriveFF); + + this.driveMotor.burnFlash(); } private void configTurn( @@ -205,7 +205,7 @@ public void update(SwerveModuleState desiredState) { this.turnController.setSetpoint(optimized.angle.getRadians()); this.turnMotor.set( - -MathUtil.clamp(this.turnController.calculate(this.getTurnAngleRad()), -1.0, 1.0)); + MathUtil.clamp(this.turnController.calculate(this.getTurnAngleRad()), -1.0, 1.0)); } public void doSendables() { From 53117dca665fb5862ff56c3d0e1abd47e9621a3e Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 2 Feb 2024 19:03:46 -0500 Subject: [PATCH 04/15] refactored drivetrain. turn controller is now onboard, moved commands into subsystem to match stylistic design of other subsystems, changed some names and modifiers. this needs to be retested --- .../org/robolancers321/BuildConstants.java | 10 +- .../org/robolancers321/RobotContainer.java | 10 +- .../commands/drivetrain/DriveToTarget.java | 111 ------------ .../commands/drivetrain/FeedForwardDrive.java | 54 ------ .../commands/drivetrain/TeleopDrive.java | 22 --- .../drivetrain/TuneDriveToTarget.java | 150 ---------------- .../commands/drivetrain/TuneModules.java | 48 ----- .../subsystems/drivetrain/Drivetrain.java | 167 +++++++++++------- .../subsystems/drivetrain/SwerveModule.java | 112 ++++++------ 9 files changed, 177 insertions(+), 507 deletions(-) delete mode 100644 src/main/java/org/robolancers321/commands/drivetrain/DriveToTarget.java delete mode 100644 src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java delete mode 100644 src/main/java/org/robolancers321/commands/drivetrain/TeleopDrive.java delete mode 100644 src/main/java/org/robolancers321/commands/drivetrain/TuneDriveToTarget.java delete mode 100644 src/main/java/org/robolancers321/commands/drivetrain/TuneModules.java diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index cabd93b..904bc72 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 8; - public static final String GIT_SHA = "f027017beb7733e6f7f9797d258cc26b1d2b4af9"; - public static final String GIT_DATE = "2024-01-31 15:25:07 EST"; + public static final int GIT_REVISION = 9; + public static final String GIT_SHA = "557ab0f36810c27abdd9bef8453e6a1592a654c6"; + public static final String GIT_DATE = "2024-02-02 16:30:14 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-02-02 16:25:50 EST"; - public static final long BUILD_UNIX_TIME = 1706909150390L; + public static final String BUILD_DATE = "2024-02-02 18:59:14 EST"; + public static final long BUILD_UNIX_TIME = 1706918354935L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 49ba81d..629095d 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -2,17 +2,13 @@ package org.robolancers321; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; -import org.robolancers321.commands.drivetrain.TeleopDrive; -import org.robolancers321.commands.drivetrain.TuneModules; import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.drivetrain.SwerveModule; public class RobotContainer { Drivetrain drivetrain; @@ -32,9 +28,9 @@ public RobotContainer() { } private void configureBindings() { - this.drivetrain.setDefaultCommand(new TeleopDrive(this.controller)); + this.drivetrain.setDefaultCommand(this.drivetrain.teleopDrive(controller, true)); - new Trigger(this.controller::getAButton).onTrue(new InstantCommand(this.drivetrain::zeroYaw)); + new Trigger(this.controller::getAButton).onTrue(this.drivetrain.zeroYaw()); } private void configureAutoChooser() { diff --git a/src/main/java/org/robolancers321/commands/drivetrain/DriveToTarget.java b/src/main/java/org/robolancers321/commands/drivetrain/DriveToTarget.java deleted file mode 100644 index cf9bacc..0000000 --- a/src/main/java/org/robolancers321/commands/drivetrain/DriveToTarget.java +++ /dev/null @@ -1,111 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.drivetrain; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.DoubleSupplier; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.robolancers321.subsystems.drivetrain.Drivetrain; - -public class DriveToTarget extends Command { - /* - * Constants - */ - - private static final double kTranslationP = 0.0; - private static final double kTranslationI = 0.0; - private static final double kTranslationD = 0.0; - - private static final double kRotationP = 0.0; - private static final double kRotationI = 0.0; - private static final double kRotationD = 0.0; - - /* - * Implementation - */ - - Drivetrain drivetrain; - PhotonCamera camera; - - DoubleSupplier desiredDxSupplier; - DoubleSupplier desiredDzSupplier; - DoubleSupplier desiredDThetaSupplier; - - PIDController xController; - PIDController zController; - PIDController thetaController; - - public DriveToTarget( - DoubleSupplier desiredDxSupplier, - DoubleSupplier desiredDzSupplier, - DoubleSupplier desiredDThetaSupplier) { - this.drivetrain = Drivetrain.getInstance(); - this.camera = new PhotonCamera("photonvision"); - - this.desiredDxSupplier = desiredDxSupplier; - this.desiredDzSupplier = desiredDzSupplier; - this.desiredDThetaSupplier = desiredDThetaSupplier; - - this.xController = new PIDController(kTranslationP, kTranslationI, kTranslationD); - this.zController = new PIDController(kTranslationP, kTranslationI, kTranslationD); - this.thetaController = new PIDController(kRotationP, kRotationI, kRotationD); - - this.addRequirements(this.drivetrain); - } - - public DriveToTarget(double desiredDx, double desiredDz, double desiredTheta) { - this(() -> desiredDx, () -> desiredDz, () -> desiredTheta); - } - - @Override - public void execute() { - double desiredDx = this.desiredDxSupplier.getAsDouble(); - double desiredDz = this.desiredDzSupplier.getAsDouble(); - double desiredDTheta = this.desiredDThetaSupplier.getAsDouble(); - - this.xController.setSetpoint(desiredDx); - this.zController.setSetpoint(desiredDz); - this.thetaController.setSetpoint(desiredDTheta); - - double actualDx; - double actualDz; - double actualDTheta; - - PhotonPipelineResult visionResults = this.camera.getLatestResult(); - - SmartDashboard.putBoolean("has vision target", visionResults.hasTargets()); - - if (visionResults.hasTargets()) { - PhotonTrackedTarget bestTarget = visionResults.getBestTarget(); - - Transform3d relativeCameraPosition = bestTarget.getBestCameraToTarget(); - - actualDx = relativeCameraPosition.getX(); - actualDz = relativeCameraPosition.getZ(); - actualDTheta = relativeCameraPosition.getRotation().getY(); - } else { - actualDx = desiredDx; - actualDz = desiredDz; - actualDTheta = desiredDTheta; - } - - SmartDashboard.putNumber("actual target dx", actualDx); - SmartDashboard.putNumber("actual target dz", actualDz); - SmartDashboard.putNumber("actual target dtheta", actualDTheta); - - double xControllerOutput = this.xController.calculate(actualDx); - double zControllerOutput = this.zController.calculate(actualDz); - double thetaControllerOutput = this.thetaController.calculate(actualDTheta); - - SmartDashboard.putNumber("x controller output", xControllerOutput); - SmartDashboard.putNumber("z controller output", zControllerOutput); - SmartDashboard.putNumber("theta controller output", thetaControllerOutput); - - // TODO: check vision outputs before actually calling drive - // this.drivetrain.drive(zControllerOutput, xControllerOutput, thetaControllerOutput, false); - } -} diff --git a/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java b/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java deleted file mode 100644 index 166bc23..0000000 --- a/src/main/java/org/robolancers321/commands/drivetrain/FeedForwardDrive.java +++ /dev/null @@ -1,54 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.drivetrain; - -import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import org.robolancers321.subsystems.drivetrain.Drivetrain; - -public class FeedForwardDrive extends Command { - private Drivetrain drivetrain; - - private DoubleSupplier throttleSupplier; - private DoubleSupplier strafeSupplier; - private DoubleSupplier omegaSupplier; - private BooleanSupplier fieldCentricSupplier; - - public FeedForwardDrive( - DoubleSupplier throttleSupplier, - DoubleSupplier strafeSupplier, - DoubleSupplier omegaSupplier, - BooleanSupplier fieldCentricSupplier) { - this.drivetrain = Drivetrain.getInstance(); - - this.throttleSupplier = throttleSupplier; - this.strafeSupplier = strafeSupplier; - this.omegaSupplier = omegaSupplier; - this.fieldCentricSupplier = fieldCentricSupplier; - - this.addRequirements(this.drivetrain); - } - - public FeedForwardDrive(double throttle, double strafe, double omega, boolean fieldCentric) { - this(() -> throttle, () -> strafe, () -> omega, () -> fieldCentric); - } - - @Override - public void execute() { - double desiredThrottle = this.throttleSupplier.getAsDouble(); - double desiredStrafe = this.strafeSupplier.getAsDouble(); - double desiredOmega = this.omegaSupplier.getAsDouble(); - boolean desiredFieldCentic = this.fieldCentricSupplier.getAsBoolean(); - this.drivetrain.drive(desiredThrottle, desiredStrafe, desiredOmega, desiredFieldCentic); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - this.drivetrain.drive(0, 0, 0, false); - } -} diff --git a/src/main/java/org/robolancers321/commands/drivetrain/TeleopDrive.java b/src/main/java/org/robolancers321/commands/drivetrain/TeleopDrive.java deleted file mode 100644 index 6b059c4..0000000 --- a/src/main/java/org/robolancers321/commands/drivetrain/TeleopDrive.java +++ /dev/null @@ -1,22 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.drivetrain; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.XboxController; - -public class TeleopDrive extends FeedForwardDrive { - private static final double kTeleopMaxTranslationalSpeed = 2.5; - private static final double kTeleopMaxRotationalSpeed = 1.0; - - public TeleopDrive(XboxController controller, boolean fieldCentric) { - super( - () -> kTeleopMaxTranslationalSpeed * MathUtil.applyDeadband(controller.getLeftY(), 0.2), - () -> -kTeleopMaxTranslationalSpeed * MathUtil.applyDeadband(controller.getLeftX(), 0.2), - () -> kTeleopMaxRotationalSpeed * MathUtil.applyDeadband(controller.getRightX(), 0.2), - () -> fieldCentric); - } - - public TeleopDrive(XboxController controller) { - this(controller, true); - } -} diff --git a/src/main/java/org/robolancers321/commands/drivetrain/TuneDriveToTarget.java b/src/main/java/org/robolancers321/commands/drivetrain/TuneDriveToTarget.java deleted file mode 100644 index 9375400..0000000 --- a/src/main/java/org/robolancers321/commands/drivetrain/TuneDriveToTarget.java +++ /dev/null @@ -1,150 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.drivetrain; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.DoubleSupplier; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.robolancers321.subsystems.drivetrain.Drivetrain; - -public class TuneDriveToTarget extends Command { - /* - * Constants - */ - - private static final double kTranslationP = 0.4; - private static final double kTranslationI = 0.0; - private static final double kTranslationD = 0.0; - - private static final double kRotationP = 0.3; - private static final double kRotationI = 0.0; - private static final double kRotationD = 0.0; - - /* - * Implementation - */ - - Drivetrain drivetrain; - PhotonCamera camera; - - DoubleSupplier desiredDxSupplier; - DoubleSupplier desiredDzSupplier; - DoubleSupplier desiredDThetaSupplier; - - PIDController positionController; - PIDController thetaController; - - public TuneDriveToTarget( - DoubleSupplier desiredDxSupplier, - DoubleSupplier desiredDzSupplier, - DoubleSupplier desiredDThetaSupplier) { - this.drivetrain = Drivetrain.getInstance(); - this.camera = new PhotonCamera("camera"); - - this.desiredDxSupplier = desiredDxSupplier; - this.desiredDzSupplier = desiredDzSupplier; - this.desiredDThetaSupplier = desiredDThetaSupplier; - - this.positionController = new PIDController(kTranslationP, kTranslationI, kTranslationD); - this.thetaController = new PIDController(kRotationP, kRotationI, kRotationD); - - SmartDashboard.putNumber( - "translation controller kp", - SmartDashboard.getNumber("translation controller kp", kTranslationP)); - SmartDashboard.putNumber( - "translation controller ki", - SmartDashboard.getNumber("translation controller ki", kTranslationI)); - SmartDashboard.putNumber( - "translation controller kd", - SmartDashboard.getNumber("translation controller kd", kTranslationD)); - - SmartDashboard.putNumber( - "rotation controller kp", SmartDashboard.getNumber("rotation controller kp", kRotationP)); - SmartDashboard.putNumber( - "rotation controller ki", SmartDashboard.getNumber("rotation controller ki", kRotationI)); - SmartDashboard.putNumber( - "rotation controller kd", SmartDashboard.getNumber("rotation controller kd", kRotationD)); - - this.addRequirements(this.drivetrain); - } - - public TuneDriveToTarget(double desiredDx, double desiredDz, double desiredTheta) { - this(() -> desiredDx, () -> desiredDz, () -> desiredTheta); - } - - @Override - public void execute() { - double tunedTranslationP = SmartDashboard.getNumber("translation controller kp", kTranslationP); - double tunedTranslationI = SmartDashboard.getNumber("translation controller ki", kTranslationI); - double tunedTranslationD = SmartDashboard.getNumber("translation controller kd", kTranslationD); - - this.positionController.setPID(tunedTranslationP, tunedTranslationI, tunedTranslationD); - - double tunedRotationP = SmartDashboard.getNumber("rotation controller kp", kRotationP); - double tunedRotationI = SmartDashboard.getNumber("rotation controller ki", kRotationI); - double tunedRotationD = SmartDashboard.getNumber("rotation controller kd", kRotationD); - - this.thetaController.setPID(tunedRotationP, tunedRotationI, tunedRotationD); - - double desiredDx = this.desiredDxSupplier.getAsDouble(); - double desiredDz = this.desiredDzSupplier.getAsDouble(); - double desiredDTheta = this.desiredDThetaSupplier.getAsDouble(); - - this.positionController.setSetpoint(0.0); - this.thetaController.setSetpoint(desiredDTheta); - - double actualDx; - double actualDz; - double actualDTheta; - - PhotonPipelineResult visionResults = this.camera.getLatestResult(); - - SmartDashboard.putBoolean("has vision target", visionResults.hasTargets()); - - if (visionResults.hasTargets()) { - PhotonTrackedTarget bestTarget = visionResults.getBestTarget(); - - Transform3d relativeCameraPosition = bestTarget.getBestCameraToTarget(); - - // goofy ah coordinate system - // these are from robot looking at tag - actualDx = -relativeCameraPosition.getY(); - actualDz = relativeCameraPosition.getX(); - actualDTheta = relativeCameraPosition.getRotation().getX() * 180.0 / Math.PI; - - } else { - actualDx = desiredDx; - actualDz = desiredDz; - actualDTheta = desiredDTheta; - } - - SmartDashboard.putNumber("actual target dx", actualDx); - SmartDashboard.putNumber("actual target dz", actualDz); - SmartDashboard.putNumber("actual target dtheta", actualDTheta); - - double errorX = desiredDx - actualDx; - double errorZ = desiredDz - actualDz; - double errorPosition = Math.hypot(errorX, errorZ); - - double positionControllerOutput = - MathUtil.clamp(this.positionController.calculate(errorPosition), -0.3, 0.3); - double thetaControllerOutput = - MathUtil.clamp(this.thetaController.calculate(actualDTheta), -0.3, 0.3); - - SmartDashboard.putNumber("position controller output", positionControllerOutput); - SmartDashboard.putNumber("theta controller output", thetaControllerOutput); - - double xComponentOfError = errorX / errorPosition; - double zComponentOfError = errorZ / errorPosition; - - double outputX = MathUtil.clamp(xComponentOfError * positionControllerOutput, -0.3, 0.3); - double outputZ = MathUtil.clamp(zComponentOfError * positionControllerOutput, -0.3, 0.3); - - this.drivetrain.drive(outputX, outputZ, thetaControllerOutput, false); - } -} diff --git a/src/main/java/org/robolancers321/commands/drivetrain/TuneModules.java b/src/main/java/org/robolancers321/commands/drivetrain/TuneModules.java deleted file mode 100644 index 8a60fef..0000000 --- a/src/main/java/org/robolancers321/commands/drivetrain/TuneModules.java +++ /dev/null @@ -1,48 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.drivetrain; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import org.robolancers321.subsystems.drivetrain.Drivetrain; -import org.robolancers321.subsystems.drivetrain.SwerveModule; - -public class TuneModules extends Command { - private Drivetrain drivetrain; - - public TuneModules() { - this.drivetrain = Drivetrain.getInstance(); - - SwerveModule.initTuning(); - - SmartDashboard.putNumber("target module angle (deg)", 0.0); - SmartDashboard.putNumber("target module vel (m/s)", 0.0); - - this.addRequirements(this.drivetrain); - } - - @Override - public void execute() { - double theta = SmartDashboard.getNumber("target module angle (deg)", 0.0); - double vel = SmartDashboard.getNumber("target module vel (m/s)", 0.0); - - SwerveModuleState[] states = new SwerveModuleState[4]; - - for (int i = 0; i < 4; i++) { - states[i] = new SwerveModuleState(vel, Rotation2d.fromDegrees(theta)); - } - - this.drivetrain.tuneDrive(states); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - this.drivetrain.drive(0.0, 0.0, 0.0, false); - } -} diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 6050f44..82519a7 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.MathUtil; 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.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -19,10 +20,13 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; public class Drivetrain extends SubsystemBase { /* @@ -48,14 +52,14 @@ public static Drivetrain getInstance() { private static final double kMaxOmegaRadiansPerSecond = 1.5 * Math.PI; private static final PathConstraints kAutoConstraints = - new PathConstraints(3.0, 3.0, 540 * Math.PI / 180, 720 * Math.PI / 180); + new PathConstraints(4.0, 2.0, 540 * Math.PI / 180, 720 * Math.PI / 180); private static final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics( new Translation2d(0.5 * kTrackWidthMeters, 0.5 * kWheelBaseMeters), // front left new Translation2d(0.5 * kTrackWidthMeters, -0.5 * kWheelBaseMeters), // front right - new Translation2d(-0.5 * kTrackWidthMeters, 0.5 * kWheelBaseMeters), // back left - new Translation2d(-0.5 * kTrackWidthMeters, -0.5 * kWheelBaseMeters) // back right + new Translation2d(-0.5 * kTrackWidthMeters, -0.5 * kWheelBaseMeters), // back right + new Translation2d(-0.5 * kTrackWidthMeters, 0.5 * kWheelBaseMeters) // back left ); private static double kSecondOrderKinematicsDt = 0.2; @@ -74,8 +78,8 @@ public static Drivetrain getInstance() { private SwerveModule frontLeft; private SwerveModule frontRight; - private SwerveModule backLeft; private SwerveModule backRight; + private SwerveModule backLeft; private AHRS gyro; @@ -86,8 +90,8 @@ public static Drivetrain getInstance() { private Drivetrain() { this.frontLeft = SwerveModule.getFrontLeft(); this.frontRight = SwerveModule.getFrontRight(); - this.backLeft = SwerveModule.getBackLeft(); this.backRight = SwerveModule.getBackRight(); + this.backLeft = SwerveModule.getBackLeft(); this.gyro = new AHRS(SPI.Port.kMXP); this.zeroYaw(); @@ -133,8 +137,8 @@ private SwerveModuleState[] getModuleStates() { return new SwerveModuleState[] { this.frontLeft.getState(), this.frontRight.getState(), - this.backLeft.getState(), - this.backRight.getState() + this.backRight.getState(), + this.backLeft.getState() }; } @@ -146,8 +150,8 @@ private SwerveModulePosition[] getModulePositions() { return new SwerveModulePosition[] { this.frontLeft.getPosition(), this.frontRight.getPosition(), - this.backLeft.getPosition(), - this.backRight.getPosition() + this.backRight.getPosition(), + this.backLeft.getPosition() }; } @@ -155,23 +159,14 @@ public double getYawDeg() { return this.gyro.getYaw(); } - public Pose2d getOdometryEstimatedPose() { - return this.odometry.getEstimatedPosition(); - } - - public void zeroYaw() { - this.gyro.zeroYaw(); - this.gyro.setAngleAdjustment(90.0); // TODO: change this depending on navx orientation - } - - public void updateModules(SwerveModuleState[] states) { + private void updateModules(SwerveModuleState[] states) { this.frontLeft.update(states[0]); this.frontRight.update(states[1]); - this.backLeft.update(states[2]); - this.backRight.update(states[3]); + this.backRight.update(states[2]); + this.backLeft.update(states[3]); } - public void drive( + private void drive( double desiredThrottleMPS, double desiredStrafeMPS, double desiredOmegaRadPerSec, @@ -200,7 +195,7 @@ public void drive( this.drive(speeds); } - public void drive(ChassisSpeeds speeds) { + private void drive(ChassisSpeeds speeds) { SwerveModuleState[] states = kSwerveKinematics.toSwerveModuleStates(speeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeedMetersPerSecond); @@ -208,57 +203,111 @@ public void drive(ChassisSpeeds speeds) { this.updateModules(states); } - public void tuneDrive(SwerveModuleState[] states) { - this.frontLeft.tune(); - this.frontRight.tune(); - this.backLeft.tune(); - this.backRight.tune(); - - this.updateModules(states); - } - - public void dangerouslyTurnModules(){ - this.frontLeft.dangerouslyRunTurn(); - this.frontRight.dangerouslyRunTurn(); - this.backLeft.dangerouslyRunTurn(); - this.backRight.dangerouslyRunTurn(); - } - - public void dangerouslyDriveModules(){ - this.frontLeft.dangerouslyRunDrive(); - this.frontRight.dangerouslyRunDrive(); - this.backLeft.dangerouslyRunDrive(); - this.backRight.dangerouslyRunDrive(); - } - - public Command followPath(PathPlannerPath path) { - return AutoBuilder.followPath(path); - } - private void doSendables() { SmartDashboard.putNumber("Drive Heading", this.getYawDeg()); - Pose2d odometryEstimatedPose = this.getOdometryEstimatedPose(); - - SmartDashboard.putNumber("Odometry Pos X (m)", odometryEstimatedPose.getX()); - SmartDashboard.putNumber("Odometry Pos Y (m)", odometryEstimatedPose.getY()); + Pose2d odometryPose = this.getPose(); - // TODO: is this angle or omega? - SmartDashboard.putNumber( - "Odometry Angle (deg)", odometryEstimatedPose.getRotation().getDegrees()); + SmartDashboard.putNumber("Odometry Pos X (m)", odometryPose.getX()); + SmartDashboard.putNumber("Odometry Pos Y (m)", odometryPose.getY()); + SmartDashboard.putNumber("Odometry Angle (deg)", odometryPose.getRotation().getDegrees()); } @Override public void periodic() { this.odometry.update(this.gyro.getRotation2d(), this.getModulePositions()); - this.field.setRobotPose(this.getOdometryEstimatedPose()); + this.field.setRobotPose(this.getPose()); this.doSendables(); this.frontLeft.doSendables(); this.frontRight.doSendables(); - this.backLeft.doSendables(); this.backRight.doSendables(); + this.backLeft.doSendables(); + } + + public Command zeroYaw() { + return runOnce( + () -> { + this.gyro.zeroYaw(); + this.gyro.setAngleAdjustment(90.0); // TODO: change this depending on navx orientation + }); + } + + private Command stop() { + return runOnce(() -> this.drive(0.0, 0.0, 0.0, false)); + } + + private Command feedForwardDrive( + DoubleSupplier throttleSupplier, + DoubleSupplier strafeSupplier, + DoubleSupplier omegaSupplier, + BooleanSupplier fieldCentricSupplier) { + return run(() -> { + double desiredThrottle = throttleSupplier.getAsDouble(); + double desiredStrafe = strafeSupplier.getAsDouble(); + double desiredOmega = omegaSupplier.getAsDouble(); + boolean desiredFieldCentic = fieldCentricSupplier.getAsBoolean(); + + this.drive(desiredThrottle, desiredStrafe, desiredOmega, desiredFieldCentic); + }) + .finallyDo(() -> this.stop()); + } + + public Command teleopDrive(XboxController controller, boolean fieldCentric) { + return this.feedForwardDrive( + () -> kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.2), + () -> -kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.2), + () -> kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.2), + () -> fieldCentric); + } + + public Command tuneModules() { + SmartDashboard.putNumber("target module angle (deg)", 0.0); + SmartDashboard.putNumber("target module vel (m/s)", 0.0); + + SwerveModule.initTuning(); + + return run(() -> { + double theta = SmartDashboard.getNumber("target module angle (deg)", 0.0); + double vel = SmartDashboard.getNumber("target module vel (m/s)", 0.0); + + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) + states[i] = new SwerveModuleState(vel, Rotation2d.fromDegrees(theta)); + + this.frontLeft.tune(); + this.frontRight.tune(); + this.backRight.tune(); + this.backLeft.tune(); + + this.updateModules(states); + }) + .finallyDo(() -> this.stop()); + } + + public Command dangerouslyRunDrive(double speed) { + return run( + () -> { + this.frontLeft.dangerouslyRunDrive(speed); + this.frontRight.dangerouslyRunDrive(speed); + this.backRight.dangerouslyRunDrive(speed); + this.backLeft.dangerouslyRunDrive(speed); + }); + } + + public Command dangerouslyRunTurn(double speed) { + return run( + () -> { + this.frontLeft.dangerouslyRunTurn(speed); + this.frontRight.dangerouslyRunTurn(speed); + this.backRight.dangerouslyRunTurn(speed); + this.backLeft.dangerouslyRunTurn(speed); + }); + } + + public Command followPath(PathPlannerPath path) { + return AutoBuilder.followPath(path); } } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index 910337b..a64dd94 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; @@ -10,8 +11,6 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -23,8 +22,6 @@ public class SwerveModule { * Singletons */ - // TODO: change these for new robot - private static SwerveModule frontLeft = null; public static SwerveModule getFrontLeft() { @@ -43,15 +40,6 @@ public static SwerveModule getFrontRight() { return frontRight; } - private static SwerveModule backLeft = null; - - public static SwerveModule getBackLeft() { - if (backLeft == null) - backLeft = new SwerveModule("Back Left", 8, 7, 12, false, true, false, -0.084473); - - return backLeft; - } - private static SwerveModule backRight = null; public static SwerveModule getBackRight() { @@ -61,17 +49,28 @@ public static SwerveModule getBackRight() { return backRight; } + private static SwerveModule backLeft = null; + + public static SwerveModule getBackLeft() { + if (backLeft == null) + backLeft = new SwerveModule("Back Left", 8, 7, 12, false, true, false, -0.084473); + + return backLeft; + } + /* * Constants */ - // TODO: change these for new robot - private static final CANcoderConfiguration kCANCoderConfig = new CANcoderConfiguration(); private static final double kWheelRadiusMeters = Units.inchesToMeters(2.0); private static final double kGearRatio = 6.8; - private static final double kRPMToMPS = 2 * Math.PI * kWheelRadiusMeters / (kGearRatio * 60.0); + private static final double kDrivePositionConversionFactor = + 2 * Math.PI * kWheelRadiusMeters / kGearRatio; + private static final double kDriveVelocityConversionFactor = + 2 * Math.PI * kWheelRadiusMeters / (kGearRatio * 60.0); + private static final double kTurnPositionConversionFactor = 1.0; private static final double kDriveP = 0.00; private static final double kDriveI = 0.00; @@ -92,10 +91,11 @@ public static SwerveModule getBackRight() { private CANSparkMax turnMotor; private RelativeEncoder driveEncoder; - private CANcoder turnEncoder; + private CANcoder absoluteTurnEncoder; + private RelativeEncoder turnEncoder; private SparkPIDController driveController; - private PIDController turnController; + private SparkPIDController turnController; private SwerveModule( String id, @@ -115,14 +115,14 @@ private SwerveModule( private void configDrive(int driveMotorPort, boolean invertDriveMotor) { this.driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless); - this.driveMotor.setInverted(invertDriveMotor); this.driveMotor.setIdleMode(IdleMode.kBrake); this.driveMotor.setSmartCurrentLimit(40); this.driveMotor.enableVoltageCompensation(12); this.driveEncoder = this.driveMotor.getEncoder(); - this.driveEncoder.setVelocityConversionFactor(kRPMToMPS); + this.driveEncoder.setPositionConversionFactor(kDrivePositionConversionFactor); + this.driveEncoder.setVelocityConversionFactor(kDriveVelocityConversionFactor); this.driveController = this.driveMotor.getPIDController(); this.driveController.setP(kDriveP); @@ -139,81 +139,89 @@ private void configTurn( boolean invertTurnMotor, boolean invertTurnEncoder, double turnEncoderOffset) { - this.turnMotor = new CANSparkMax(turnMotorPort, MotorType.kBrushless); + this.turnMotor = new CANSparkMax(turnMotorPort, MotorType.kBrushless); this.turnMotor.setInverted(invertTurnMotor); this.turnMotor.setIdleMode(IdleMode.kBrake); this.turnMotor.setSmartCurrentLimit(40); this.turnMotor.enableVoltageCompensation(12); - this.turnMotor.burnFlash(); - - this.turnEncoder = new CANcoder(turnEncoderPort); + this.absoluteTurnEncoder = new CANcoder(turnEncoderPort); CANcoderConfiguration config = kCANCoderConfig; + config.MagnetSensor.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Signed_PlusMinusHalf); config.MagnetSensor.withMagnetOffset(turnEncoderOffset); config.MagnetSensor.withSensorDirection( invertTurnEncoder ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive); + this.absoluteTurnEncoder.getConfigurator().apply(config); + + this.turnEncoder = this.turnMotor.getEncoder(); + this.turnEncoder.setPositionConversionFactor(kTurnPositionConversionFactor); + this.turnEncoder.setPosition( + this.absoluteTurnEncoder + .getAbsolutePosition() + .getValueAsDouble()); // TODO: do we need to delay this call? + + this.turnController = this.turnMotor.getPIDController(); + this.turnController.setPositionPIDWrappingMinInput(-Math.PI); + this.turnController.setPositionPIDWrappingMaxInput(Math.PI); + this.turnController.setPositionPIDWrappingEnabled(true); + this.turnController.setP(kTurnP); + this.turnController.setI(kTurnI); + this.turnController.setD(kTurnD); - this.turnEncoder.getConfigurator().apply(config); - - this.turnController = new PIDController(kTurnP, kTurnI, kTurnD); - this.turnController.enableContinuousInput(-Math.PI, Math.PI); + this.turnMotor.burnFlash(); } - public double getDriveVelocity() { + public double getDriveVelocityMPS() { return this.driveEncoder.getVelocity(); } + public double getTurnAngleRotations() { + return this.turnEncoder.getPosition(); + } + public double getTurnAngleRad() { - return this.turnEncoder.getAbsolutePosition().getValue() - * 2 - * Math.PI; // TODO: conversion rate? + return 2 * Math.PI * this.turnEncoder.getPosition(); } public double getTurnAngleDeg() { - return this.getTurnAngleRad() * 180.0 / Math.PI; + return 360.0 * this.getTurnAngleRotations(); } public SwerveModulePosition getPosition() { - // TODO: should drive encoder position have a conversion factor? return new SwerveModulePosition( this.driveEncoder.getPosition(), Rotation2d.fromRadians(this.getTurnAngleRad())); } public SwerveModuleState getState() { return new SwerveModuleState( - this.getDriveVelocity(), Rotation2d.fromRadians(this.getTurnAngleRad())); + this.getDriveVelocityMPS(), Rotation2d.fromRadians(this.getTurnAngleRad())); } - // for determining inversions - public void dangerouslyRunTurn(){ - this.turnMotor.set(0.05); + protected void dangerouslyRunDrive(double speed) { + this.driveMotor.set(speed); } - // for determining inversions - public void dangerouslyRunDrive(){ - this.driveMotor.set(0.05); + protected void dangerouslyRunTurn(double speed) { + this.turnMotor.set(speed); } - public void update(SwerveModuleState desiredState) { + protected void update(SwerveModuleState desiredState) { SwerveModuleState optimized = SwerveModuleState.optimize(desiredState, Rotation2d.fromRadians(this.getTurnAngleRad())); this.driveController.setReference(optimized.speedMetersPerSecond, ControlType.kVelocity); - - this.turnController.setSetpoint(optimized.angle.getRadians()); - this.turnMotor.set( - MathUtil.clamp(this.turnController.calculate(this.getTurnAngleRad()), -1.0, 1.0)); + this.turnController.setReference(optimized.angle.getRotations(), ControlType.kPosition); } - public void doSendables() { - SmartDashboard.putNumber(this.id + " Drive Vel (m/s)", this.getDriveVelocity()); + protected void doSendables() { + SmartDashboard.putNumber(this.id + " Drive Vel (m/s)", this.getDriveVelocityMPS()); SmartDashboard.putNumber(this.id + " Turn Angle (deg)", this.getTurnAngleDeg()); } - public static void initTuning() { + protected static void initTuning() { SmartDashboard.putNumber( "module drive kp", SmartDashboard.getNumber("module drive kp", kDriveP)); SmartDashboard.putNumber( @@ -228,7 +236,7 @@ public static void initTuning() { SmartDashboard.putNumber("module turn kd", SmartDashboard.getNumber("module turn kd", kTurnD)); } - public void tune() { + protected void tune() { double tunedDriveP = SmartDashboard.getNumber("module drive kp", kDriveP); double tunedDriveI = SmartDashboard.getNumber("module drive ki", kDriveI); double tunedDriveD = SmartDashboard.getNumber("module drive kd", kDriveD); @@ -243,6 +251,8 @@ public void tune() { double tunedTurnI = SmartDashboard.getNumber("module turn ki", kTurnI); double tunedTurnD = SmartDashboard.getNumber("module turn kd", kTurnD); - this.turnController.setPID(tunedTurnP, tunedTurnI, tunedTurnD); + this.turnController.setP(tunedTurnP); + this.turnController.setI(tunedTurnI); + this.turnController.setD(tunedTurnD); } } From c4b10f978d803477716935c797b7b86861da6146 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Sat, 3 Feb 2024 11:19:40 -0500 Subject: [PATCH 05/15] made adjustments for new bot and debugged new onboard control --- build.gradle | 2 +- .../java/org/robolancers321/BuildConstants.java | 10 +++++----- .../java/org/robolancers321/RobotContainer.java | 2 ++ .../subsystems/drivetrain/Drivetrain.java | 6 +++--- .../subsystems/drivetrain/SwerveModule.java | 16 ++++++++++------ 5 files changed, 21 insertions(+), 15 deletions(-) diff --git a/build.gradle b/build.gradle index c4ef2db..e780d7d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id 'com.diffplug.spotless' version '6.12.0' id "com.peterabeles.gversion" version "1.10" } diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 904bc72..466328b 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 9; - public static final String GIT_SHA = "557ab0f36810c27abdd9bef8453e6a1592a654c6"; - public static final String GIT_DATE = "2024-02-02 16:30:14 EST"; + public static final int GIT_REVISION = 10; + public static final String GIT_SHA = "53117dca665fb5862ff56c3d0e1abd47e9621a3e"; + public static final String GIT_DATE = "2024-02-02 19:03:46 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-02-02 18:59:14 EST"; - public static final long BUILD_UNIX_TIME = 1706918354935L; + public static final String BUILD_DATE = "2024-02-03 11:14:07 EST"; + public static final long BUILD_UNIX_TIME = 1706976847308L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 629095d..fefce29 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -28,6 +28,8 @@ public RobotContainer() { } private void configureBindings() { + // this.drivetrain.setDefaultCommand(this.drivetrain.tuneModules()); + // this.drivetrain.setDefaultCommand(this.drivetrain.dangerouslyRunTurn(0.05)); this.drivetrain.setDefaultCommand(this.drivetrain.teleopDrive(controller, true)); new Trigger(this.controller::getAButton).onTrue(this.drivetrain.zeroYaw()); diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 82519a7..33febec 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -257,9 +257,9 @@ private Command feedForwardDrive( public Command teleopDrive(XboxController controller, boolean fieldCentric) { return this.feedForwardDrive( - () -> kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.2), - () -> -kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.2), - () -> kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.2), + () -> 0.5 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.2), + () -> -0.5 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.2), + () -> -0.5 * kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.2), () -> fieldCentric); } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index a64dd94..1c4ef51 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -44,7 +44,7 @@ public static SwerveModule getFrontRight() { public static SwerveModule getBackRight() { if (backRight == null) - backRight = new SwerveModule("Back Right", 6, 5, 11, true, true, false, -0.071045); + backRight = new SwerveModule("Back Right", 6, 5, 11, true, true, false, -0.122314); return backRight; } @@ -70,16 +70,16 @@ public static SwerveModule getBackLeft() { 2 * Math.PI * kWheelRadiusMeters / kGearRatio; private static final double kDriveVelocityConversionFactor = 2 * Math.PI * kWheelRadiusMeters / (kGearRatio * 60.0); - private static final double kTurnPositionConversionFactor = 1.0; + private static final double kTurnPositionConversionFactor = 7.0 / 150.0; private static final double kDriveP = 0.00; private static final double kDriveI = 0.00; private static final double kDriveD = 0.00; private static final double kDriveFF = 0.22; - private static final double kTurnP = 0.50; + private static final double kTurnP = 20; private static final double kTurnI = 0.00; - private static final double kTurnD = 0.005; + private static final double kTurnD = 0; /* * Implementation @@ -209,8 +209,10 @@ protected void dangerouslyRunTurn(double speed) { } protected void update(SwerveModuleState desiredState) { - SwerveModuleState optimized = - SwerveModuleState.optimize(desiredState, Rotation2d.fromRadians(this.getTurnAngleRad())); + SwerveModuleState optimized = + SwerveModuleState.optimize(desiredState, Rotation2d.fromRadians(this.getTurnAngleRad())); + + SmartDashboard.putNumber(this.id + " ref angle", optimized.angle.getDegrees()); this.driveController.setReference(optimized.speedMetersPerSecond, ControlType.kVelocity); this.turnController.setReference(optimized.angle.getRotations(), ControlType.kPosition); @@ -219,6 +221,8 @@ protected void update(SwerveModuleState desiredState) { protected void doSendables() { SmartDashboard.putNumber(this.id + " Drive Vel (m/s)", this.getDriveVelocityMPS()); SmartDashboard.putNumber(this.id + " Turn Angle (deg)", this.getTurnAngleDeg()); + + SmartDashboard.putNumber(this.id + " abs enc deg", 360 * this.absoluteTurnEncoder.getAbsolutePosition().getValueAsDouble()); } protected static void initTuning() { From c04741f83395091ce0b5e6b7b62bd58043ebccc0 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Sat, 3 Feb 2024 16:27:13 -0500 Subject: [PATCH 06/15] tuned path following --- .../pathplanner/autos/Example Auto.auto | 25 ++++++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Example Path.path | 49 +++++++++++++++++++ .../org/robolancers321/BuildConstants.java | 10 ++-- .../org/robolancers321/RobotContainer.java | 30 ++++++++++-- .../subsystems/drivetrain/Drivetrain.java | 10 ++-- .../subsystems/drivetrain/SwerveModule.java | 7 ++- 7 files changed, 117 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Example Auto.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto new file mode 100644 index 0000000..a4de33a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..547eb2d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.5, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 7.0 + }, + "prevControl": { + "x": 2.5, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 466328b..7254d2f 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 10; - public static final String GIT_SHA = "53117dca665fb5862ff56c3d0e1abd47e9621a3e"; - public static final String GIT_DATE = "2024-02-02 19:03:46 EST"; + public static final int GIT_REVISION = 11; + public static final String GIT_SHA = "c4b10f978d803477716935c797b7b86861da6146"; + public static final String GIT_DATE = "2024-02-03 11:19:40 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-02-03 11:14:07 EST"; - public static final long BUILD_UNIX_TIME = 1706976847308L; + public static final String BUILD_DATE = "2024-02-03 16:24:53 EST"; + public static final long BUILD_UNIX_TIME = 1706995493915L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index fefce29..bc3b386 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -2,11 +2,22 @@ package org.robolancers321; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; + +import java.util.List; + import org.robolancers321.subsystems.drivetrain.Drivetrain; import org.robolancers321.subsystems.drivetrain.SwerveModule; @@ -42,10 +53,23 @@ private void configureAutoChooser() { } public Command getAutonomousCommand() { - return new InstantCommand(); + // return new InstantCommand(); - // return new PathPlannerAuto("Example Auto"); + this.drivetrain.resetPose(new Pose2d(2.0, 7.0, new Rotation2d())); - // return this.autoChooser.getSelected(); + List waypoints = List.of( + new Translation2d(2.0, 7.0), + new Translation2d(2.5, 7.0), + new Translation2d(3.5, 7.0), + new Translation2d(4.0, 7.0) + ); + + return AutoBuilder.followPath(new PathPlannerPath(waypoints, Drivetrain.kAutoConstraints, new GoalEndState(0, new Rotation2d()))); + + // works + // return AutoBuilder.followPath(PathPlannerPath.fromPathFile("Example Path")); + + // works + // return new PathPlannerAuto("Example Auto"); } } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 33febec..a39755b 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -51,7 +51,7 @@ public static Drivetrain getInstance() { private static final double kMaxSpeedMetersPerSecond = 4.0; private static final double kMaxOmegaRadiansPerSecond = 1.5 * Math.PI; - private static final PathConstraints kAutoConstraints = + public static final PathConstraints kAutoConstraints = new PathConstraints(4.0, 2.0, 540 * Math.PI / 180, 720 * Math.PI / 180); private static final SwerveDriveKinematics kSwerveKinematics = @@ -64,7 +64,7 @@ public static Drivetrain getInstance() { private static double kSecondOrderKinematicsDt = 0.2; - private static final double kTranslationP = 0.0; + private static final double kTranslationP = 2.0; private static final double kTranslationI = 0.0; private static final double kTranslationD = 0.0; @@ -257,9 +257,9 @@ private Command feedForwardDrive( public Command teleopDrive(XboxController controller, boolean fieldCentric) { return this.feedForwardDrive( - () -> 0.5 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.2), - () -> -0.5 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.2), - () -> -0.5 * kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.2), + () -> 0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.2), + () -> -0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.2), + () -> -0.3 * kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.2), () -> fieldCentric); } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index 1c4ef51..b5939e4 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -65,7 +65,7 @@ public static SwerveModule getBackLeft() { private static final CANcoderConfiguration kCANCoderConfig = new CANcoderConfiguration(); private static final double kWheelRadiusMeters = Units.inchesToMeters(2.0); - private static final double kGearRatio = 6.8; + private static final double kGearRatio = 6.12; private static final double kDrivePositionConversionFactor = 2 * Math.PI * kWheelRadiusMeters / kGearRatio; private static final double kDriveVelocityConversionFactor = @@ -75,7 +75,7 @@ public static SwerveModule getBackLeft() { private static final double kDriveP = 0.00; private static final double kDriveI = 0.00; private static final double kDriveD = 0.00; - private static final double kDriveFF = 0.22; + private static final double kDriveFF = 0.198; private static final double kTurnP = 20; private static final double kTurnI = 0.00; @@ -121,6 +121,7 @@ private void configDrive(int driveMotorPort, boolean invertDriveMotor) { this.driveMotor.enableVoltageCompensation(12); this.driveEncoder = this.driveMotor.getEncoder(); + this.driveEncoder.setPosition(0.0); this.driveEncoder.setPositionConversionFactor(kDrivePositionConversionFactor); this.driveEncoder.setVelocityConversionFactor(kDriveVelocityConversionFactor); @@ -219,6 +220,8 @@ protected void update(SwerveModuleState desiredState) { } protected void doSendables() { + SmartDashboard.putNumber(this.id + " position m", this.getPosition().distanceMeters); + SmartDashboard.putNumber(this.id + " Drive Vel (m/s)", this.getDriveVelocityMPS()); SmartDashboard.putNumber(this.id + " Turn Angle (deg)", this.getTurnAngleDeg()); From 649780f83e67f8c7cdcf1b9957feae9e8fb1329e Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Thu, 8 Feb 2024 14:49:14 -0500 Subject: [PATCH 07/15] added working changes to test vision on the opi, implements multi tag localization to odometry. TODO: error matrix to make kalman filter more effective --- .../org/robolancers321/BuildConstants.java | 10 ++--- .../org/robolancers321/RobotContainer.java | 38 +++++++---------- .../subsystems/drivetrain/Drivetrain.java | 41 ++++++++++++++++++- .../subsystems/drivetrain/SwerveModule.java | 8 ++-- vendordeps/photonlib.json | 10 ++--- 5 files changed, 69 insertions(+), 38 deletions(-) diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 7254d2f..2a3d572 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 11; - public static final String GIT_SHA = "c4b10f978d803477716935c797b7b86861da6146"; - public static final String GIT_DATE = "2024-02-03 11:19:40 EST"; + public static final int GIT_REVISION = 12; + public static final String GIT_SHA = "c04741f83395091ce0b5e6b7b62bd58043ebccc0"; + public static final String GIT_DATE = "2024-02-03 16:27:13 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-02-03 16:24:53 EST"; - public static final long BUILD_UNIX_TIME = 1706995493915L; + public static final String BUILD_DATE = "2024-02-08 14:47:12 EST"; + public static final long BUILD_UNIX_TIME = 1707421632384L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index bc3b386..78ea2fd 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -2,24 +2,11 @@ package org.robolancers321; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -import java.util.List; - import org.robolancers321.subsystems.drivetrain.Drivetrain; -import org.robolancers321.subsystems.drivetrain.SwerveModule; public class RobotContainer { Drivetrain drivetrain; @@ -41,9 +28,9 @@ public RobotContainer() { private void configureBindings() { // this.drivetrain.setDefaultCommand(this.drivetrain.tuneModules()); // this.drivetrain.setDefaultCommand(this.drivetrain.dangerouslyRunTurn(0.05)); - this.drivetrain.setDefaultCommand(this.drivetrain.teleopDrive(controller, true)); + // this.drivetrain.setDefaultCommand(this.drivetrain.teleopDrive(controller, true)); - new Trigger(this.controller::getAButton).onTrue(this.drivetrain.zeroYaw()); + // new Trigger(this.controller::getAButton).onTrue(this.drivetrain.zeroYaw()); } private void configureAutoChooser() { @@ -53,18 +40,21 @@ private void configureAutoChooser() { } public Command getAutonomousCommand() { - // return new InstantCommand(); + return new InstantCommand(); + + // works - this.drivetrain.resetPose(new Pose2d(2.0, 7.0, new Rotation2d())); + // this.drivetrain.resetPose(new Pose2d(2.0, 7.0, new Rotation2d())); - List waypoints = List.of( - new Translation2d(2.0, 7.0), - new Translation2d(2.5, 7.0), - new Translation2d(3.5, 7.0), - new Translation2d(4.0, 7.0) - ); + // List waypoints = List.of( + // new Translation2d(2.0, 7.0), + // new Translation2d(2.5, 7.0), + // new Translation2d(3.5, 7.0), + // new Translation2d(4.0, 7.0) + // ); - return AutoBuilder.followPath(new PathPlannerPath(waypoints, Drivetrain.kAutoConstraints, new GoalEndState(0, new Rotation2d()))); + // return AutoBuilder.followPath(new PathPlannerPath(waypoints, Drivetrain.kAutoConstraints, new + // GoalEndState(0, new Rotation2d()))); // works // return AutoBuilder.followPath(PathPlannerPath.fromPathFile("Example Path")); diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index a39755b..fb8580a 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -12,6 +12,7 @@ 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.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -27,6 +28,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PhotonPipelineResult; public class Drivetrain extends SubsystemBase { /* @@ -45,6 +49,8 @@ public static Drivetrain getInstance() { * Constants */ + private static final String kCameraName = "MainCamera"; + private static final double kTrackWidthMeters = Units.inchesToMeters(17.5); private static final double kWheelBaseMeters = Units.inchesToMeters(17.5); @@ -83,6 +89,7 @@ public static Drivetrain getInstance() { private AHRS gyro; + private PhotonCamera camera; private SwerveDrivePoseEstimator odometry; private Field2d field; @@ -96,6 +103,8 @@ private Drivetrain() { this.gyro = new AHRS(SPI.Port.kMXP); this.zeroYaw(); + this.camera = new PhotonCamera(kCameraName); + this.odometry = new SwerveDrivePoseEstimator( kSwerveKinematics, this.gyro.getRotation2d(), this.getModulePositions(), new Pose2d()); @@ -217,6 +226,35 @@ private void doSendables() { public void periodic() { this.odometry.update(this.gyro.getRotation2d(), this.getModulePositions()); + PhotonPipelineResult latestResult = this.camera.getLatestResult(); + + if (latestResult.hasTargets()) { + MultiTargetPNPResult multiTagResult = latestResult.getMultiTagResult(); + + if (multiTagResult.estimatedPose.isPresent) { + Transform3d multiTagPoseEstimation = multiTagResult.estimatedPose.best; + + SmartDashboard.putNumber("vision dx (m)", multiTagPoseEstimation.getX()); + SmartDashboard.putNumber("vision dy (m)", multiTagPoseEstimation.getY()); + SmartDashboard.putNumber("vision dz (m)", multiTagPoseEstimation.getZ()); + + SmartDashboard.putNumber("vision rz (m)", multiTagPoseEstimation.getRotation().getX()); + SmartDashboard.putNumber("vision ry (m)", multiTagPoseEstimation.getRotation().getY()); + SmartDashboard.putNumber("vision rz (m)", multiTagPoseEstimation.getRotation().getZ()); + + // TODO: determine a threshold within which data can be used + SmartDashboard.putNumber("vision error pixels", multiTagResult.estimatedPose.bestReprojErr); + + Pose2d fieldPose = + new Pose2d( + multiTagPoseEstimation.getX(), + multiTagPoseEstimation.getY(), + Rotation2d.fromRadians(multiTagPoseEstimation.getRotation().getZ())); + + this.odometry.addVisionMeasurement(fieldPose, latestResult.getTimestampSeconds()); + } + } + this.field.setRobotPose(this.getPose()); this.doSendables(); @@ -259,7 +297,8 @@ public Command teleopDrive(XboxController controller, boolean fieldCentric) { return this.feedForwardDrive( () -> 0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.2), () -> -0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.2), - () -> -0.3 * kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.2), + () -> + -0.3 * kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.2), () -> fieldCentric); } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index b5939e4..fd395a0 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -210,8 +210,8 @@ protected void dangerouslyRunTurn(double speed) { } protected void update(SwerveModuleState desiredState) { - SwerveModuleState optimized = - SwerveModuleState.optimize(desiredState, Rotation2d.fromRadians(this.getTurnAngleRad())); + SwerveModuleState optimized = + SwerveModuleState.optimize(desiredState, Rotation2d.fromRadians(this.getTurnAngleRad())); SmartDashboard.putNumber(this.id + " ref angle", optimized.angle.getDegrees()); @@ -225,7 +225,9 @@ protected void doSendables() { SmartDashboard.putNumber(this.id + " Drive Vel (m/s)", this.getDriveVelocityMPS()); SmartDashboard.putNumber(this.id + " Turn Angle (deg)", this.getTurnAngleDeg()); - SmartDashboard.putNumber(this.id + " abs enc deg", 360 * this.absoluteTurnEncoder.getAbsolutePosition().getValueAsDouble()); + SmartDashboard.putNumber( + this.id + " abs enc deg", + 360 * this.absoluteTurnEncoder.getAbsolutePosition().getValueAsDouble()); } protected static void initTuning() { diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 2d7c546..46211fc 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.1", + "version": "v2024.2.4", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.1", + "version": "v2024.2.4", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.1", + "version": "v2024.2.4", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.1" + "version": "v2024.2.4" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.1" + "version": "v2024.2.4" } ] } \ No newline at end of file From 91017fadaab580ff582e82e9ac8da416bb4f0df9 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Thu, 8 Feb 2024 14:55:44 -0500 Subject: [PATCH 08/15] added more pipeline details for result accuracy --- .../org/robolancers321/subsystems/drivetrain/Drivetrain.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index fb8580a..67593bd 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -244,6 +244,8 @@ public void periodic() { // TODO: determine a threshold within which data can be used SmartDashboard.putNumber("vision error pixels", multiTagResult.estimatedPose.bestReprojErr); + SmartDashboard.putNumber( + "vision pose ambiguity", latestResult.getMultiTagResult().estimatedPose.ambiguity); Pose2d fieldPose = new Pose2d( From f4351c3e8439665558277652574e0dcfda32ad7c Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 9 Feb 2024 12:42:30 -0500 Subject: [PATCH 09/15] vision implementation, TODO: fix stuff for single tag pose estimation --- .../subsystems/drivetrain/Drivetrain.java | 69 ++++++++++--------- 1 file changed, 38 insertions(+), 31 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 67593bd..c1fa662 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -12,6 +12,7 @@ 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.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -29,6 +30,7 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; @@ -212,6 +214,40 @@ private void drive(ChassisSpeeds speeds) { this.updateModules(states); } + private void fuseVision(){ + PhotonPipelineResult latestResult = this.camera.getLatestResult(); + + if (!latestResult.hasTargets()) return; + + Pose2d fieldRelativeEstimatedPose; + + MultiTargetPNPResult multiTagResult = latestResult.getMultiTagResult(); + + if(multiTagResult.estimatedPose.isPresent){ + Transform3d bestMultiTagEstimatedPose = multiTagResult.estimatedPose.best; + + // TODO: apply camera offset + fieldRelativeEstimatedPose = new Pose2d( + bestMultiTagEstimatedPose.getX(), + bestMultiTagEstimatedPose.getY(), + Rotation2d.fromRadians(bestMultiTagEstimatedPose.getRotation().getZ())); + } else { + Transform3d bestCameraToTarget3D = latestResult.getBestTarget().getBestCameraToTarget(); + + // TODO: check components + Transform2d bestCameraToTarget = new Transform2d( + bestCameraToTarget3D.getX(), + bestCameraToTarget3D.getY(), + Rotation2d.fromRadians(bestCameraToTarget3D.getRotation().getZ()) + ); + + // TODO: apply tag field location and camera offset + fieldRelativeEstimatedPose = PhotonUtils.estimateFieldToRobot(bestCameraToTarget, new Pose2d(), new Transform2d()); + } + + this.odometry.addVisionMeasurement(fieldRelativeEstimatedPose, latestResult.getTimestampSeconds()); + } + private void doSendables() { SmartDashboard.putNumber("Drive Heading", this.getYawDeg()); @@ -225,37 +261,8 @@ private void doSendables() { @Override public void periodic() { this.odometry.update(this.gyro.getRotation2d(), this.getModulePositions()); - - PhotonPipelineResult latestResult = this.camera.getLatestResult(); - - if (latestResult.hasTargets()) { - MultiTargetPNPResult multiTagResult = latestResult.getMultiTagResult(); - - if (multiTagResult.estimatedPose.isPresent) { - Transform3d multiTagPoseEstimation = multiTagResult.estimatedPose.best; - - SmartDashboard.putNumber("vision dx (m)", multiTagPoseEstimation.getX()); - SmartDashboard.putNumber("vision dy (m)", multiTagPoseEstimation.getY()); - SmartDashboard.putNumber("vision dz (m)", multiTagPoseEstimation.getZ()); - - SmartDashboard.putNumber("vision rz (m)", multiTagPoseEstimation.getRotation().getX()); - SmartDashboard.putNumber("vision ry (m)", multiTagPoseEstimation.getRotation().getY()); - SmartDashboard.putNumber("vision rz (m)", multiTagPoseEstimation.getRotation().getZ()); - - // TODO: determine a threshold within which data can be used - SmartDashboard.putNumber("vision error pixels", multiTagResult.estimatedPose.bestReprojErr); - SmartDashboard.putNumber( - "vision pose ambiguity", latestResult.getMultiTagResult().estimatedPose.ambiguity); - - Pose2d fieldPose = - new Pose2d( - multiTagPoseEstimation.getX(), - multiTagPoseEstimation.getY(), - Rotation2d.fromRadians(multiTagPoseEstimation.getRotation().getZ())); - - this.odometry.addVisionMeasurement(fieldPose, latestResult.getTimestampSeconds()); - } - } + + this.fuseVision(); this.field.setRobotPose(this.getPose()); From 7524a074a715f005b04cae9276deea852e287597 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 9 Feb 2024 16:02:45 -0500 Subject: [PATCH 10/15] new inversions and offsets --- .../subsystems/drivetrain/SwerveModule.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index fd395a0..f662e99 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -26,7 +26,7 @@ public class SwerveModule { public static SwerveModule getFrontLeft() { if (frontLeft == null) - frontLeft = new SwerveModule("Front Left", 2, 1, 9, false, true, false, -0.453857); + frontLeft = new SwerveModule("Front Left", 2, 1, 9, false, true, false, -0.457520); return frontLeft; } @@ -35,7 +35,7 @@ public static SwerveModule getFrontLeft() { public static SwerveModule getFrontRight() { if (frontRight == null) - frontRight = new SwerveModule("Front Right", 4, 3, 10, true, true, false, -0.359375); + frontRight = new SwerveModule("Front Right", 4, 3, 10, false, true, false, 0.140137); return frontRight; } @@ -44,7 +44,7 @@ public static SwerveModule getFrontRight() { public static SwerveModule getBackRight() { if (backRight == null) - backRight = new SwerveModule("Back Right", 6, 5, 11, true, true, false, -0.122314); + backRight = new SwerveModule("Back Right", 6, 5, 11, true, true, false, -0.128906); return backRight; } @@ -53,7 +53,7 @@ public static SwerveModule getBackRight() { public static SwerveModule getBackLeft() { if (backLeft == null) - backLeft = new SwerveModule("Back Left", 8, 7, 12, false, true, false, -0.084473); + backLeft = new SwerveModule("Back Left", 8, 7, 12, false, true, false, -0.083984); return backLeft; } From d7e01c36dc0d5ca596bf3cb8fcf91750ca04d579 Mon Sep 17 00:00:00 2001 From: Vincent Zheng Date: Sat, 10 Feb 2024 09:27:35 -0500 Subject: [PATCH 11/15] replace manual pose math with PhotonPoseEstimator --- .../subsystems/drivetrain/Drivetrain.java | 90 +++++++++++++------ 1 file changed, 61 insertions(+), 29 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index c1fa662..eaace1e 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -8,11 +8,13 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.MathUtil; 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.geometry.Transform2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -27,12 +29,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; -import org.photonvision.PhotonUtils; -import org.photonvision.targeting.MultiTargetPNPResult; -import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; public class Drivetrain extends SubsystemBase { /* @@ -53,6 +56,13 @@ public static Drivetrain getInstance() { private static final String kCameraName = "MainCamera"; + private static final AprilTagFieldLayout kAprilTagFieldLayout = + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + + // TODO: find this transform + private static final Transform3d kRobotToCameraTransform = + new Transform3d(0, 0, 0, new Rotation3d()); + private static final double kTrackWidthMeters = Units.inchesToMeters(17.5); private static final double kWheelBaseMeters = Units.inchesToMeters(17.5); @@ -92,6 +102,8 @@ public static Drivetrain getInstance() { private AHRS gyro; private PhotonCamera camera; + private PhotonPoseEstimator visionEstimator; + private SwerveDrivePoseEstimator odometry; private Field2d field; @@ -107,6 +119,13 @@ private Drivetrain() { this.camera = new PhotonCamera(kCameraName); + this.visionEstimator = + new PhotonPoseEstimator( + kAprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + kRobotToCameraTransform); + this.odometry = new SwerveDrivePoseEstimator( kSwerveKinematics, this.gyro.getRotation2d(), this.getModulePositions(), new Pose2d()); @@ -214,38 +233,51 @@ private void drive(ChassisSpeeds speeds) { this.updateModules(states); } - private void fuseVision(){ - PhotonPipelineResult latestResult = this.camera.getLatestResult(); + private void fuseVision() { + // PhotonPipelineResult latestResult = this.camera.getLatestResult(); + + // if (!latestResult.hasTargets()) return; - if (!latestResult.hasTargets()) return; + // Pose2d fieldRelativeEstimatedPose; - Pose2d fieldRelativeEstimatedPose; + // MultiTargetPNPResult multiTagResult = latestResult.getMultiTagResult(); - MultiTargetPNPResult multiTagResult = latestResult.getMultiTagResult(); + // if (multiTagResult.estimatedPose.isPresent) { + // Transform3d bestMultiTagEstimatedPose = multiTagResult.estimatedPose.best; - if(multiTagResult.estimatedPose.isPresent){ - Transform3d bestMultiTagEstimatedPose = multiTagResult.estimatedPose.best; + // // TODO: apply camera offset + // fieldRelativeEstimatedPose = + // new Pose2d( + // bestMultiTagEstimatedPose.getX(), + // bestMultiTagEstimatedPose.getY(), + // Rotation2d.fromRadians(bestMultiTagEstimatedPose.getRotation().getZ())); + // } else { + // Transform3d bestCameraToTarget3D = latestResult.getBestTarget().getBestCameraToTarget(); - // TODO: apply camera offset - fieldRelativeEstimatedPose = new Pose2d( - bestMultiTagEstimatedPose.getX(), - bestMultiTagEstimatedPose.getY(), - Rotation2d.fromRadians(bestMultiTagEstimatedPose.getRotation().getZ())); - } else { - Transform3d bestCameraToTarget3D = latestResult.getBestTarget().getBestCameraToTarget(); + // // TODO: check components + // Transform2d bestCameraToTarget = + // new Transform2d( + // bestCameraToTarget3D.getX(), + // bestCameraToTarget3D.getY(), + // Rotation2d.fromRadians(bestCameraToTarget3D.getRotation().getZ())); - // TODO: check components - Transform2d bestCameraToTarget = new Transform2d( - bestCameraToTarget3D.getX(), - bestCameraToTarget3D.getY(), - Rotation2d.fromRadians(bestCameraToTarget3D.getRotation().getZ()) - ); + // Optional fieldRelativeTargetPose = + // kAprilTagFieldLayout.getTagPose(latestResult.getBestTarget().getFiducialId()); - // TODO: apply tag field location and camera offset - fieldRelativeEstimatedPose = PhotonUtils.estimateFieldToRobot(bestCameraToTarget, new Pose2d(), new Transform2d()); - } + // if (fieldRelativeTargetPose.isEmpty()) return; - this.odometry.addVisionMeasurement(fieldRelativeEstimatedPose, latestResult.getTimestampSeconds()); + // // TODO: apply tag field location and camera offset + // fieldRelativeEstimatedPose = + // PhotonUtils.estimateFieldToRobot( + // bestCameraToTarget, fieldRelativeTargetPose.get().toPose2d(), new Transform2d()); + // } + + Optional visionEstimate = visionEstimator.update(); + + if (visionEstimate.isEmpty()) return; + + this.odometry.addVisionMeasurement( + visionEstimate.get().estimatedPose.toPose2d(), visionEstimate.get().timestampSeconds); } private void doSendables() { @@ -261,7 +293,7 @@ private void doSendables() { @Override public void periodic() { this.odometry.update(this.gyro.getRotation2d(), this.getModulePositions()); - + this.fuseVision(); this.field.setRobotPose(this.getPose()); From 48eca2c536a83baaae96767336266104a1f0666a Mon Sep 17 00:00:00 2001 From: Vincent Zheng Date: Mon, 12 Feb 2024 15:25:32 -0500 Subject: [PATCH 12/15] successfully tested fusing vision into odometry and generating paths to a hard-coded field-relative pose on the fly --- .pathplanner/settings.json | 12 ++++ .../org/robolancers321/BuildConstants.java | 12 ++-- .../org/robolancers321/RobotContainer.java | 65 +++++++++++++++---- .../subsystems/drivetrain/Drivetrain.java | 21 ++++-- .../subsystems/drivetrain/SwerveModule.java | 2 +- 5 files changed, 86 insertions(+), 26 deletions(-) create mode 100644 .pathplanner/settings.json diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..2aaab01 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,12 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 2a3d572..7ea5139 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "321-Crescendo-2024"; + public static final String MAVEN_NAME = "swerve-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 12; - public static final String GIT_SHA = "c04741f83395091ce0b5e6b7b62bd58043ebccc0"; - public static final String GIT_DATE = "2024-02-03 16:27:13 EST"; + public static final int GIT_REVISION = 17; + public static final String GIT_SHA = "d7e01c36dc0d5ca596bf3cb8fcf91750ca04d579"; + public static final String GIT_DATE = "2024-02-10 09:27:35 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-02-08 14:47:12 EST"; - public static final long BUILD_UNIX_TIME = 1707421632384L; + public static final String BUILD_DATE = "2024-02-10 16:53:46 EST"; + public static final long BUILD_UNIX_TIME = 1707602026798L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 78ea2fd..78687dc 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -2,10 +2,14 @@ package org.robolancers321; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; import org.robolancers321.subsystems.drivetrain.Drivetrain; public class RobotContainer { @@ -28,9 +32,9 @@ public RobotContainer() { private void configureBindings() { // this.drivetrain.setDefaultCommand(this.drivetrain.tuneModules()); // this.drivetrain.setDefaultCommand(this.drivetrain.dangerouslyRunTurn(0.05)); - // this.drivetrain.setDefaultCommand(this.drivetrain.teleopDrive(controller, true)); + this.drivetrain.setDefaultCommand(this.drivetrain.teleopDrive(controller, true)); - // new Trigger(this.controller::getAButton).onTrue(this.drivetrain.zeroYaw()); + new Trigger(this.controller::getAButton).onTrue(this.drivetrain.zeroYaw()); } private void configureAutoChooser() { @@ -40,21 +44,54 @@ private void configureAutoChooser() { } public Command getAutonomousCommand() { - return new InstantCommand(); - // works - // this.drivetrain.resetPose(new Pose2d(2.0, 7.0, new Rotation2d())); + // this.drivetrain.resetPose(this.drivetrain.getPose()); + + // var targ = new Pose2d(1, 5.3, Rotation2d.fromDegrees(0)); + + var curr = drivetrain.getPose(); + + // var ctrl1 = curr.plus(new Transform2d(-0.2, 0, Rotation2d.fromDegrees(0))); + + // var ctrl2 = curr.plus(new Transform2d(-0.8, 0, Rotation2d.fromDegrees(0))); + + // var end = curr.plus(new Transform2d(-1.0, 0, Rotation2d.fromDegrees(0))); + + // List waypoints = PathPlannerPath.bezierFromPoses(curr, ctrl1, ctrl2, end); + + var path = + // new PathPlannerPath( + // PathPlannerPath.bezierFromPoses( + // curr, + // curr.plus(new Transform2d(0.2, 0, Rotation2d.fromDegrees(0))), + // curr.plus(new Transform2d(0.8, 0, Rotation2d.fromDegrees(0))), + // curr.plus(new Transform2d(1, 0, Rotation2d.fromDegrees(0)))), + // Drivetrain.kAutoConstraints, + // new GoalEndState(0, Rotation2d.fromDegrees(90))); + + new PathPlannerPath( + PathPlannerPath.bezierFromPoses( + curr, + new Pose2d(1.2, 5.3, Rotation2d.fromDegrees(0)), + new Pose2d(1, 5.3, Rotation2d.fromDegrees(0))), + Drivetrain.kAutoConstraints, + new GoalEndState(0, Rotation2d.fromDegrees(0))); + + // PathPlannerPath.fromPathPoints( + // List.of( + // new PathPoint(new Translation2d(curr.getX(), curr.getY())), + // new PathPoint(new Translation2d(1, 5.3))), + // Drivetrain.kAutoConstraints, + // new GoalEndState(0, Rotation2d.fromDegrees(0))); + + // var path = + // new PathPlannerPath( + // waypoints, Drivetrain.kAutoConstraints, new GoalEndState(0, new Rotation2d())); - // List waypoints = List.of( - // new Translation2d(2.0, 7.0), - // new Translation2d(2.5, 7.0), - // new Translation2d(3.5, 7.0), - // new Translation2d(4.0, 7.0) - // ); + path.preventFlipping = true; - // return AutoBuilder.followPath(new PathPlannerPath(waypoints, Drivetrain.kAutoConstraints, new - // GoalEndState(0, new Rotation2d()))); + return AutoBuilder.followPath(path); // works // return AutoBuilder.followPath(PathPlannerPath.fromPathFile("Example Path")); diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index eaace1e..3a33389 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -61,7 +62,7 @@ public static Drivetrain getInstance() { // TODO: find this transform private static final Transform3d kRobotToCameraTransform = - new Transform3d(0, 0, 0, new Rotation3d()); + new Transform3d(0, 0, 0, new Rotation3d(0, Math.PI, 0)); private static final double kTrackWidthMeters = Units.inchesToMeters(17.5); private static final double kWheelBaseMeters = Units.inchesToMeters(17.5); @@ -70,7 +71,7 @@ public static Drivetrain getInstance() { private static final double kMaxOmegaRadiansPerSecond = 1.5 * Math.PI; public static final PathConstraints kAutoConstraints = - new PathConstraints(4.0, 2.0, 540 * Math.PI / 180, 720 * Math.PI / 180); + new PathConstraints(4.0, 1.0, 270 * Math.PI / 180, 360 * Math.PI / 180); private static final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics( @@ -86,7 +87,7 @@ public static Drivetrain getInstance() { private static final double kTranslationI = 0.0; private static final double kTranslationD = 0.0; - private static final double kRotationP = 0.0; + private static final double kRotationP = 1.769; private static final double kRotationI = 0.0; private static final double kRotationD = 0.0; @@ -153,6 +154,13 @@ private Drivetrain() { return false; }, this); + + PathPlannerLogging.setLogActivePathCallback( + poses -> this.field.getObject("pathplanner path poses").setPoses(poses)); + PathPlannerLogging.setLogTargetPoseCallback( + targ -> this.field.getObject("pathplanner targ pose").setPose(targ)); + PathPlannerLogging.setLogCurrentPoseCallback( + curr -> this.field.getObject("pathplanner curr pose").setPose(curr)); } public Pose2d getPose() { @@ -160,7 +168,10 @@ public Pose2d getPose() { } public void resetPose(Pose2d pose) { - this.odometry.resetPosition(this.gyro.getRotation2d(), this.getModulePositions(), pose); + this.odometry.resetPosition( + Rotation2d.fromDegrees(this.gyro.getAngle()), // .plus(Rotation2d.fromDegrees(180)), + this.getModulePositions(), + pose); } private SwerveModuleState[] getModuleStates() { @@ -310,7 +321,7 @@ public Command zeroYaw() { return runOnce( () -> { this.gyro.zeroYaw(); - this.gyro.setAngleAdjustment(90.0); // TODO: change this depending on navx orientation + this.gyro.setAngleAdjustment(0.0); // TODO: change this depending on navx orientation }); } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java index f662e99..62a9b0e 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java @@ -77,7 +77,7 @@ public static SwerveModule getBackLeft() { private static final double kDriveD = 0.00; private static final double kDriveFF = 0.198; - private static final double kTurnP = 20; + private static final double kTurnP = 10; // 20 private static final double kTurnI = 0.00; private static final double kTurnD = 0; From ff080c1eff567a1ee3b838e24c1805a5aa804045 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Mon, 12 Feb 2024 16:11:49 -0500 Subject: [PATCH 13/15] tuned turn controller for path following --- .../autos/{Example Auto.auto => tuning.auto} | 0 .../pathplanner/paths/Example Path.path | 6 +-- .../org/robolancers321/BuildConstants.java | 12 ++--- .../org/robolancers321/RobotContainer.java | 41 +---------------- .../subsystems/drivetrain/Drivetrain.java | 46 ++----------------- 5 files changed, 15 insertions(+), 90 deletions(-) rename src/main/deploy/pathplanner/autos/{Example Auto.auto => tuning.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/tuning.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Example Auto.auto rename to src/main/deploy/pathplanner/autos/tuning.auto diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 547eb2d..9b00b5a 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.0, + "x": 4.0, "y": 7.0 }, "prevControl": { - "x": 2.5, + "x": 3.5, "y": 7.0 }, "nextControl": null, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 180.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 7ea5139..4d1c9c0 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "swerve-2024"; + public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 17; - public static final String GIT_SHA = "d7e01c36dc0d5ca596bf3cb8fcf91750ca04d579"; - public static final String GIT_DATE = "2024-02-10 09:27:35 EST"; + public static final int GIT_REVISION = 18; + public static final String GIT_SHA = "48eca2c536a83baaae96767336266104a1f0666a"; + public static final String GIT_DATE = "2024-02-12 15:25:32 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-02-10 16:53:46 EST"; - public static final long BUILD_UNIX_TIME = 1707602026798L; + public static final String BUILD_DATE = "2024-02-12 16:09:22 EST"; + public static final long BUILD_UNIX_TIME = 1707772162064L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 78687dc..dd9aee2 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -2,6 +2,7 @@ package org.robolancers321; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.geometry.Pose2d; @@ -44,32 +45,11 @@ private void configureAutoChooser() { } public Command getAutonomousCommand() { - // works - - // this.drivetrain.resetPose(this.drivetrain.getPose()); - - // var targ = new Pose2d(1, 5.3, Rotation2d.fromDegrees(0)); + // return new PathPlannerAuto("tuning"); var curr = drivetrain.getPose(); - // var ctrl1 = curr.plus(new Transform2d(-0.2, 0, Rotation2d.fromDegrees(0))); - - // var ctrl2 = curr.plus(new Transform2d(-0.8, 0, Rotation2d.fromDegrees(0))); - - // var end = curr.plus(new Transform2d(-1.0, 0, Rotation2d.fromDegrees(0))); - - // List waypoints = PathPlannerPath.bezierFromPoses(curr, ctrl1, ctrl2, end); - var path = - // new PathPlannerPath( - // PathPlannerPath.bezierFromPoses( - // curr, - // curr.plus(new Transform2d(0.2, 0, Rotation2d.fromDegrees(0))), - // curr.plus(new Transform2d(0.8, 0, Rotation2d.fromDegrees(0))), - // curr.plus(new Transform2d(1, 0, Rotation2d.fromDegrees(0)))), - // Drivetrain.kAutoConstraints, - // new GoalEndState(0, Rotation2d.fromDegrees(90))); - new PathPlannerPath( PathPlannerPath.bezierFromPoses( curr, @@ -78,25 +58,8 @@ public Command getAutonomousCommand() { Drivetrain.kAutoConstraints, new GoalEndState(0, Rotation2d.fromDegrees(0))); - // PathPlannerPath.fromPathPoints( - // List.of( - // new PathPoint(new Translation2d(curr.getX(), curr.getY())), - // new PathPoint(new Translation2d(1, 5.3))), - // Drivetrain.kAutoConstraints, - // new GoalEndState(0, Rotation2d.fromDegrees(0))); - - // var path = - // new PathPlannerPath( - // waypoints, Drivetrain.kAutoConstraints, new GoalEndState(0, new Rotation2d())); - path.preventFlipping = true; return AutoBuilder.followPath(path); - - // works - // return AutoBuilder.followPath(PathPlannerPath.fromPathFile("Example Path")); - - // works - // return new PathPlannerAuto("Example Auto"); } } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 3a33389..29d6307 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -87,7 +87,7 @@ public static Drivetrain getInstance() { private static final double kTranslationI = 0.0; private static final double kTranslationD = 0.0; - private static final double kRotationP = 1.769; + private static final double kRotationP = 4.0; private static final double kRotationI = 0.0; private static final double kRotationD = 0.0; @@ -245,44 +245,6 @@ private void drive(ChassisSpeeds speeds) { } private void fuseVision() { - // PhotonPipelineResult latestResult = this.camera.getLatestResult(); - - // if (!latestResult.hasTargets()) return; - - // Pose2d fieldRelativeEstimatedPose; - - // MultiTargetPNPResult multiTagResult = latestResult.getMultiTagResult(); - - // if (multiTagResult.estimatedPose.isPresent) { - // Transform3d bestMultiTagEstimatedPose = multiTagResult.estimatedPose.best; - - // // TODO: apply camera offset - // fieldRelativeEstimatedPose = - // new Pose2d( - // bestMultiTagEstimatedPose.getX(), - // bestMultiTagEstimatedPose.getY(), - // Rotation2d.fromRadians(bestMultiTagEstimatedPose.getRotation().getZ())); - // } else { - // Transform3d bestCameraToTarget3D = latestResult.getBestTarget().getBestCameraToTarget(); - - // // TODO: check components - // Transform2d bestCameraToTarget = - // new Transform2d( - // bestCameraToTarget3D.getX(), - // bestCameraToTarget3D.getY(), - // Rotation2d.fromRadians(bestCameraToTarget3D.getRotation().getZ())); - - // Optional fieldRelativeTargetPose = - // kAprilTagFieldLayout.getTagPose(latestResult.getBestTarget().getFiducialId()); - - // if (fieldRelativeTargetPose.isEmpty()) return; - - // // TODO: apply tag field location and camera offset - // fieldRelativeEstimatedPose = - // PhotonUtils.estimateFieldToRobot( - // bestCameraToTarget, fieldRelativeTargetPose.get().toPose2d(), new Transform2d()); - // } - Optional visionEstimate = visionEstimator.update(); if (visionEstimate.isEmpty()) return; @@ -347,10 +309,10 @@ private Command feedForwardDrive( public Command teleopDrive(XboxController controller, boolean fieldCentric) { return this.feedForwardDrive( - () -> 0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.2), - () -> -0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.2), + () -> 0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.05), + () -> -0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.05), () -> - -0.3 * kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.2), + -0.3 * kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.05), () -> fieldCentric); } From 37bfac570993384d727bfac8ea4baedfb649bb1a Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Wed, 14 Feb 2024 15:13:31 -0500 Subject: [PATCH 14/15] added robot relative note pose calculation, not perfect due to camera intrinsics but works for small yaw --- .../org/robolancers321/BuildConstants.java | 10 ++--- .../subsystems/drivetrain/Drivetrain.java | 38 +++++++++++++++++-- 2 files changed, 39 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java index 4d1c9c0..f888ee8 100644 --- a/src/main/java/org/robolancers321/BuildConstants.java +++ b/src/main/java/org/robolancers321/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "321-Crescendo-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 18; - public static final String GIT_SHA = "48eca2c536a83baaae96767336266104a1f0666a"; - public static final String GIT_DATE = "2024-02-12 15:25:32 EST"; + public static final int GIT_REVISION = 19; + public static final String GIT_SHA = "ff080c1eff567a1ee3b838e24c1805a5aa804045"; + public static final String GIT_DATE = "2024-02-12 16:11:49 EST"; public static final String GIT_BRANCH = "dev-drivetrain"; - public static final String BUILD_DATE = "2024-02-12 16:09:22 EST"; - public static final long BUILD_UNIX_TIME = 1707772162064L; + public static final String BUILD_DATE = "2024-02-13 15:19:49 EST"; + public static final long BUILD_UNIX_TIME = 1707855589176L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 29d6307..b10705c 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -37,6 +37,9 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.proto.Photon; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; public class Drivetrain extends SubsystemBase { /* @@ -55,7 +58,8 @@ public static Drivetrain getInstance() { * Constants */ - private static final String kCameraName = "MainCamera"; + private static final String kMainCameraName = "MainCamera"; + private static final String kNoteCameraName = "NoteCamera"; private static final AprilTagFieldLayout kAprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); @@ -63,6 +67,9 @@ public static Drivetrain getInstance() { // TODO: find this transform private static final Transform3d kRobotToCameraTransform = new Transform3d(0, 0, 0, new Rotation3d(0, Math.PI, 0)); + + private static final double kNoteCameraMountHeight = Units.inchesToMeters(11.0); // rough estimate of camera height while mounted on crate + private static final double kNoteCameraMountPitch = 0.0; // degrees w/r to the horizontal, above horizontal is positive private static final double kTrackWidthMeters = Units.inchesToMeters(17.5); private static final double kWheelBaseMeters = Units.inchesToMeters(17.5); @@ -102,11 +109,13 @@ public static Drivetrain getInstance() { private AHRS gyro; - private PhotonCamera camera; + private PhotonCamera mainCamera; private PhotonPoseEstimator visionEstimator; private SwerveDrivePoseEstimator odometry; + private PhotonCamera noteCamera; + private Field2d field; private Drivetrain() { @@ -118,19 +127,21 @@ private Drivetrain() { this.gyro = new AHRS(SPI.Port.kMXP); this.zeroYaw(); - this.camera = new PhotonCamera(kCameraName); + this.mainCamera = new PhotonCamera(kMainCameraName); this.visionEstimator = new PhotonPoseEstimator( kAprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - camera, + mainCamera, kRobotToCameraTransform); this.odometry = new SwerveDrivePoseEstimator( kSwerveKinematics, this.gyro.getRotation2d(), this.getModulePositions(), new Pose2d()); + this.noteCamera = new PhotonCamera(kNoteCameraName); + this.field = new Field2d(); SmartDashboard.putData(this.field); @@ -253,6 +264,20 @@ private void fuseVision() { visionEstimate.get().estimatedPose.toPose2d(), visionEstimate.get().timestampSeconds); } + private Translation2d getRelativeNoteLocation(){ + PhotonPipelineResult latestResult = this.noteCamera.getLatestResult(); + + if (!latestResult.hasTargets()) return new Translation2d(); + + PhotonTrackedTarget bestTarget = latestResult.getBestTarget(); + + // TODO: plus or minus mount pitch? + double dz = kNoteCameraMountHeight / Math.tan((-bestTarget.getPitch() + kNoteCameraMountPitch) * Math.PI / 180.0); + double dx = dz * Math.tan(bestTarget.getYaw() * Math.PI / 180.0); + + return new Translation2d(dx, dz); + } + private void doSendables() { SmartDashboard.putNumber("Drive Heading", this.getYawDeg()); @@ -261,6 +286,11 @@ private void doSendables() { SmartDashboard.putNumber("Odometry Pos X (m)", odometryPose.getX()); SmartDashboard.putNumber("Odometry Pos Y (m)", odometryPose.getY()); SmartDashboard.putNumber("Odometry Angle (deg)", odometryPose.getRotation().getDegrees()); + + Translation2d note = this.getRelativeNoteLocation(); + + SmartDashboard.putNumber("note x pffset", note.getX()); + SmartDashboard.putNumber("note z offset", note.getY()); } @Override From cedd9f1014e59b507cb93f9ed27b9562f2165714 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Sat, 17 Feb 2024 10:15:48 -0500 Subject: [PATCH 15/15] spotless (damn it raf) --- .../org/robolancers321/RobotContainer.java | 17 +- .../org/robolancers321/commands/Mate.java | 2 - .../subsystems/drivetrain/Drivetrain.java | 15 +- .../subsystems/intake/Retractor.java | 35 ++- .../subsystems/launcher/AimTable.java | 4 +- .../subsystems/launcher/Flywheel.java | 4 +- .../subsystems/launcher/Indexer.java | 1 - .../subsystems/launcher/Launcher.java | 10 - .../subsystems/launcher/Pivot.java | 27 +- .../java/org/robolancers321/AimTableTest.java | 259 +++++++++--------- 10 files changed, 186 insertions(+), 188 deletions(-) diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 76b1a4b..24f5908 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -2,11 +2,6 @@ package org.robolancers321; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; @@ -45,13 +40,17 @@ private void configureBindings() { new Trigger(this.driverController::getAButton).onTrue(this.drivetrain.zeroYaw()); - new Trigger(() -> this.driverController.getRightTriggerAxis() > 0.8).onTrue(this.intake.retractor.moveToIntake()); + new Trigger(() -> this.driverController.getRightTriggerAxis() > 0.8) + .onTrue(this.intake.retractor.moveToIntake()); - new Trigger(() -> this.driverController.getRightTriggerAxis() > 0.8).onFalse(this.intake.retractor.moveToRetracted()); + new Trigger(() -> this.driverController.getRightTriggerAxis() > 0.8) + .onFalse(this.intake.retractor.moveToRetracted()); - new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.8).onTrue(this.launcher.pivot.aimAtAmp()); + new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.8) + .onTrue(this.launcher.pivot.aimAtAmp()); - new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.8).onFalse(this.launcher.pivot.moveToRetracted()); + new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.8) + .onFalse(this.launcher.pivot.moveToRetracted()); } private void configureAutoChooser() { diff --git a/src/main/java/org/robolancers321/commands/Mate.java b/src/main/java/org/robolancers321/commands/Mate.java index 964286b..a161fae 100644 --- a/src/main/java/org/robolancers321/commands/Mate.java +++ b/src/main/java/org/robolancers321/commands/Mate.java @@ -1,8 +1,6 @@ /* (C) Robolancers 2024 */ package org.robolancers321.commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import java.util.function.DoubleSupplier; import org.robolancers321.subsystems.intake.Intake; diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index b10705c..752f55b 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -37,7 +37,6 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.proto.Photon; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -67,9 +66,11 @@ public static Drivetrain getInstance() { // TODO: find this transform private static final Transform3d kRobotToCameraTransform = new Transform3d(0, 0, 0, new Rotation3d(0, Math.PI, 0)); - - private static final double kNoteCameraMountHeight = Units.inchesToMeters(11.0); // rough estimate of camera height while mounted on crate - private static final double kNoteCameraMountPitch = 0.0; // degrees w/r to the horizontal, above horizontal is positive + + private static final double kNoteCameraMountHeight = + Units.inchesToMeters(11.0); // rough estimate of camera height while mounted on crate + private static final double kNoteCameraMountPitch = + 0.0; // degrees w/r to the horizontal, above horizontal is positive private static final double kTrackWidthMeters = Units.inchesToMeters(17.5); private static final double kWheelBaseMeters = Units.inchesToMeters(17.5); @@ -264,7 +265,7 @@ private void fuseVision() { visionEstimate.get().estimatedPose.toPose2d(), visionEstimate.get().timestampSeconds); } - private Translation2d getRelativeNoteLocation(){ + private Translation2d getRelativeNoteLocation() { PhotonPipelineResult latestResult = this.noteCamera.getLatestResult(); if (!latestResult.hasTargets()) return new Translation2d(); @@ -272,7 +273,9 @@ private Translation2d getRelativeNoteLocation(){ PhotonTrackedTarget bestTarget = latestResult.getBestTarget(); // TODO: plus or minus mount pitch? - double dz = kNoteCameraMountHeight / Math.tan((-bestTarget.getPitch() + kNoteCameraMountPitch) * Math.PI / 180.0); + double dz = + kNoteCameraMountHeight + / Math.tan((-bestTarget.getPitch() + kNoteCameraMountPitch) * Math.PI / 180.0); double dx = dz * Math.tan(bestTarget.getYaw() * Math.PI / 180.0); return new Translation2d(dx, dz); diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index da20401..ca99eb3 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -8,7 +8,6 @@ import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder.Type; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -41,7 +40,7 @@ public static Retractor getInstance() { private static final boolean kInvertEncoder = true; private static final int kCurrentLimit = 40; - + private static final double kGearRatio = 360.0; private static final float kMinAngle = -20.0f; @@ -105,7 +104,7 @@ private void configureMotor() { // this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle); // this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); // this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); - + this.motor.enableSoftLimit(SoftLimitDirection.kForward, false); this.motor.enableSoftLimit(SoftLimitDirection.kReverse, false); } @@ -126,14 +125,16 @@ private void configureController() { this.m_enabled = true; } - + @Override public double getMeasurement() { return this.getPositionDeg(); } public double getPositionDeg() { - return this.encoder.getPosition() > 180 ? this.encoder.getPosition() - 360.0 : this.encoder.getPosition(); + return this.encoder.getPosition() > 180 + ? this.encoder.getPosition() - 360.0 + : this.encoder.getPosition(); } public double getVelocityDeg() { @@ -147,7 +148,8 @@ private boolean atGoal() { @Override protected void useOutput(double output, TrapezoidProfile.State setpoint) { double feedforwardOutput = - this.feedforwardController.calculate(setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0); + this.feedforwardController.calculate( + setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0); SmartDashboard.putNumber("retractor position setpoint mp (deg)", setpoint.position); SmartDashboard.putNumber("retractor velocity setpoint mp (deg)", setpoint.velocity); @@ -170,7 +172,7 @@ private void doSendables() { SmartDashboard.putNumber("retractor position (deg)", this.getPositionDeg()); SmartDashboard.putNumber("retractor velocity (deg)", this.getVelocityDeg()); } - + @Override public void periodic() { super.periodic(); @@ -187,8 +189,12 @@ private void initTuning() { SmartDashboard.putNumber("retractor kv", SmartDashboard.getNumber("retractor kv", kV)); SmartDashboard.putNumber("retractor kg", SmartDashboard.getNumber("retractor kg", kG)); - SmartDashboard.putNumber("retractor max vel (deg)", SmartDashboard.getNumber("retractor max vel (deg)", kMaxVelocityDeg)); - SmartDashboard.putNumber("retractor max acc (deg)", SmartDashboard.getNumber("retractor max acc (deg)", kMaxAccelerationDeg)); + SmartDashboard.putNumber( + "retractor max vel (deg)", + SmartDashboard.getNumber("retractor max vel (deg)", kMaxVelocityDeg)); + SmartDashboard.putNumber( + "retractor max acc (deg)", + SmartDashboard.getNumber("retractor max acc (deg)", kMaxAccelerationDeg)); SmartDashboard.putNumber("retractor target position (deg)", this.getPositionDeg()); } @@ -211,11 +217,16 @@ private void tune() { this.m_controller.setConstraints(new Constraints(tunedMaxVel, tunedMaxAcc)); - this.setGoal(MathUtil.clamp(SmartDashboard.getNumber("retractor target position (deg)", this.getPositionDeg()), kMinAngle, kMaxAngle)); + this.setGoal( + MathUtil.clamp( + SmartDashboard.getNumber("retractor target position (deg)", this.getPositionDeg()), + kMinAngle, + kMaxAngle)); } - + private Command moveToAngle(double angleDeg) { - return run(() -> this.setGoal(MathUtil.clamp(angleDeg, kMinAngle, kMaxAngle))).until(this::atGoal); + return run(() -> this.setGoal(MathUtil.clamp(angleDeg, kMinAngle, kMaxAngle))) + .until(this::atGoal); } public Command moveToRetracted() { diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index cb11f6c..cec7c07 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java @@ -42,12 +42,12 @@ private static AimCharacteristic calculateSpeakerAimCharacteristic(double distan double lowerBound = keys.get(0); double upperBound = keys.get(keys.size() - 1); - if (distance < lowerBound){ + if (distance < lowerBound) { AimTable.AimCharacteristic lowerValue = table.get(lowerBound); return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm); } - + if (distance > upperBound) { AimTable.AimCharacteristic upperValue = table.get(upperBound); diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index 744df0e..bb1d127 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -5,8 +5,6 @@ import com.revrobotics.*; import com.revrobotics.CANSparkBase.ControlType; - -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -40,7 +38,7 @@ private enum FlywheelSetpoint { public final double rpm; - private FlywheelSetpoint(double rpm){ + private FlywheelSetpoint(double rpm) { this.rpm = rpm; } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 8cf2bdc..7dee95f 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -5,7 +5,6 @@ import com.revrobotics.*; import com.revrobotics.CANSparkBase.ControlType; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index 6d4f18a..0c1e3d5 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java @@ -1,17 +1,7 @@ /* (C) Robolancers 2024 */ package org.robolancers321.subsystems.launcher; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import java.util.function.DoubleSupplier; public class Launcher extends SubsystemBase { /* diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 18479c3..6306f63 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -6,7 +6,6 @@ import com.revrobotics.*; import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.SparkAbsoluteEncoder.Type; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -129,9 +128,11 @@ private void configureController() { public double getMeasurement() { return this.getPositionDeg(); } - + public double getPositionDeg() { - return this.encoder.getPosition() > 180 ? this.encoder.getPosition() - 360.0 : this.encoder.getPosition(); + return this.encoder.getPosition() > 180 + ? this.encoder.getPosition() - 360.0 + : this.encoder.getPosition(); } public double getVelocityDeg() { @@ -145,7 +146,8 @@ private boolean atGoal() { @Override protected void useOutput(double output, TrapezoidProfile.State setpoint) { double feedforwardOutput = - this.feedforwardController.calculate(setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0); + this.feedforwardController.calculate( + setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0); SmartDashboard.putNumber("pivot position setpoint mp (deg)", setpoint.position); SmartDashboard.putNumber("pivot velocity setpoint mp (deg)", setpoint.velocity); @@ -184,8 +186,11 @@ private void initTuning() { SmartDashboard.putNumber("pivot kg", SmartDashboard.getNumber("pivot kg", kG)); SmartDashboard.putNumber("pivot kv", SmartDashboard.getNumber("pivot kv", kV)); - SmartDashboard.putNumber("pivot max vel (deg)", SmartDashboard.getNumber("pivot max vel (deg)", kMaxVelocityDeg)); - SmartDashboard.putNumber("pivot max acc (deg)", SmartDashboard.getNumber("pivot max acc (deg)", kMaxAccelerationDeg)); + SmartDashboard.putNumber( + "pivot max vel (deg)", SmartDashboard.getNumber("pivot max vel (deg)", kMaxVelocityDeg)); + SmartDashboard.putNumber( + "pivot max acc (deg)", + SmartDashboard.getNumber("pivot max acc (deg)", kMaxAccelerationDeg)); SmartDashboard.putNumber("pivot target position (deg)", this.getPositionDeg()); } @@ -208,11 +213,17 @@ private void tune() { this.m_controller.setConstraints(new Constraints(tunedMaxVel, tunedMaxAcc)); - this.setGoal(MathUtil.clamp(SmartDashboard.getNumber("pivot target position (deg)", this.getPositionDeg()), kMinAngle, kMaxAngle)); + this.setGoal( + MathUtil.clamp( + SmartDashboard.getNumber("pivot target position (deg)", this.getPositionDeg()), + kMinAngle, + kMaxAngle)); } private Command moveToAngle(DoubleSupplier angleDegSupplier) { - return run(() -> this.setGoal(MathUtil.clamp(angleDegSupplier.getAsDouble(), kMinAngle, kMaxAngle))).until(this::atGoal); + return run(() -> + this.setGoal(MathUtil.clamp(angleDegSupplier.getAsDouble(), kMinAngle, kMaxAngle))) + .until(this::atGoal); } private Command moveToAngle(double angleDeg) { diff --git a/src/test/java/org/robolancers321/AimTableTest.java b/src/test/java/org/robolancers321/AimTableTest.java index 674a70d..90f8f37 100644 --- a/src/test/java/org/robolancers321/AimTableTest.java +++ b/src/test/java/org/robolancers321/AimTableTest.java @@ -1,169 +1,158 @@ +/* (C) Robolancers 2024 */ package org.robolancers321; -import static java.lang.System.in; import static org.junit.jupiter.api.Assertions.assertArrayEquals; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.robolancers321.util.MathUtils.epsilonEquals; +import java.util.*; import org.junit.jupiter.api.Test; import org.robolancers321.subsystems.launcher.AimTable; -import java.util.*; - - public class AimTableTest { - private static final LinkedHashMap table = - new LinkedHashMap<>() { - { - put(0.0, new AimTable.AimCharacteristic(0, 0.0)); - put(1.0, new AimTable.AimCharacteristic(10, 1020)); - put(2.0, new AimTable.AimCharacteristic(20, 1500)); - put(4.0, new AimTable.AimCharacteristic(40, 2500)); - put(5.0, new AimTable.AimCharacteristic(54, 3300)); - put(6.0, new AimTable.AimCharacteristic(61, 4227)); - put(8.0, new AimTable.AimCharacteristic(83, 5599)); - } - }; - - private class DummyAimTable { - private AimTable.AimCharacteristic lastAimCharacteristic; - - private final double lastDistance = 0; - - private double kInterpolationCacheThreshold = 0.0; - - private static double interpolate( - double lowKey, double lowValue, double highKey, double highValue, double x) { - double percent = (x - lowKey) / (highKey - lowKey); - - return lowValue + percent * (highValue - lowValue); - } - - public static AimTable.AimCharacteristic getSpeakerAimCharacteristic(double distance) { - List keys = table.keySet().stream().sorted().toList(); - double lowerBound = keys.get(0); - double upperBound = keys.get(keys.size() - 1); - - if ((distance < lowerBound)) { - AimTable.AimCharacteristic lowerValue = table.get(lowerBound); - - return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm); - } - else if (distance > upperBound) { - AimTable.AimCharacteristic upperValue = table.get(upperBound); - - return new AimTable.AimCharacteristic(upperValue.angle, upperValue.rpm); - } - - System.out.println("Distance " + distance); - - NavigableSet values = new TreeSet<>(keys); - double lowKey = values.floor(distance); - double highKey = values.ceiling(distance); - - return new AimTable.AimCharacteristic( - interpolate(lowKey, table.get(lowKey).angle, highKey, table.get(highKey).angle, distance), - interpolate(lowKey, table.get(lowKey).rpm, highKey, table.get(highKey).rpm, distance)); - } - - public AimTable.AimCharacteristic getLastAimCharacteristic(double distance) { - if (!epsilonEquals(lastDistance, distance, kInterpolationCacheThreshold)) { - lastAimCharacteristic = getSpeakerAimCharacteristic(lastDistance); - kInterpolationCacheThreshold = lastDistance; - } - - return lastAimCharacteristic; - } + private static final LinkedHashMap table = + new LinkedHashMap<>() { + { + put(0.0, new AimTable.AimCharacteristic(0, 0.0)); + put(1.0, new AimTable.AimCharacteristic(10, 1020)); + put(2.0, new AimTable.AimCharacteristic(20, 1500)); + put(4.0, new AimTable.AimCharacteristic(40, 2500)); + put(5.0, new AimTable.AimCharacteristic(54, 3300)); + put(6.0, new AimTable.AimCharacteristic(61, 4227)); + put(8.0, new AimTable.AimCharacteristic(83, 5599)); } + }; + private class DummyAimTable { + private AimTable.AimCharacteristic lastAimCharacteristic; - private List getKeys() { - return table.keySet().stream().sorted().toList(); - } - private static double interpolate( - double lowKey, double lowValue, double highKey, double highValue, double x) { - double percent = (x - lowKey) / (highKey - lowKey); + private final double lastDistance = 0; + + private double kInterpolationCacheThreshold = 0.0; + private static double interpolate( + double lowKey, double lowValue, double highKey, double highValue, double x) { + double percent = (x - lowKey) / (highKey - lowKey); - return lowValue + percent * (highValue - lowValue); + return lowValue + percent * (highValue - lowValue); } - @Test - void testKeys() { - var keys = getKeys(); - - var expectedKeys = new Double[] { - 0.0, - 1.0, - 2.0, - 4.0, - 5.0, - 6.0, - 8.0, - }; + public static AimTable.AimCharacteristic getSpeakerAimCharacteristic(double distance) { + List keys = table.keySet().stream().sorted().toList(); + double lowerBound = keys.get(0); + double upperBound = keys.get(keys.size() - 1); - assertArrayEquals(keys.toArray(new Double[0]), expectedKeys); - } - @Test - void testUpperBoundKey() { - var keys = getKeys(); - double upperBound = keys.get(keys.size() - 1); + if ((distance < lowerBound)) { + AimTable.AimCharacteristic lowerValue = table.get(lowerBound); - assertEquals(14.0, upperBound); - } + return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm); + } else if (distance > upperBound) { + AimTable.AimCharacteristic upperValue = table.get(upperBound); - @Test - void testLowerBoundKey(){ - var keys = getKeys(); - double lowerBound = keys.get(0); + return new AimTable.AimCharacteristic(upperValue.angle, upperValue.rpm); + } - assertEquals(-1.0, lowerBound); - } + System.out.println("Distance " + distance); - @Test - void findAdjacentKeysTest() { - NavigableSet values = new TreeSet<>(getKeys()); - double distance = 3.0; - double lowKey = values.floor(distance); - double highKey = values.ceiling(distance); + NavigableSet values = new TreeSet<>(keys); + double lowKey = values.floor(distance); + double highKey = values.ceiling(distance); - assertEquals(2.0, lowKey); - assertEquals(4.0, highKey); + return new AimTable.AimCharacteristic( + interpolate(lowKey, table.get(lowKey).angle, highKey, table.get(highKey).angle, distance), + interpolate(lowKey, table.get(lowKey).rpm, highKey, table.get(highKey).rpm, distance)); } - @Test - void interpolateTest() { - - double lowKey = 2.0; - double lowValue = 1500; - double highKey = 4.0; - double highValue = 2500; - double x = 3.0; + public AimTable.AimCharacteristic getLastAimCharacteristic(double distance) { + if (!epsilonEquals(lastDistance, distance, kInterpolationCacheThreshold)) { + lastAimCharacteristic = getSpeakerAimCharacteristic(lastDistance); + kInterpolationCacheThreshold = lastDistance; + } - assertEquals(2000, interpolate(lowKey, lowValue, highKey, highValue, x)); + return lastAimCharacteristic; } + } + private List getKeys() { + return table.keySet().stream().sorted().toList(); + } - private Double[] generateNumbers(int count, int range) { - Random rand = new Random(); - List nums = new ArrayList<>(); - for (int i = 0; i < count; i++) { - nums.add(rand.nextDouble(range + 1)); - } - return nums.toArray(new Double[0]); - } - @Test - void testAimTable() { - DummyAimTable aimTable = new DummyAimTable(); - Double[] nums = generateNumbers(20, 20); + private static double interpolate( + double lowKey, double lowValue, double highKey, double highValue, double x) { + double percent = (x - lowKey) / (highKey - lowKey); + return lowValue + percent * (highValue - lowValue); + } - for (Double num : nums) { - AimTable.AimCharacteristic characteristic = aimTable.getSpeakerAimCharacteristic(num); - System.out.println(num); - System.out.println(characteristic.angle + " " + characteristic.rpm); - } - } + @Test + void testKeys() { + var keys = getKeys(); + var expectedKeys = + new Double[] { + 0.0, 1.0, 2.0, 4.0, 5.0, 6.0, 8.0, + }; + assertArrayEquals(keys.toArray(new Double[0]), expectedKeys); + } + + @Test + void testUpperBoundKey() { + var keys = getKeys(); + double upperBound = keys.get(keys.size() - 1); + + assertEquals(14.0, upperBound); + } + + @Test + void testLowerBoundKey() { + var keys = getKeys(); + double lowerBound = keys.get(0); + + assertEquals(-1.0, lowerBound); + } + + @Test + void findAdjacentKeysTest() { + NavigableSet values = new TreeSet<>(getKeys()); + double distance = 3.0; + double lowKey = values.floor(distance); + double highKey = values.ceiling(distance); + + assertEquals(2.0, lowKey); + assertEquals(4.0, highKey); + } + + @Test + void interpolateTest() { + + double lowKey = 2.0; + double lowValue = 1500; + double highKey = 4.0; + double highValue = 2500; + double x = 3.0; + + assertEquals(2000, interpolate(lowKey, lowValue, highKey, highValue, x)); + } + + private Double[] generateNumbers(int count, int range) { + Random rand = new Random(); + List nums = new ArrayList<>(); + for (int i = 0; i < count; i++) { + nums.add(rand.nextDouble(range + 1)); + } + return nums.toArray(new Double[0]); + } + + @Test + void testAimTable() { + DummyAimTable aimTable = new DummyAimTable(); + Double[] nums = generateNumbers(20, 20); + + for (Double num : nums) { + AimTable.AimCharacteristic characteristic = aimTable.getSpeakerAimCharacteristic(num); + System.out.println(num); + System.out.println(characteristic.angle + " " + characteristic.rpm); + } + } }