From be1b79b712de39844a6673e5fc363fc0ce6e643b Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 17 Jan 2024 15:27:21 -0600 Subject: [PATCH] Fixed the conversion factor for talonfx's. Added new command generators. Signed-off-by: thenetworkgrinch --- .../falcon/modules/physicalproperties.json | 4 +- .../swervedrive/SwerveSubsystem.java | 59 ++++++++++++++++--- .../java/swervelib/motors/TalonFXSwerve.java | 2 +- 3 files changed, 54 insertions(+), 11 deletions(-) diff --git a/src/main/deploy/swerve/falcon/modules/physicalproperties.json b/src/main/deploy/swerve/falcon/modules/physicalproperties.json index d09b181bc..8d11c0275 100644 --- a/src/main/deploy/swerve/falcon/modules/physicalproperties.json +++ b/src/main/deploy/swerve/falcon/modules/physicalproperties.json @@ -1,7 +1,7 @@ { "conversionFactor": { - "angle": 0.01373291015625, - "drive": 1.914649238933429E-5 + "angle": 28.125, + "drive": 0.047286787200699704 }, "currentLimit": { "drive": 40, diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 35a381913..0e887a483 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -11,6 +11,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.io.File; +import java.util.function.DoubleSupplier; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.math.SwerveMath; @@ -76,6 +78,7 @@ public SwerveSubsystem(File directory) } swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setGyroOffset(new Rotation3d(0, 0, Units.degreesToRadians(90))); setupPathPlanner(); } @@ -104,12 +107,12 @@ public void setupPathPlanner() // Default path replanning config. See the API for the options here ), () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - }, + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + }, this // Reference to this subsystem to set requirements ); } @@ -135,6 +138,45 @@ public Command getAutonomousCommand(String pathName, boolean setOdomToStart) return AutoBuilder.followPath(path); } + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. + * @param translationY Translation in the Y direction. + * @param headingX Heading X to calculate angle of the joystick. + * @param headingY Heading Y to calculate angle of the joystick. + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, + DoubleSupplier headingY) + { + return run(() -> { + // Make the robot move + driveFieldOriented(getTargetSpeeds(translationX.getAsDouble(), translationY.getAsDouble(), + headingX.getAsDouble(), + headingY.getAsDouble())); + }); + } + + /** + * Command to drive the robot using translative values and heading as angular velocity. + * + * @param translationX Translation in the X direction. + * @param translationY Translation in the Y direction. + * @param angularRotationX Rotation of the robot to set + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) + { + return run(() -> { + // Make the robot move + swerveDrive.drive(new Translation2d(translationX.getAsDouble() * maximumSpeed, translationY.getAsDouble()), + angularRotationX.getAsDouble() * swerveDrive.swerveController.config.maxAngularVelocity, + true, + false); + }); + } + /** * Construct the swerve drive. * @@ -301,7 +343,8 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin } /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. + * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of + * 90deg. * * @param xInput X joystick input for the robot to move in the X direction. * @param yInput Y joystick input for the robot to move in the Y direction. @@ -315,7 +358,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an return swerveDrive.swerveController.getTargetSpeeds(xInput, yInput, angle.getRadians(), - getHeading().getRadians(), + getHeading().getRadians() - Units.degreesToRadians(90), maximumSpeed); } diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index fab3a016d..e112d6061 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -154,7 +154,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration); - positionConversionFactor = 1 / positionConversionFactor; + // positionConversionFactor = 1 / positionConversionFactor; configuration.MotionMagic = configuration.MotionMagic .withMotionMagicCruiseVelocity(100 / positionConversionFactor)