From ea232b5810a8011dc36dd254d27aff702dc1f375 Mon Sep 17 00:00:00 2001 From: mmilunicmobile Date: Thu, 12 Dec 2024 01:13:10 -0500 Subject: [PATCH] Bound the WheelRadiusCharacterization command to the trigger for testing --- src/main/java/frc/robot/RobotContainer.java | 3 +++ .../frc/robot/subsystems/CommandSwerveDrivetrain.java | 9 +-------- .../robot/subsystems/WheelRadiusCharacterization.java | 6 ++++-- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5c1f83f0..e4ad7b70 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.constants.TunerConstants; import frc.robot.constants.GlobalConstants.ControllerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.WheelRadiusCharacterization; public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -74,6 +75,8 @@ private void configureBindings() { point.withModuleDirection(new Rotation2d(-operatorController.getLeftYAxis().get(), -operatorController.getLeftXAxis().get())) )); + leftDriveController.getTrigger().whileTrue(new WheelRadiusCharacterization(WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain)); + // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. operatorController.getBack().and(operatorController.getY()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 471bc230..6619ad5c 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -294,14 +294,7 @@ public SwerveRequest driveRobotRelative(double xVelocity, double yVelocity, doub // Note: it is important to not discretize speeds before or after // using the setpoint generator, as it will discretize them for you ChassisSpeeds speeds = new ChassisSpeeds(xVelocity, yVelocity, rotationRate); - previousSetpoint = setpointGenerator.generateSetpoint( - previousSetpoint, // The previous setpoint - speeds, // The desired target speeds - 0.02 // The loop time of the robot code, in seconds - ); - return m_applyRobotSpeeds.withSpeeds(previousSetpoint.robotRelativeSpeeds()) - .withWheelForceFeedforwardsX(previousSetpoint.feedforwards().robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(previousSetpoint.feedforwards().robotRelativeForcesYNewtons()); + return m_applyFieldSpeedsOrbit.withChassisSpeeds(speeds); // Method that will drive the robot given target module states } diff --git a/src/main/java/frc/robot/subsystems/WheelRadiusCharacterization.java b/src/main/java/frc/robot/subsystems/WheelRadiusCharacterization.java index 267df21c..495be288 100644 --- a/src/main/java/frc/robot/subsystems/WheelRadiusCharacterization.java +++ b/src/main/java/frc/robot/subsystems/WheelRadiusCharacterization.java @@ -16,7 +16,7 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; -public class WheelRadiusCharacterization { +public class WheelRadiusCharacterization extends Command { private static final LoggedNetworkNumber characterizationSpeed = new LoggedNetworkNumber("WheelRadiusCharacterization/SpeedRadsPerSec", 0.1); private static final Pigeon2 pigeon = new Pigeon2(0,""); @@ -45,11 +45,13 @@ private Direction(double speed) { private final CommandSwerveDrivetrain drive; - private double[] startWheelPositions; + private double[] startWheelPositions = new double[4]; public WheelRadiusCharacterization(Direction omegaDirection, CommandSwerveDrivetrain drivetrain) { this.omegaDirection = omegaDirection; this.drive = drivetrain; + + addRequirements(drivetrain); }