From c1205ce7ae40db9eb675ac9e03b8322ff4f65f26 Mon Sep 17 00:00:00 2001 From: mmilunicmobile Date: Sat, 21 Dec 2024 11:19:44 -0500 Subject: [PATCH 1/5] formatted like 3 files --- src/main/java/frc/robot/RobotContainer.java | 197 +++--- .../subsystems/CommandSwerveDrivetrain.java | 595 +++++++++--------- .../FieldOrientedOrbitSwerveRequest.java | 321 +++++----- 3 files changed, 596 insertions(+), 517 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ca46494..5e5def49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,96 +15,129 @@ import frc.lib.controller.LogitechController; import frc.lib.controller.ThrustmasterJoystick; import frc.robot.constants.GlobalConstants; -import frc.robot.constants.TunerConstants; import frc.robot.constants.GlobalConstants.ControllerConstants; +import frc.robot.constants.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.WheelRadiusCharacterization; public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - - private final ThrustmasterJoystick leftDriveController = - new ThrustmasterJoystick(ControllerConstants.LEFT_DRIVE_CONTROLLER); - private final ThrustmasterJoystick rightDriveController = - new ThrustmasterJoystick(ControllerConstants.RIGHT_DRIVE_CONTROLLER); - private final LogitechController operatorController = - new LogitechController(ControllerConstants.OPERATOR_CONTROLLER); - - private final Telemetry logger = new Telemetry(MaxSpeed); - - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - - public Auto auto = new Auto(drivetrain); - - // Use open-loop control for drive motors - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - public RobotContainer() { - configureBindings(); - - drivetrain.setUpPathPlanner(); - // Establish the "Trajectory Field" Field2d into the dashboard - } - - private void configureBindings() { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically + private double MaxSpeed = + GlobalConstants.MAX_TRANSLATIONAL_SPEED.in( + MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = + GlobalConstants.MAX_ROTATIONAL_SPEED.in( + RadiansPerSecond); // kMaxAngularRate desired top rotational speed + + private final ThrustmasterJoystick leftDriveController = + new ThrustmasterJoystick(ControllerConstants.LEFT_DRIVE_CONTROLLER); + private final ThrustmasterJoystick rightDriveController = + new ThrustmasterJoystick(ControllerConstants.RIGHT_DRIVE_CONTROLLER); + private final LogitechController operatorController = + new LogitechController(ControllerConstants.OPERATOR_CONTROLLER); + + private final Telemetry logger = new Telemetry(MaxSpeed); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + + public Auto auto = new Auto(drivetrain); + + // Use open-loop control for drive motors + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + public RobotContainer() { + configureBindings(); + + drivetrain.setUpPathPlanner(); + // Establish the "Trajectory Field" Field2d into the dashboard + } + + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest( + () -> { + ChassisSpeeds driverDesiredSpeeds = + new ChassisSpeeds( + GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond) + * sps(deadband(leftDriveController.getYAxis().get(), 0.1)), + sps(deadband(leftDriveController.getXAxis().get(), 0.1)) + * GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond), + -sps(deadband(rightDriveController.getXAxis().get(), 0.1)) + * GlobalConstants.MAX_ROTATIONAL_SPEED.in(RadiansPerSecond)); + return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds(driverDesiredSpeeds); + // return drivetrain.m_applyFieldSpeeds.withSpeeds(driverDesiredSpeeds); + })); + + // drive.withVelocityX(-leftDriveController.getYAxis().get() * + // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward) + // .withVelocityY(-leftDriveController.getXAxis().get() * + // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left) + // .withRotationalRate(-rightDriveController.getXAxis().get() * + // GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left) + + operatorController.getA().whileTrue(drivetrain.applyRequest(() -> brake)); + operatorController + .getB() + .whileTrue( drivetrain.applyRequest( - () -> { - ChassisSpeeds driverDesiredSpeeds = new ChassisSpeeds( - GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond) * sps(deadband(leftDriveController.getYAxis().get(), 0.1)), - sps(deadband(leftDriveController.getXAxis().get(),0.1)) * GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond), - -sps(deadband(rightDriveController.getXAxis().get(),0.1)) * GlobalConstants.MAX_ROTATIONAL_SPEED.in(RadiansPerSecond) - ); - return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds(driverDesiredSpeeds); - // return drivetrain.m_applyFieldSpeeds.withSpeeds(driverDesiredSpeeds); - } - ) - ); - - // drive.withVelocityX(-leftDriveController.getYAxis().get() * GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward) - // .withVelocityY(-leftDriveController.getXAxis().get() * GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left) - // .withRotationalRate(-rightDriveController.getXAxis().get() * GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left) - - operatorController.getA().whileTrue(drivetrain.applyRequest(() -> brake)); - operatorController.getB().whileTrue(drivetrain.applyRequest(() -> - 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)); - operatorController.getBack().and(operatorController.getX()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - operatorController.getStart().and(operatorController.getY()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - operatorController.getStart().and(operatorController.getX()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - // reset the field-centric heading on left bumper press - operatorController.getLeftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - operatorController.getRightBumper().onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d()))); - - drivetrain.registerTelemetry(logger::telemeterize); - } - - private double deadband(double value, double deadband) { - if (value <= deadband && -deadband <= value) { - return 0; - } - - return value; + () -> + 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)); + operatorController + .getBack() + .and(operatorController.getX()) + .whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + operatorController + .getStart() + .and(operatorController.getY()) + .whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + operatorController + .getStart() + .and(operatorController.getX()) + .whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + // reset the field-centric heading on left bumper press + operatorController + .getLeftBumper() + .onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + operatorController + .getRightBumper() + .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d()))); + + drivetrain.registerTelemetry(logger::telemeterize); + } + + private double deadband(double value, double deadband) { + if (value <= deadband && -deadband <= value) { + return 0; } - private double sps(double value) { - return value * Math.abs(value); - } + return value; + } - public Command getAutonomousCommand() { - return auto.getAuto(); - } + private double sps(double value) { + return value * Math.abs(value); + } + public Command getAutonomousCommand() { + return auto.getAuto(); + } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index e0c20cbb..22786cdf 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -2,30 +2,19 @@ import static edu.wpi.first.units.Units.*; -import java.lang.annotation.Inherited; -import java.util.Optional; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -40,311 +29,327 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.RobotContainer; import frc.robot.constants.GlobalConstants; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; /** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily + * be used in command-based projects. */ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - private final SwerveRequest.ApplyRobotSpeeds m_applyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds() - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo) - .withDesaturateWheelSpeeds(false); - - public final SwerveRequest.ApplyFieldSpeeds m_applyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds(); - - public final FieldOrientedOrbitSwerveRequest m_applyFieldSpeedsOrbit; - RobotConfig config; // PathPlanner robot configuration - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = + new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = + new SwerveRequest.SysIdSwerveRotation(); + + private final SwerveRequest.ApplyRobotSpeeds m_applyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds() + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withDesaturateWheelSpeeds(false); + + public final SwerveRequest.ApplyFieldSpeeds m_applyFieldSpeeds = + new SwerveRequest.ApplyFieldSpeeds(); + + public final FieldOrientedOrbitSwerveRequest m_applyFieldSpeedsOrbit; + RobotConfig config; // PathPlanner robot configuration + + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private final SysIdRoutine m_sysIdRoutineTranslation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); + + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + private final SysIdRoutine m_sysIdRoutineSteer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); + + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private final SysIdRoutine m_sysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { /* output is actually radians per second, but SysId only supports "volts" */ setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); /* also log the requested output for SysId */ SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /* The Setpoint Generator */ - private SwerveSetpointGenerator setpointGenerator; - private SwerveSetpoint previousSetpoint; - - public void setUpPathPlanner() { - - - } - - public Pose2d getRobotPose() { - return this.getState().Pose; - } - - public ChassisSpeeds getChassisSpeeds() { - return this.getState().Speeds; + }, + null, + this)); + + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + + /* The Setpoint Generator */ + private SwerveSetpointGenerator setpointGenerator; + private SwerveSetpoint previousSetpoint; + + public void setUpPathPlanner() {} + + public Pose2d getRobotPose() { + return this.getState().Pose; + } + + public ChassisSpeeds getChassisSpeeds() { + return this.getState().Speeds; + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them - * through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to + * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double OdometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(drivetrainConstants, OdometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them - * through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { - super(drivetrainConstants, OdometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - - m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * @param visionStandardDeviation The standard deviation for vision calculation - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, - Matrix odometryStandardDeviation, Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - - m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to + * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * @param visionStandardDeviation The standard deviation for vision calculation + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + if (Utils.isSimulation()) { + startSimThread(); } - private FieldOrientedOrbitSwerveRequest generateSwerveSetpointConfig() - { - RobotConfig config = GlobalConstants.getRobotConfigPathplanner(); - - setpointGenerator = new SwerveSetpointGenerator( - config, - Units.rotationsToRadians(10.0) //max rotational speed - ); - - ChassisSpeeds currentSpeeds = getState().Speeds; - SwerveModuleState[] currentStates = getState().ModuleStates; - SwerveSetpoint previousSetpoint = new SwerveSetpoint(currentSpeeds, currentStates, DriveFeedforwards.zeros(config.numModules)); - - var request = new FieldOrientedOrbitSwerveRequest(setpointGenerator, previousSetpoint, getState().Pose.getRotation()); - request.withDriverOrientation(true); - return request; - } - - // - //The desired robot-relative speeds - //returns the module states where robot can drive while obeying physics and not slipping - public SwerveRequest driveRobotRelative(ChassisSpeeds speeds) { - // Note: it is important to not discretize speeds before or after - // using the setpoint generator, as it will discretize them for you - previousSetpoint = setpointGenerator.generateSetpoint( + m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + } + + private FieldOrientedOrbitSwerveRequest generateSwerveSetpointConfig() { + RobotConfig config = GlobalConstants.getRobotConfigPathplanner(); + + setpointGenerator = + new SwerveSetpointGenerator( + config, Units.rotationsToRadians(10.0) // max rotational speed + ); + + ChassisSpeeds currentSpeeds = getState().Speeds; + SwerveModuleState[] currentStates = getState().ModuleStates; + SwerveSetpoint previousSetpoint = + new SwerveSetpoint( + currentSpeeds, currentStates, DriveFeedforwards.zeros(config.numModules)); + + var request = + new FieldOrientedOrbitSwerveRequest( + setpointGenerator, previousSetpoint, getState().Pose.getRotation()); + request.withDriverOrientation(true); + return request; + } + + // + // The desired robot-relative speeds + // returns the module states where robot can drive while obeying physics and not slipping + public SwerveRequest driveRobotRelative(ChassisSpeeds speeds) { + // Note: it is important to not discretize speeds before or after + // using the setpoint generator, as it will discretize them for you + 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()); - // Method that will drive the robot given target module states - } - - public SwerveRequest driveRobotRelative(double xVelocity, double yVelocity, double rotationRate) { - // 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); - return m_applyFieldSpeedsOrbit.withChassisSpeeds(speeds); - // Method that will drive the robot given target module states - } - - public SwerveRequest driveFieldRelative(ChassisSpeeds speeds) { - ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getState().Pose.getRotation()); - return driveRobotRelative(speeds); - } + ); + return m_applyRobotSpeeds + .withSpeeds(previousSetpoint.robotRelativeSpeeds()) + .withWheelForceFeedforwardsX(previousSetpoint.feedforwards().robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(previousSetpoint.feedforwards().robotRelativeForcesYNewtons()); + // Method that will drive the robot given target module states + } + + public SwerveRequest driveRobotRelative(double xVelocity, double yVelocity, double rotationRate) { + // 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); + return m_applyFieldSpeedsOrbit.withChassisSpeeds(speeds); + // Method that will drive the robot given target module states + } + + public SwerveRequest driveFieldRelative(ChassisSpeeds speeds) { + ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getState().Pose.getRotation()); + return driveRobotRelative(speeds); + } + + public SwerveRequest driveWithFeedforwards(ChassisSpeeds speeds, DriveFeedforwards feedforwards) { + return m_applyRobotSpeeds + .withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()); + } + + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + /** + * Runs the SysId Quasistatic test in the given direction for the routine specified by {@link + * #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + /** + * Runs the SysId Dynamic test in the given direction for the routine specified by {@link + * #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + @Override + public void periodic() { - public SwerveRequest driveWithFeedforwards(ChassisSpeeds speeds, DriveFeedforwards feedforwards) { - return m_applyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()); - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - @Override - public void periodic() { - - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance() + .ifPresent( + allianceColor -> { setOperatorPerspectiveForward( allianceColor == Alliance.Red ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); + : kBlueAlliancePerspectiveRotation); m_hasAppliedOperatorPerspective = true; - }); - } - - Logger.recordOutput("Drive/desiredChassisSpeeds", m_applyFieldSpeedsOrbit.getChassisSpeeds()); - Logger.recordOutput("Drive/setpointChassisSpeeds", m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds()); - - Logger.recordOutput("Drive/desiredWheelSpeeds", getState().Speeds); + }); } - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } + Logger.recordOutput("Drive/desiredChassisSpeeds", m_applyFieldSpeedsOrbit.getChassisSpeeds()); + Logger.recordOutput( + "Drive/slewedChassisSpeeds", m_applyFieldSpeedsOrbit.getSlewedFieldChassisSpeeds()); + Logger.recordOutput( + "Drive/setpointChassisSpeeds", + m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds()); + + Logger.recordOutput("Drive/ModuleTargets", getState().ModuleTargets); + Logger.recordOutput("Drive/ModulePositions", getState().ModulePositions); + Logger.recordOutput("Drive/ModuleStates", getState().ModuleStates); + + Logger.recordOutput("Drive/actualChassisSpeeds", getState().Speeds); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } } diff --git a/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java b/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java index ccd16dfa..929c134c 100644 --- a/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java +++ b/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java @@ -9,154 +9,195 @@ import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; - public class FieldOrientedOrbitSwerveRequest implements SwerveRequest { - private final SwerveRequest.ApplyRobotSpeeds applyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds() - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo) - .withDesaturateWheelSpeeds(false); - - private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - - private SwerveSetpointGenerator setpointGenerator; - private SwerveSetpoint previousSetpoint; - private ChassisSpeeds slewedFieldChassisSpeeds = new ChassisSpeeds(); - - private boolean useDriverOrientation = true; - - private double forwardXRateLimit = Double.POSITIVE_INFINITY; - private double backwardXRateLimit = Double.POSITIVE_INFINITY; - - private double forwardYRateLimit = Double.POSITIVE_INFINITY; - private double backwardYRateLimit = Double.POSITIVE_INFINITY; - - private double timestep = 0.004; - - private boolean maintainStraightStopping = true; - - public ChassisSpeeds getChassisSpeeds() { - return chassisSpeeds; + private final SwerveRequest.ApplyRobotSpeeds applyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds() + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withDesaturateWheelSpeeds(false); + + private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + + private SwerveSetpointGenerator setpointGenerator; + private SwerveSetpoint previousSetpoint; + private ChassisSpeeds slewedFieldChassisSpeeds = new ChassisSpeeds(); + + private boolean useDriverOrientation = true; + + private double forwardXRateLimit = Double.POSITIVE_INFINITY; + private double backwardXRateLimit = Double.POSITIVE_INFINITY; + + private double forwardYRateLimit = Double.POSITIVE_INFINITY; + private double backwardYRateLimit = Double.POSITIVE_INFINITY; + + private double timestep = 0.004; + + private boolean maintainStraightStopping = true; + + public ChassisSpeeds getChassisSpeeds() { + return chassisSpeeds; + } + + public ChassisSpeeds getSlewedFieldChassisSpeeds() { + return slewedFieldChassisSpeeds; + } + + public SwerveSetpoint getPreviousSetpoint() { + return previousSetpoint; + } + + /** + * @param xTipLimiter + * @param yTipLimiter + * @param setpointGenerator + * @param initialSetpoint + *

This creates a FieldOrientedOrbitSwerveRequest. + *

The anti-tipping logic currently does not actually work due to rotation and is taken out + * right now. + */ + public FieldOrientedOrbitSwerveRequest( + SwerveSetpointGenerator setpointGenerator, + SwerveSetpoint initialSetpoint, + Rotation2d robotOrientation) { + + this.setpointGenerator = setpointGenerator; + + this.withPreviousSetpoint(initialSetpoint, robotOrientation); + } + + @Override + public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { + double toApplyX = chassisSpeeds.vxMetersPerSecond; + double toApplyY = chassisSpeeds.vyMetersPerSecond; + + if (useDriverOrientation) { + Translation2d tmp = new Translation2d(toApplyX, toApplyY); + tmp = tmp.rotateBy(parameters.operatorForwardDirection); + toApplyX = tmp.getX(); + toApplyY = tmp.getY(); } - public SwerveSetpoint getPreviousSetpoint() { - return previousSetpoint; + double xAcceleration = toApplyX - slewedFieldChassisSpeeds.vxMetersPerSecond; + double yAcceleration = toApplyY - slewedFieldChassisSpeeds.vyMetersPerSecond; + + ChassisSpeeds accelerations = new ChassisSpeeds(xAcceleration, yAcceleration, 0); + accelerations = + ChassisSpeeds.fromFieldRelativeSpeeds(accelerations, parameters.currentPose.getRotation()); + + if (accelerations.vxMetersPerSecond > forwardXRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vyMetersPerSecond + * forwardXRateLimit + * timestep + / accelerations.vxMetersPerSecond; + accelerations.vxMetersPerSecond = forwardXRateLimit * timestep; + } else if (accelerations.vxMetersPerSecond < -backwardXRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vyMetersPerSecond + * -backwardXRateLimit + * timestep + / accelerations.vxMetersPerSecond; + accelerations.vxMetersPerSecond = -backwardXRateLimit * timestep; } - /** - * @param xTipLimiter - * @param yTipLimiter - * @param setpointGenerator - * @param initialSetpoint - * - * This creates a FieldOrientedOrbitSwerveRequest. - * - * The anti-tipping logic currently does not actually work due to rotation and is taken out right now. - */ - public FieldOrientedOrbitSwerveRequest(SwerveSetpointGenerator setpointGenerator, SwerveSetpoint initialSetpoint, Rotation2d robotOrientation) { - - this.setpointGenerator = setpointGenerator; - - this.withPreviousSetpoint(initialSetpoint, robotOrientation); + if (accelerations.vyMetersPerSecond > forwardYRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vxMetersPerSecond + * forwardYRateLimit + * timestep + / accelerations.vyMetersPerSecond; + accelerations.vyMetersPerSecond = forwardYRateLimit * timestep; + } else if (accelerations.vyMetersPerSecond < -backwardYRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vxMetersPerSecond = + accelerations.vxMetersPerSecond + * -backwardYRateLimit + * timestep + / accelerations.vyMetersPerSecond; + accelerations.vyMetersPerSecond = -backwardYRateLimit * timestep; } - @Override - public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { - double toApplyX = chassisSpeeds.vxMetersPerSecond; - double toApplyY = chassisSpeeds.vyMetersPerSecond; - - if (useDriverOrientation) { - Translation2d tmp = new Translation2d(toApplyX, toApplyY); - tmp = tmp.rotateBy(parameters.operatorForwardDirection); - toApplyX = tmp.getX(); - toApplyY = tmp.getY(); - } - - double xAcceleration = toApplyX - slewedFieldChassisSpeeds.vxMetersPerSecond; - double yAcceleration = toApplyY - slewedFieldChassisSpeeds.vyMetersPerSecond; - - ChassisSpeeds accelerations = new ChassisSpeeds(xAcceleration, yAcceleration, 0); - accelerations = ChassisSpeeds.fromFieldRelativeSpeeds(accelerations,parameters.currentPose.getRotation()); - - if (accelerations.vxMetersPerSecond > forwardXRateLimit*timestep) { - if (maintainStraightStopping) accelerations.vyMetersPerSecond = accelerations.vyMetersPerSecond * forwardXRateLimit*timestep / accelerations.vxMetersPerSecond; - accelerations.vxMetersPerSecond = forwardXRateLimit*timestep; - } else if (accelerations.vxMetersPerSecond < -backwardXRateLimit*timestep) { - if (maintainStraightStopping) accelerations.vyMetersPerSecond = accelerations.vyMetersPerSecond * -backwardXRateLimit*timestep / accelerations.vxMetersPerSecond; - accelerations.vxMetersPerSecond = -backwardXRateLimit*timestep; - } - - if (accelerations.vyMetersPerSecond > forwardYRateLimit*timestep) { - if (maintainStraightStopping) accelerations.vyMetersPerSecond = accelerations.vxMetersPerSecond * forwardYRateLimit*timestep / accelerations.vyMetersPerSecond; - accelerations.vyMetersPerSecond = forwardYRateLimit*timestep; - } else if (accelerations.vyMetersPerSecond < -backwardYRateLimit*timestep) { - if (maintainStraightStopping) accelerations.vxMetersPerSecond = accelerations.vxMetersPerSecond * -backwardYRateLimit*timestep / accelerations.vyMetersPerSecond; - accelerations.vyMetersPerSecond = -backwardYRateLimit*timestep; - } - - accelerations = ChassisSpeeds.fromFieldRelativeSpeeds(accelerations,parameters.currentPose.getRotation()); - - slewedFieldChassisSpeeds = slewedFieldChassisSpeeds.plus(accelerations); - - ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds(slewedFieldChassisSpeeds.vxMetersPerSecond, slewedFieldChassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond); - - robotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(robotRelativeSpeeds,parameters.currentPose.getRotation()); - - // Keep the robot from tipping over - // robotRelativeSpeeds.vxMetersPerSecond = xLimiter.calculate(robotRelativeSpeeds.vxMetersPerSecond); - // robotRelativeSpeeds.vyMetersPerSecond = yLimiter.calculate(robotRelativeSpeeds.vyMetersPerSecond); - - // Apply all other limits - previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, robotRelativeSpeeds, timestep); - - DriveFeedforwards feedforwards = previousSetpoint.feedforwards(); - - return applyRobotSpeeds - .withSpeeds(previousSetpoint.robotRelativeSpeeds()) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - .apply(parameters, modulesToApply); - } - - public FieldOrientedOrbitSwerveRequest withChassisSpeeds(ChassisSpeeds chassisSpeeds) { - this.chassisSpeeds = chassisSpeeds; - return this; - } - - public FieldOrientedOrbitSwerveRequest withPreviousSetpoint(SwerveSetpoint previousSetpoint, Rotation2d robotOrientation) { - this.previousSetpoint = previousSetpoint; - this.slewedFieldChassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, robotOrientation); - return this; - } - - public FieldOrientedOrbitSwerveRequest withDriverOrientation(boolean useDriverOrientation) { - this.useDriverOrientation = useDriverOrientation; - return this; - } - - public FieldOrientedOrbitSwerveRequest withXRateLimits(double forwardXRateLimit, double backwardXRateLimit) { - this.forwardXRateLimit = forwardXRateLimit; - this.backwardXRateLimit = backwardXRateLimit; - return this; - } - - public FieldOrientedOrbitSwerveRequest withYRateLimits(double forwardYRateLimit, double backwardYRateLimit) { - this.forwardYRateLimit = forwardYRateLimit; - this.backwardYRateLimit = backwardYRateLimit; - return this; - } - - public FieldOrientedOrbitSwerveRequest withTimestep(double timestep) { - this.timestep = timestep; - return this; - } - - public FieldOrientedOrbitSwerveRequest withMaintainStraightStopping(boolean maintainStraightStopping) { - this.maintainStraightStopping = maintainStraightStopping; - return this; - } -} \ No newline at end of file + accelerations = + ChassisSpeeds.fromFieldRelativeSpeeds(accelerations, parameters.currentPose.getRotation()); + + slewedFieldChassisSpeeds = slewedFieldChassisSpeeds.plus(accelerations); + + ChassisSpeeds robotRelativeSpeeds = + new ChassisSpeeds( + slewedFieldChassisSpeeds.vxMetersPerSecond, + slewedFieldChassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); + + robotRelativeSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + robotRelativeSpeeds, parameters.currentPose.getRotation()); + + // Keep the robot from tipping over + // robotRelativeSpeeds.vxMetersPerSecond = + // xLimiter.calculate(robotRelativeSpeeds.vxMetersPerSecond); + // robotRelativeSpeeds.vyMetersPerSecond = + // yLimiter.calculate(robotRelativeSpeeds.vyMetersPerSecond); + + // Apply all other limits + previousSetpoint = + setpointGenerator.generateSetpoint(previousSetpoint, robotRelativeSpeeds, timestep); + + DriveFeedforwards feedforwards = previousSetpoint.feedforwards(); + + return applyRobotSpeeds + .withSpeeds(previousSetpoint.robotRelativeSpeeds()) + // .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + // .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) + .apply(parameters, modulesToApply); + } + + public FieldOrientedOrbitSwerveRequest withChassisSpeeds(ChassisSpeeds chassisSpeeds) { + this.chassisSpeeds = chassisSpeeds; + return this; + } + + public FieldOrientedOrbitSwerveRequest withPreviousSetpoint( + SwerveSetpoint previousSetpoint, Rotation2d robotOrientation) { + this.previousSetpoint = previousSetpoint; + this.slewedFieldChassisSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, robotOrientation); + return this; + } + + public FieldOrientedOrbitSwerveRequest withDriverOrientation(boolean useDriverOrientation) { + this.useDriverOrientation = useDriverOrientation; + return this; + } + + public FieldOrientedOrbitSwerveRequest withXRateLimits( + double forwardXRateLimit, double backwardXRateLimit) { + this.forwardXRateLimit = forwardXRateLimit; + this.backwardXRateLimit = backwardXRateLimit; + return this; + } + + public FieldOrientedOrbitSwerveRequest withYRateLimits( + double forwardYRateLimit, double backwardYRateLimit) { + this.forwardYRateLimit = forwardYRateLimit; + this.backwardYRateLimit = backwardYRateLimit; + return this; + } + + public FieldOrientedOrbitSwerveRequest withTimestep(double timestep) { + this.timestep = timestep; + return this; + } + + public FieldOrientedOrbitSwerveRequest withMaintainStraightStopping( + boolean maintainStraightStopping) { + this.maintainStraightStopping = maintainStraightStopping; + return this; + } +} From 13240ea715d77fcb020c95e42fd2269978f8afab Mon Sep 17 00:00:00 2001 From: mmilunicmobile Date: Sat, 21 Dec 2024 11:32:37 -0500 Subject: [PATCH 2/5] Fixed a field orientation calculation being incorrect. --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 246 +++---- .../subsystems/CommandSwerveDrivetrain.java | 626 +++++++++--------- .../FieldOrientedOrbitSwerveRequest.java | 358 +++++----- 4 files changed, 631 insertions(+), 601 deletions(-) diff --git a/build.gradle b/build.gradle index 6148d2fc..d16f84fb 100644 --- a/build.gradle +++ b/build.gradle @@ -136,7 +136,7 @@ spotless { exclude '**/build/**', '**/build-*/**' } toggleOffOn() - googleJavaFormat() + googleJavaFormat().aosp() trimTrailingWhitespace() endWithNewline() } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5e5def49..59504d90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,123 +21,139 @@ import frc.robot.subsystems.WheelRadiusCharacterization; public class RobotContainer { - private double MaxSpeed = - GlobalConstants.MAX_TRANSLATIONAL_SPEED.in( - MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = - GlobalConstants.MAX_ROTATIONAL_SPEED.in( - RadiansPerSecond); // kMaxAngularRate desired top rotational speed - - private final ThrustmasterJoystick leftDriveController = - new ThrustmasterJoystick(ControllerConstants.LEFT_DRIVE_CONTROLLER); - private final ThrustmasterJoystick rightDriveController = - new ThrustmasterJoystick(ControllerConstants.RIGHT_DRIVE_CONTROLLER); - private final LogitechController operatorController = - new LogitechController(ControllerConstants.OPERATOR_CONTROLLER); - - private final Telemetry logger = new Telemetry(MaxSpeed); - - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - - public Auto auto = new Auto(drivetrain); - - // Use open-loop control for drive motors - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - - public RobotContainer() { - configureBindings(); - - drivetrain.setUpPathPlanner(); - // Establish the "Trajectory Field" Field2d into the dashboard - } - - private void configureBindings() { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - drivetrain.applyRequest( - () -> { - ChassisSpeeds driverDesiredSpeeds = - new ChassisSpeeds( - GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond) - * sps(deadband(leftDriveController.getYAxis().get(), 0.1)), - sps(deadband(leftDriveController.getXAxis().get(), 0.1)) - * GlobalConstants.MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond), - -sps(deadband(rightDriveController.getXAxis().get(), 0.1)) - * GlobalConstants.MAX_ROTATIONAL_SPEED.in(RadiansPerSecond)); - return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds(driverDesiredSpeeds); - // return drivetrain.m_applyFieldSpeeds.withSpeeds(driverDesiredSpeeds); - })); - - // drive.withVelocityX(-leftDriveController.getYAxis().get() * - // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward) - // .withVelocityY(-leftDriveController.getXAxis().get() * - // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left) - // .withRotationalRate(-rightDriveController.getXAxis().get() * - // GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left) - - operatorController.getA().whileTrue(drivetrain.applyRequest(() -> brake)); - operatorController - .getB() - .whileTrue( - drivetrain.applyRequest( - () -> - 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)); - operatorController - .getBack() - .and(operatorController.getX()) - .whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); - operatorController - .getStart() - .and(operatorController.getY()) - .whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); - operatorController - .getStart() - .and(operatorController.getX()) - .whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); - - // reset the field-centric heading on left bumper press - operatorController - .getLeftBumper() - .onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - operatorController - .getRightBumper() - .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d()))); - - drivetrain.registerTelemetry(logger::telemeterize); - } - - private double deadband(double value, double deadband) { - if (value <= deadband && -deadband <= value) { - return 0; + private double MaxSpeed = + GlobalConstants.MAX_TRANSLATIONAL_SPEED.in( + MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = + GlobalConstants.MAX_ROTATIONAL_SPEED.in( + RadiansPerSecond); // kMaxAngularRate desired top rotational speed + + private final ThrustmasterJoystick leftDriveController = + new ThrustmasterJoystick(ControllerConstants.LEFT_DRIVE_CONTROLLER); + private final ThrustmasterJoystick rightDriveController = + new ThrustmasterJoystick(ControllerConstants.RIGHT_DRIVE_CONTROLLER); + private final LogitechController operatorController = + new LogitechController(ControllerConstants.OPERATOR_CONTROLLER); + + private final Telemetry logger = new Telemetry(MaxSpeed); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + + public Auto auto = new Auto(drivetrain); + + // Use open-loop control for drive motors + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + public RobotContainer() { + configureBindings(); + + drivetrain.setUpPathPlanner(); + // Establish the "Trajectory Field" Field2d into the dashboard } - return value; - } + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest( + () -> { + ChassisSpeeds driverDesiredSpeeds = + new ChassisSpeeds( + GlobalConstants.MAX_TRANSLATIONAL_SPEED.in( + MetersPerSecond) + * sps( + deadband( + leftDriveController + .getYAxis() + .get(), + 0.1)), + sps(deadband(leftDriveController.getXAxis().get(), 0.1)) + * GlobalConstants.MAX_TRANSLATIONAL_SPEED.in( + MetersPerSecond), + -sps( + deadband( + rightDriveController + .getXAxis() + .get(), + 0.1)) + * GlobalConstants.MAX_ROTATIONAL_SPEED.in( + RadiansPerSecond)); + return drivetrain.m_applyFieldSpeedsOrbit.withChassisSpeeds( + driverDesiredSpeeds); + // return drivetrain.m_applyFieldSpeeds.withSpeeds(driverDesiredSpeeds); + })); + + // drive.withVelocityX(-leftDriveController.getYAxis().get() * + // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive forward with negative Y (forward) + // .withVelocityY(-leftDriveController.getXAxis().get() * + // GlobalConstants.MAX_TRANSLATIONAL_SPEED) // Drive left with negative X (left) + // .withRotationalRate(-rightDriveController.getXAxis().get() * + // GlobalConstants.MAX_ROTATIONAL_SPEED) // Drive counterclockwise with negative X (left) + + operatorController.getA().whileTrue(drivetrain.applyRequest(() -> brake)); + operatorController + .getB() + .whileTrue( + drivetrain.applyRequest( + () -> + 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)); + operatorController + .getBack() + .and(operatorController.getX()) + .whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + operatorController + .getStart() + .and(operatorController.getY()) + .whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + operatorController + .getStart() + .and(operatorController.getX()) + .whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + // reset the field-centric heading on left bumper press + operatorController + .getLeftBumper() + .onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + operatorController + .getRightBumper() + .onTrue(drivetrain.runOnce(() -> drivetrain.resetPose(new Pose2d()))); + + drivetrain.registerTelemetry(logger::telemeterize); + } + + private double deadband(double value, double deadband) { + if (value <= deadband && -deadband <= value) { + return 0; + } + + return value; + } - private double sps(double value) { - return value * Math.abs(value); - } + private double sps(double value) { + return value * Math.abs(value); + } - public Command getAutonomousCommand() { - return auto.getAuto(); - } + public Command getAutonomousCommand() { + return auto.getAuto(); + } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 22786cdf..e3a254ca 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -38,318 +38,334 @@ * be used in command-based projects. */ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = - new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); - - private final SwerveRequest.ApplyRobotSpeeds m_applyRobotSpeeds = - new SwerveRequest.ApplyRobotSpeeds() - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo) - .withDesaturateWheelSpeeds(false); - - public final SwerveRequest.ApplyFieldSpeeds m_applyFieldSpeeds = - new SwerveRequest.ApplyFieldSpeeds(); - - public final FieldOrientedOrbitSwerveRequest m_applyFieldSpeedsOrbit; - RobotConfig config; // PathPlanner robot configuration - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); - - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this)); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /* The Setpoint Generator */ - private SwerveSetpointGenerator setpointGenerator; - private SwerveSetpoint previousSetpoint; - - public void setUpPathPlanner() {} - - public Pose2d getRobotPose() { - return this.getState().Pose; - } - - public ChassisSpeeds getChassisSpeeds() { - return this.getState().Speeds; - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = + new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = + new SwerveRequest.SysIdSwerveRotation(); + + private final SwerveRequest.ApplyRobotSpeeds m_applyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds() + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withDesaturateWheelSpeeds(false); + + public final SwerveRequest.ApplyFieldSpeeds m_applyFieldSpeeds = + new SwerveRequest.ApplyFieldSpeeds(); + + public final FieldOrientedOrbitSwerveRequest m_applyFieldSpeedsOrbit; + RobotConfig config; // PathPlanner robot configuration + + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private final SysIdRoutine m_sysIdRoutineTranslation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> + SignalLogger.writeString( + "SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this)); + + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + private final SysIdRoutine m_sysIdRoutineSteer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> + SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this)); + + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private final SysIdRoutine m_sysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> + SignalLogger.writeString( + "SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl( + m_rotationCharacterization.withRotationalRate( + output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + }, + null, + this)); + + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + + /* The Setpoint Generator */ + private SwerveSetpointGenerator setpointGenerator; + private SwerveSetpoint previousSetpoint; + + public void setUpPathPlanner() {} + + public Pose2d getRobotPose() { + return this.getState().Pose; } - m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to - * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double OdometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(drivetrainConstants, OdometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); + + public ChassisSpeeds getChassisSpeeds() { + return this.getState().Speeds; } - m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to - * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * @param visionStandardDeviation The standard deviation for vision calculation - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); - if (Utils.isSimulation()) { - startSimThread(); + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); } - m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); - } - - private FieldOrientedOrbitSwerveRequest generateSwerveSetpointConfig() { - RobotConfig config = GlobalConstants.getRobotConfigPathplanner(); - - setpointGenerator = - new SwerveSetpointGenerator( - config, Units.rotationsToRadians(10.0) // max rotational speed - ); - - ChassisSpeeds currentSpeeds = getState().Speeds; - SwerveModuleState[] currentStates = getState().ModuleStates; - SwerveSetpoint previousSetpoint = - new SwerveSetpoint( - currentSpeeds, currentStates, DriveFeedforwards.zeros(config.numModules)); - - var request = - new FieldOrientedOrbitSwerveRequest( - setpointGenerator, previousSetpoint, getState().Pose.getRotation()); - request.withDriverOrientation(true); - return request; - } - - // - // The desired robot-relative speeds - // returns the module states where robot can drive while obeying physics and not slipping - public SwerveRequest driveRobotRelative(ChassisSpeeds speeds) { - // Note: it is important to not discretize speeds before or after - // using the setpoint generator, as it will discretize them for you - 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()); - // Method that will drive the robot given target module states - } - - public SwerveRequest driveRobotRelative(double xVelocity, double yVelocity, double rotationRate) { - // 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); - return m_applyFieldSpeedsOrbit.withChassisSpeeds(speeds); - // Method that will drive the robot given target module states - } - - public SwerveRequest driveFieldRelative(ChassisSpeeds speeds) { - ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getState().Pose.getRotation()); - return driveRobotRelative(speeds); - } - - public SwerveRequest driveWithFeedforwards(ChassisSpeeds speeds, DriveFeedforwards feedforwards) { - return m_applyRobotSpeeds - .withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()); - } - - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine specified by {@link - * #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine specified by {@link - * #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - @Override - public void periodic() { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double OdometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(drivetrainConstants, OdometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + + m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + } - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * @param visionStandardDeviation The standard deviation for vision calculation + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + if (Utils.isSimulation()) { + startSimThread(); + } + + m_applyFieldSpeedsOrbit = generateSwerveSetpointConfig(); + } + + private FieldOrientedOrbitSwerveRequest generateSwerveSetpointConfig() { + RobotConfig config = GlobalConstants.getRobotConfigPathplanner(); + + setpointGenerator = + new SwerveSetpointGenerator( + config, Units.rotationsToRadians(10.0) // max rotational speed + ); + + ChassisSpeeds currentSpeeds = getState().Speeds; + SwerveModuleState[] currentStates = getState().ModuleStates; + SwerveSetpoint previousSetpoint = + new SwerveSetpoint( + currentSpeeds, currentStates, DriveFeedforwards.zeros(config.numModules)); + + var request = + new FieldOrientedOrbitSwerveRequest( + setpointGenerator, previousSetpoint, getState().Pose.getRotation()); + request.withDriverOrientation(true); + return request; + } + + // + // The desired robot-relative speeds + // returns the module states where robot can drive while obeying physics and not slipping + public SwerveRequest driveRobotRelative(ChassisSpeeds speeds) { + // Note: it is important to not discretize speeds before or after + // using the setpoint generator, as it will discretize them for you + 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()); + // Method that will drive the robot given target module states + } + + public SwerveRequest driveRobotRelative( + double xVelocity, double yVelocity, double rotationRate) { + // 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); + return m_applyFieldSpeedsOrbit.withChassisSpeeds(speeds); + // Method that will drive the robot given target module states + } + + public SwerveRequest driveFieldRelative(ChassisSpeeds speeds) { + ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getState().Pose.getRotation()); + return driveRobotRelative(speeds); + } + + public SwerveRequest driveWithFeedforwards( + ChassisSpeeds speeds, DriveFeedforwards feedforwards) { + return m_applyRobotSpeeds + .withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()); + } + + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance() - .ifPresent( - allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation); - m_hasAppliedOperatorPerspective = true; - }); + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); } - Logger.recordOutput("Drive/desiredChassisSpeeds", m_applyFieldSpeedsOrbit.getChassisSpeeds()); - Logger.recordOutput( - "Drive/slewedChassisSpeeds", m_applyFieldSpeedsOrbit.getSlewedFieldChassisSpeeds()); - Logger.recordOutput( - "Drive/setpointChassisSpeeds", - m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds()); - - Logger.recordOutput("Drive/ModuleTargets", getState().ModuleTargets); - Logger.recordOutput("Drive/ModulePositions", getState().ModulePositions); - Logger.recordOutput("Drive/ModuleStates", getState().ModuleStates); - - Logger.recordOutput("Drive/actualChassisSpeeds", getState().Speeds); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = - new Notifier( - () -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } + /** + * Runs the SysId Quasistatic test in the given direction for the routine specified by {@link + * #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + /** + * Runs the SysId Dynamic test in the given direction for the routine specified by {@link + * #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + @Override + public void periodic() { + + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance() + .ifPresent( + allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation); + m_hasAppliedOperatorPerspective = true; + }); + } + + Logger.recordOutput( + "Drive/desiredChassisSpeeds", m_applyFieldSpeedsOrbit.getChassisSpeeds()); + Logger.recordOutput( + "Drive/slewedChassisSpeeds", m_applyFieldSpeedsOrbit.getSlewedFieldChassisSpeeds()); + Logger.recordOutput( + "Drive/setpointChassisSpeeds", + m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds()); + + Logger.recordOutput("Drive/ModuleTargets", getState().ModuleTargets); + Logger.recordOutput("Drive/ModulePositions", getState().ModulePositions); + Logger.recordOutput("Drive/ModuleStates", getState().ModuleStates); + + Logger.recordOutput("Drive/actualChassisSpeeds", getState().Speeds); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } } diff --git a/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java b/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java index 929c134c..f0360830 100644 --- a/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java +++ b/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java @@ -14,190 +14,188 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; public class FieldOrientedOrbitSwerveRequest implements SwerveRequest { - private final SwerveRequest.ApplyRobotSpeeds applyRobotSpeeds = - new SwerveRequest.ApplyRobotSpeeds() - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo) - .withDesaturateWheelSpeeds(false); - - private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - - private SwerveSetpointGenerator setpointGenerator; - private SwerveSetpoint previousSetpoint; - private ChassisSpeeds slewedFieldChassisSpeeds = new ChassisSpeeds(); - - private boolean useDriverOrientation = true; - - private double forwardXRateLimit = Double.POSITIVE_INFINITY; - private double backwardXRateLimit = Double.POSITIVE_INFINITY; - - private double forwardYRateLimit = Double.POSITIVE_INFINITY; - private double backwardYRateLimit = Double.POSITIVE_INFINITY; - - private double timestep = 0.004; - - private boolean maintainStraightStopping = true; - - public ChassisSpeeds getChassisSpeeds() { - return chassisSpeeds; - } - - public ChassisSpeeds getSlewedFieldChassisSpeeds() { - return slewedFieldChassisSpeeds; - } - - public SwerveSetpoint getPreviousSetpoint() { - return previousSetpoint; - } - - /** - * @param xTipLimiter - * @param yTipLimiter - * @param setpointGenerator - * @param initialSetpoint - *

This creates a FieldOrientedOrbitSwerveRequest. - *

The anti-tipping logic currently does not actually work due to rotation and is taken out - * right now. - */ - public FieldOrientedOrbitSwerveRequest( - SwerveSetpointGenerator setpointGenerator, - SwerveSetpoint initialSetpoint, - Rotation2d robotOrientation) { - - this.setpointGenerator = setpointGenerator; - - this.withPreviousSetpoint(initialSetpoint, robotOrientation); - } - - @Override - public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { - double toApplyX = chassisSpeeds.vxMetersPerSecond; - double toApplyY = chassisSpeeds.vyMetersPerSecond; - - if (useDriverOrientation) { - Translation2d tmp = new Translation2d(toApplyX, toApplyY); - tmp = tmp.rotateBy(parameters.operatorForwardDirection); - toApplyX = tmp.getX(); - toApplyY = tmp.getY(); + private final SwerveRequest.ApplyRobotSpeeds applyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds() + .withDriveRequestType(DriveRequestType.Velocity) + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withDesaturateWheelSpeeds(false); + + private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + + private SwerveSetpointGenerator setpointGenerator; + private SwerveSetpoint previousSetpoint; + private ChassisSpeeds slewedFieldChassisSpeeds = new ChassisSpeeds(); + + private boolean useDriverOrientation = true; + + private double forwardXRateLimit = Double.POSITIVE_INFINITY; + private double backwardXRateLimit = Double.POSITIVE_INFINITY; + + private double forwardYRateLimit = Double.POSITIVE_INFINITY; + private double backwardYRateLimit = Double.POSITIVE_INFINITY; + + private double timestep = 0.004; + + private boolean maintainStraightStopping = true; + + public ChassisSpeeds getChassisSpeeds() { + return chassisSpeeds; + } + + public ChassisSpeeds getSlewedFieldChassisSpeeds() { + return slewedFieldChassisSpeeds; } - double xAcceleration = toApplyX - slewedFieldChassisSpeeds.vxMetersPerSecond; - double yAcceleration = toApplyY - slewedFieldChassisSpeeds.vyMetersPerSecond; - - ChassisSpeeds accelerations = new ChassisSpeeds(xAcceleration, yAcceleration, 0); - accelerations = - ChassisSpeeds.fromFieldRelativeSpeeds(accelerations, parameters.currentPose.getRotation()); - - if (accelerations.vxMetersPerSecond > forwardXRateLimit * timestep) { - if (maintainStraightStopping) - accelerations.vyMetersPerSecond = - accelerations.vyMetersPerSecond - * forwardXRateLimit - * timestep - / accelerations.vxMetersPerSecond; - accelerations.vxMetersPerSecond = forwardXRateLimit * timestep; - } else if (accelerations.vxMetersPerSecond < -backwardXRateLimit * timestep) { - if (maintainStraightStopping) - accelerations.vyMetersPerSecond = - accelerations.vyMetersPerSecond - * -backwardXRateLimit - * timestep - / accelerations.vxMetersPerSecond; - accelerations.vxMetersPerSecond = -backwardXRateLimit * timestep; + public SwerveSetpoint getPreviousSetpoint() { + return previousSetpoint; } - if (accelerations.vyMetersPerSecond > forwardYRateLimit * timestep) { - if (maintainStraightStopping) - accelerations.vyMetersPerSecond = - accelerations.vxMetersPerSecond - * forwardYRateLimit - * timestep - / accelerations.vyMetersPerSecond; - accelerations.vyMetersPerSecond = forwardYRateLimit * timestep; - } else if (accelerations.vyMetersPerSecond < -backwardYRateLimit * timestep) { - if (maintainStraightStopping) - accelerations.vxMetersPerSecond = - accelerations.vxMetersPerSecond - * -backwardYRateLimit - * timestep - / accelerations.vyMetersPerSecond; - accelerations.vyMetersPerSecond = -backwardYRateLimit * timestep; + /** + * @param xTipLimiter + * @param yTipLimiter + * @param setpointGenerator + * @param initialSetpoint + *

This creates a FieldOrientedOrbitSwerveRequest. + *

The anti-tipping logic currently does not actually work due to rotation and is taken + * out right now. + */ + public FieldOrientedOrbitSwerveRequest( + SwerveSetpointGenerator setpointGenerator, + SwerveSetpoint initialSetpoint, + Rotation2d robotOrientation) { + + this.setpointGenerator = setpointGenerator; + + this.withPreviousSetpoint(initialSetpoint, robotOrientation); } - accelerations = - ChassisSpeeds.fromFieldRelativeSpeeds(accelerations, parameters.currentPose.getRotation()); - - slewedFieldChassisSpeeds = slewedFieldChassisSpeeds.plus(accelerations); - - ChassisSpeeds robotRelativeSpeeds = - new ChassisSpeeds( - slewedFieldChassisSpeeds.vxMetersPerSecond, - slewedFieldChassisSpeeds.vyMetersPerSecond, - chassisSpeeds.omegaRadiansPerSecond); - - robotRelativeSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - robotRelativeSpeeds, parameters.currentPose.getRotation()); - - // Keep the robot from tipping over - // robotRelativeSpeeds.vxMetersPerSecond = - // xLimiter.calculate(robotRelativeSpeeds.vxMetersPerSecond); - // robotRelativeSpeeds.vyMetersPerSecond = - // yLimiter.calculate(robotRelativeSpeeds.vyMetersPerSecond); - - // Apply all other limits - previousSetpoint = - setpointGenerator.generateSetpoint(previousSetpoint, robotRelativeSpeeds, timestep); - - DriveFeedforwards feedforwards = previousSetpoint.feedforwards(); - - return applyRobotSpeeds - .withSpeeds(previousSetpoint.robotRelativeSpeeds()) - // .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - // .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - .apply(parameters, modulesToApply); - } - - public FieldOrientedOrbitSwerveRequest withChassisSpeeds(ChassisSpeeds chassisSpeeds) { - this.chassisSpeeds = chassisSpeeds; - return this; - } - - public FieldOrientedOrbitSwerveRequest withPreviousSetpoint( - SwerveSetpoint previousSetpoint, Rotation2d robotOrientation) { - this.previousSetpoint = previousSetpoint; - this.slewedFieldChassisSpeeds = - ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, robotOrientation); - return this; - } - - public FieldOrientedOrbitSwerveRequest withDriverOrientation(boolean useDriverOrientation) { - this.useDriverOrientation = useDriverOrientation; - return this; - } - - public FieldOrientedOrbitSwerveRequest withXRateLimits( - double forwardXRateLimit, double backwardXRateLimit) { - this.forwardXRateLimit = forwardXRateLimit; - this.backwardXRateLimit = backwardXRateLimit; - return this; - } - - public FieldOrientedOrbitSwerveRequest withYRateLimits( - double forwardYRateLimit, double backwardYRateLimit) { - this.forwardYRateLimit = forwardYRateLimit; - this.backwardYRateLimit = backwardYRateLimit; - return this; - } - - public FieldOrientedOrbitSwerveRequest withTimestep(double timestep) { - this.timestep = timestep; - return this; - } - - public FieldOrientedOrbitSwerveRequest withMaintainStraightStopping( - boolean maintainStraightStopping) { - this.maintainStraightStopping = maintainStraightStopping; - return this; - } + @Override + public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { + double toApplyX = chassisSpeeds.vxMetersPerSecond; + double toApplyY = chassisSpeeds.vyMetersPerSecond; + + if (useDriverOrientation) { + Translation2d tmp = new Translation2d(toApplyX, toApplyY); + tmp = tmp.rotateBy(parameters.operatorForwardDirection); + toApplyX = tmp.getX(); + toApplyY = tmp.getY(); + } + + double xAcceleration = toApplyX - slewedFieldChassisSpeeds.vxMetersPerSecond; + double yAcceleration = toApplyY - slewedFieldChassisSpeeds.vyMetersPerSecond; + + ChassisSpeeds accelerations = new ChassisSpeeds(xAcceleration, yAcceleration, 0); + accelerations = + ChassisSpeeds.fromFieldRelativeSpeeds( + accelerations, parameters.currentPose.getRotation()); + + if (accelerations.vxMetersPerSecond > forwardXRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vyMetersPerSecond + * forwardXRateLimit + * timestep + / accelerations.vxMetersPerSecond; + accelerations.vxMetersPerSecond = forwardXRateLimit * timestep; + } else if (accelerations.vxMetersPerSecond < -backwardXRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vyMetersPerSecond + * -backwardXRateLimit + * timestep + / accelerations.vxMetersPerSecond; + accelerations.vxMetersPerSecond = -backwardXRateLimit * timestep; + } + + if (accelerations.vyMetersPerSecond > forwardYRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vyMetersPerSecond = + accelerations.vxMetersPerSecond + * forwardYRateLimit + * timestep + / accelerations.vyMetersPerSecond; + accelerations.vyMetersPerSecond = forwardYRateLimit * timestep; + } else if (accelerations.vyMetersPerSecond < -backwardYRateLimit * timestep) { + if (maintainStraightStopping) + accelerations.vxMetersPerSecond = + accelerations.vxMetersPerSecond + * -backwardYRateLimit + * timestep + / accelerations.vyMetersPerSecond; + accelerations.vyMetersPerSecond = -backwardYRateLimit * timestep; + } + + accelerations = + ChassisSpeeds.fromRobotRelativeSpeeds( + accelerations, parameters.currentPose.getRotation()); + + slewedFieldChassisSpeeds = slewedFieldChassisSpeeds.plus(accelerations); + + ChassisSpeeds robotRelativeSpeeds = + new ChassisSpeeds( + slewedFieldChassisSpeeds.vxMetersPerSecond, + slewedFieldChassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond); + + robotRelativeSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + robotRelativeSpeeds, parameters.currentPose.getRotation()); + + // Apply all other limits + previousSetpoint = + setpointGenerator.generateSetpoint(previousSetpoint, robotRelativeSpeeds, timestep, 12.0); + + DriveFeedforwards feedforwards = previousSetpoint.feedforwards(); + + ChassisSpeeds driveSpeeds = previousSetpoint.robotRelativeSpeeds(); + + return applyRobotSpeeds + .withSpeeds(driveSpeeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) + .apply(parameters, modulesToApply); + } + + public FieldOrientedOrbitSwerveRequest withChassisSpeeds(ChassisSpeeds chassisSpeeds) { + this.chassisSpeeds = chassisSpeeds; + return this; + } + + public FieldOrientedOrbitSwerveRequest withPreviousSetpoint( + SwerveSetpoint previousSetpoint, Rotation2d robotOrientation) { + this.previousSetpoint = previousSetpoint; + this.slewedFieldChassisSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, robotOrientation); + return this; + } + + public FieldOrientedOrbitSwerveRequest withDriverOrientation(boolean useDriverOrientation) { + this.useDriverOrientation = useDriverOrientation; + return this; + } + + public FieldOrientedOrbitSwerveRequest withXRateLimits( + double forwardXRateLimit, double backwardXRateLimit) { + this.forwardXRateLimit = forwardXRateLimit; + this.backwardXRateLimit = backwardXRateLimit; + return this; + } + + public FieldOrientedOrbitSwerveRequest withYRateLimits( + double forwardYRateLimit, double backwardYRateLimit) { + this.forwardYRateLimit = forwardYRateLimit; + this.backwardYRateLimit = backwardYRateLimit; + return this; + } + + public FieldOrientedOrbitSwerveRequest withTimestep(double timestep) { + this.timestep = timestep; + return this; + } + + public FieldOrientedOrbitSwerveRequest withMaintainStraightStopping( + boolean maintainStraightStopping) { + this.maintainStraightStopping = maintainStraightStopping; + return this; + } } From 9316326bb720c99a075068a4b9a6327afd544edd Mon Sep 17 00:00:00 2001 From: mmilunicmobile Date: Sat, 21 Dec 2024 17:12:55 -0500 Subject: [PATCH 3/5] added a few more important fixes and logging --- src/main/deploy/pathplanner/paths/1M.path | 2 +- .../pathplanner/paths/Half-Field Sprint.path | 2 +- src/main/deploy/pathplanner/settings.json | 24 ++++----- .../frc/robot/constants/GlobalConstants.java | 51 +++++++++++-------- .../frc/robot/constants/TunerConstants.java | 6 ++- .../subsystems/CommandSwerveDrivetrain.java | 14 +++++ 6 files changed, 64 insertions(+), 35 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/1M.path b/src/main/deploy/pathplanner/paths/1M.path index fec249cd..04db582d 100644 --- a/src/main/deploy/pathplanner/paths/1M.path +++ b/src/main/deploy/pathplanner/paths/1M.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": -0.27527965608765825, - "y": 0.0 + "y": 1.561767148845488e-16 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Half-Field Sprint.path b/src/main/deploy/pathplanner/paths/Half-Field Sprint.path index 5d078552..234946c2 100644 --- a/src/main/deploy/pathplanner/paths/Half-Field Sprint.path +++ b/src/main/deploy/pathplanner/paths/Half-Field Sprint.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 6.0, + "maxVelocity": 4.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 17293b32..42c00dd5 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,23 +9,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 68.0389, "robotMOI": 6.883, "robotTrackwidth": 0.508, "driveWheelRadius": 0.0508, - "driveGearing": 6.75, - "maxDriveSpeed": 4.572, + "driveGearing": 5.142857, + "maxDriveSpeed": 6.21, "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 60.0, + "driveCurrentLimit": 85.0, "wheelCOF": 1.0, - "flModuleX": 0.298, - "flModuleY": 0.298, - "frModuleX": 0.298, - "frModuleY": -0.298, - "blModuleX": -0.298, - "blModuleY": 0.298, - "brModuleX": -0.298, - "brModuleY": -0.298, + "flModuleX": 0.2921, + "flModuleY": 0.292, + "frModuleX": 0.2921, + "frModuleY": -0.2921, + "blModuleX": -0.2921, + "blModuleY": 0.2921, + "brModuleX": -0.2921, + "brModuleY": -0.2921, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/constants/GlobalConstants.java b/src/main/java/frc/robot/constants/GlobalConstants.java index ad4f83a6..604dc944 100644 --- a/src/main/java/frc/robot/constants/GlobalConstants.java +++ b/src/main/java/frc/robot/constants/GlobalConstants.java @@ -3,10 +3,8 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; @@ -36,27 +34,40 @@ public static class ControllerConstants { private static RobotConfig robotConfigPathplanner; + // public static RobotConfig getRobotConfigPathplanner() { + // if (robotConfigPathplanner == null) { + // try{ + // Translation2d[] moduleOffsets = new Translation2d[4]; + // SwerveModuleConstants[] constants = new SwerveModuleConstants[]{ + // TunerConstants.FrontLeft, TunerConstants.FrontRight, TunerConstants.BackLeft, TunerConstants.BackRight + // }; + // for (int i = 0; i < constants.length; i++) { + // moduleOffsets[i] = new Translation2d(constants[i].LocationX, constants[i].LocationY); + // } + + // robotConfigPathplanner = new RobotConfig(ROBOT_MASS, ROBOT_MOI, + // new ModuleConfig( + // Meters.of(EXAMPLE_MODULE.WheelRadius), + // MAX_TRANSLATIONAL_SPEED, + // COEFFICIENT_OF_FRICTION, + // DRIVE_MOTOR, + // EXAMPLE_MODULE.DriveMotorInitialConfigs.CurrentLimits.getStatorCurrentLimitMeasure(), + // 1 + // ), + // moduleOffsets); + // } catch (Exception e) { + // // Handle exception as needed + // e.printStackTrace(); + // throw new RuntimeException("Failed to load robot config from pathplanner."); + // } + // } + // return robotConfigPathplanner; + // } + public static RobotConfig getRobotConfigPathplanner() { if (robotConfigPathplanner == null) { try{ - Translation2d[] moduleOffsets = new Translation2d[4]; - SwerveModuleConstants[] constants = new SwerveModuleConstants[]{ - TunerConstants.FrontLeft, TunerConstants.FrontRight, TunerConstants.BackLeft, TunerConstants.BackRight - }; - for (int i = 0; i < constants.length; i++) { - moduleOffsets[i] = new Translation2d(constants[i].LocationX, constants[i].LocationY); - } - - robotConfigPathplanner = new RobotConfig(ROBOT_MASS, ROBOT_MOI, - new ModuleConfig( - Meters.of(EXAMPLE_MODULE.WheelRadius), - MAX_TRANSLATIONAL_SPEED, - COEFFICIENT_OF_FRICTION, - DRIVE_MOTOR, - EXAMPLE_MODULE.DriveMotorInitialConfigs.CurrentLimits.getStatorCurrentLimitMeasure(), - 1 - ), - moduleOffsets); + robotConfigPathplanner = RobotConfig.fromGUISettings(); } catch (Exception e) { // Handle exception as needed e.printStackTrace(); diff --git a/src/main/java/frc/robot/constants/TunerConstants.java b/src/main/java/frc/robot/constants/TunerConstants.java index fe1f2401..356855c6 100644 --- a/src/main/java/frc/robot/constants/TunerConstants.java +++ b/src/main/java/frc/robot/constants/TunerConstants.java @@ -29,11 +29,15 @@ public class TunerConstants { .withKP(40).withKI(0).withKD(0.0) .withKS(0.00).withKV(0).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) + .withKP(0.0).withKI(0).withKD(0) .withKS(0).withKV(0.124); + // private static final Slot0Configs driveGains = new Slot0Configs() + // .withKP(0.1).withKI(0).withKD(0) + // .withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index e3a254ca..cde73001 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -315,6 +315,8 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutineToApply.dynamic(direction); } + private double lastSpeed = 0; + @Override public void periodic() { @@ -345,6 +347,18 @@ public void periodic() { "Drive/setpointChassisSpeeds", m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds()); + ChassisSpeeds speedsPreview = m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds(); + + double currentSpeed = Math.hypot(speedsPreview.vxMetersPerSecond, speedsPreview.vyMetersPerSecond); + double acceleration = (currentSpeed - lastSpeed) / 0.02; + lastSpeed = currentSpeed; + + Logger.recordOutput("Drive/Velocity", currentSpeed); + Logger.recordOutput("Drive/Acceleration", acceleration); + + Logger.recordOutput("Drive/NotZero", m_applyFieldSpeedsOrbit.getChassisSpeeds().vxMetersPerSecond != 0); + + Logger.recordOutput("Drive/ModuleTargets", getState().ModuleTargets); Logger.recordOutput("Drive/ModulePositions", getState().ModulePositions); Logger.recordOutput("Drive/ModuleStates", getState().ModuleStates); From c15bbd7ef056571867f3574f995fca56566178b3 Mon Sep 17 00:00:00 2001 From: mmilunicmobile Date: Sat, 21 Dec 2024 17:13:07 -0500 Subject: [PATCH 4/5] added swerve setpoint generator fix --- .../util/swerve/SwerveSetpointGenerator.java | 600 ++++++++++++++++++ 1 file changed, 600 insertions(+) create mode 100644 src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java diff --git a/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java b/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java new file mode 100644 index 00000000..30912d1d --- /dev/null +++ b/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java @@ -0,0 +1,600 @@ +package com.pathplanner.lib.util.swerve; + +import static edu.wpi.first.units.Units.*; + +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.util.DriveFeedforwards; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +/** + * Swerve setpoint generator based on a version created by FRC team 254. + * + *

Takes a prior setpoint, a desired setpoint, and outputs a new setpoint that respects all the + * kinematic constraints on module rotation and wheel velocity/torque, as well as preventing any + * forces acting on a module's wheel from exceeding the force of friction. + */ +public class SwerveSetpointGenerator { + private static final double kEpsilon = 1E-8; + private static final int MAX_STEER_ITERATIONS = 8; + private static final int MAX_DRIVE_ITERATIONS = 10; + + private final RobotConfig config; + private final double maxSteerVelocityRadsPerSec; + private final double brownoutVoltage; + + /** + * Create a new swerve setpoint generator + * + * @param config The robot configuration + * @param maxSteerVelocityRadsPerSec The maximum rotation velocity of a swerve module, in radians + * per second + */ + public SwerveSetpointGenerator(RobotConfig config, double maxSteerVelocityRadsPerSec) { + this.config = config; + this.maxSteerVelocityRadsPerSec = maxSteerVelocityRadsPerSec; + this.brownoutVoltage = RobotController.getBrownoutVoltage(); + } + + /** + * Create a new swerve setpoint generator + * + * @param config The robot configuration + * @param maxSteerVelocity The maximum rotation velocity of a swerve module + */ + public SwerveSetpointGenerator(RobotConfig config, AngularVelocity maxSteerVelocity) { + this(config, maxSteerVelocity.in(RadiansPerSecond)); + } + + /** + * Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds + * passed into or returned from this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks or + * a path following algorithm. + * @param dt The loop time. + * @param inputVoltage The input voltage of the drive motor controllers, in volts. This can also + * be a static nominal voltage if you do not want the setpoint generator to react to changes + * in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input + * voltage will be clamped to a minimum of the robot controller's brownout voltage. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredStateRobotRelative, + double dt, + double inputVoltage) { + if (Double.isNaN(inputVoltage)) { + inputVoltage = 12.0; + } else { + inputVoltage = Math.max(inputVoltage, brownoutVoltage); + } + + SwerveModuleState[] desiredModuleStates = + config.toSwerveModuleStates(desiredStateRobotRelative); + // Make sure desiredState respects velocity limits. + SwerveDriveKinematics.desaturateWheelSpeeds( + desiredModuleStates, config.moduleConfig.maxDriveVelocityMPS); + desiredStateRobotRelative = config.toChassisSpeeds(desiredModuleStates); + + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so + // just use the previous angle. + boolean need_to_steer = true; + if (epsilonEquals(desiredStateRobotRelative, new ChassisSpeeds())) { + need_to_steer = false; + for (int m = 0; m < config.numModules; m++) { + desiredModuleStates[m].angle = prevSetpoint.moduleStates()[m].angle; + desiredModuleStates[m].speedMetersPerSecond = 0.0; + } + } + + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[config.numModules]; + double[] prev_vy = new double[config.numModules]; + Rotation2d[] prev_heading = new Rotation2d[config.numModules]; + double[] desired_vx = new double[config.numModules]; + double[] desired_vy = new double[config.numModules]; + Rotation2d[] desired_heading = new Rotation2d[config.numModules]; + boolean all_modules_should_flip = true; + for (int m = 0; m < config.numModules; m++) { + prev_vx[m] = + prevSetpoint.moduleStates()[m].angle.getCos() + * prevSetpoint.moduleStates()[m].speedMetersPerSecond; + prev_vy[m] = + prevSetpoint.moduleStates()[m].angle.getSin() + * prevSetpoint.moduleStates()[m].speedMetersPerSecond; + prev_heading[m] = prevSetpoint.moduleStates()[m].angle; + if (prevSetpoint.moduleStates()[m].speedMetersPerSecond < 0.0) { + prev_heading[m] = prev_heading[m].rotateBy(Rotation2d.k180deg); + } + desired_vx[m] = + desiredModuleStates[m].angle.getCos() * desiredModuleStates[m].speedMetersPerSecond; + desired_vy[m] = + desiredModuleStates[m].angle.getSin() * desiredModuleStates[m].speedMetersPerSecond; + desired_heading[m] = desiredModuleStates[m].angle; + if (desiredModuleStates[m].speedMetersPerSecond < 0.0) { + desired_heading[m] = desired_heading[m].rotateBy(Rotation2d.k180deg); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs(prev_heading[m].unaryMinus().rotateBy(desired_heading[m]).getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !epsilonEquals(prevSetpoint.robotRelativeSpeeds(), new ChassisSpeeds()) + && !epsilonEquals(desiredStateRobotRelative, new ChassisSpeeds())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the complement + // of the desired angle, and accelerate again. + return generateSetpoint(prevSetpoint, new ChassisSpeeds(), dt, inputVoltage); + } + + // Compute the deltas between start and goal. We can then interpolate from the start state to + // the goal state; then find the amount we can move from start towards goal in this cycle such + // that no kinematic limit is exceeded. + double dx = + desiredStateRobotRelative.vxMetersPerSecond + - prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; + double dy = + desiredStateRobotRelative.vyMetersPerSecond + - prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; + double dtheta = + desiredStateRobotRelative.omegaRadiansPerSecond + - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond; + + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; + + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since inverse kinematics doesn't care about angle, we can be opportunistically + // lazy). + List> overrideSteering = new ArrayList<>(config.numModules); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, and then backing out the maximum interpolant between start and goal + // states. We remember the minimum across all modules, since that is the active constraint. + for (int m = 0; m < config.numModules; m++) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[m].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + + double max_theta_step = dt * maxSteerVelocityRadsPerSec; + + if (epsilonEquals(prevSetpoint.moduleStates()[m].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based purely on rotation in place. + if (epsilonEquals(desiredModuleStates[m].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(m, Optional.of(prevSetpoint.moduleStates()[m].angle)); + continue; + } + + var necessaryRotation = + prevSetpoint + .moduleStates()[m] + .angle + .unaryMinus() + .rotateBy(desiredModuleStates[m].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d.kPi); + } + + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(m, Optional.of(desiredModuleStates[m].angle)); + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + m, + Optional.of( + prevSetpoint.moduleStates()[m].angle.rotateBy( + Rotation2d.fromRadians( + Math.signum(necessaryRotation.getRadians()) * max_theta_step)))); + min_s = 0.0; + } + continue; + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + // Enforce centripetal force limits to prevent sliding. + // We do this by changing max_theta_step to the maximum change in heading over dt + // that would create a large enough radius to keep the centripetal force under the + // friction force. + double maxHeadingChange = + (dt * config.wheelFrictionForce) + / ((config.massKG / config.numModules) + * Math.abs(prevSetpoint.moduleStates()[m].speedMetersPerSecond)); + max_theta_step = Math.min(max_theta_step, maxHeadingChange); + + double s = + findSteeringMaxS( + prev_vx[m], + prev_vy[m], + prev_heading[m].getRadians(), + desired_vx[m], + desired_vy[m], + desired_heading[m].getRadians(), + max_theta_step); + min_s = Math.min(min_s, s); + } + + // Enforce drive wheel torque limits + Translation2d chassisForceVec = new Translation2d(); + double chassisTorque = 0.0; + for (int m = 0; m < config.numModules; m++) { + double lastVelRadPerSec = + prevSetpoint.moduleStates()[m].speedMetersPerSecond + / config.moduleConfig.wheelRadiusMeters; + // Use the current battery voltage since we won't be able to supply 12v if the + // battery is sagging down to 11v, which will affect the max torque output + double currentDraw = + config.moduleConfig.driveMotor.getCurrent(Math.abs(lastVelRadPerSec), inputVoltage); + double reverseCurrentDraw = + Math.abs(config.moduleConfig.driveMotor.getCurrent(Math.abs(lastVelRadPerSec), -inputVoltage)); + currentDraw = Math.min(currentDraw, config.moduleConfig.driveCurrentLimit); + reverseCurrentDraw = Math.min(reverseCurrentDraw, config.moduleConfig.driveCurrentLimit); + double forwardModuleTorque = config.moduleConfig.driveMotor.getTorque(currentDraw); + double reverseModuleTorque = config.moduleConfig.driveMotor.getTorque(reverseCurrentDraw); + + double prevSpeed = prevSetpoint.moduleStates()[m].speedMetersPerSecond; + desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); + double desiredSpeed = desiredModuleStates[m].speedMetersPerSecond; + + int forceSign; + Rotation2d forceAngle = prevSetpoint.moduleStates()[m].angle; + double moduleTorque; + if (epsilonEquals(prevSpeed, 0.0) + || (prevSpeed > 0 && desiredSpeed >= prevSpeed) + || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { + moduleTorque = forwardModuleTorque; + // Torque loss will be fighting motor + moduleTorque -= config.moduleConfig.torqueLoss; + forceSign = 1; // Force will be applied in direction of module + if (prevSpeed < 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } else { + moduleTorque = reverseModuleTorque; + // Torque loss will be helping the motor + moduleTorque += config.moduleConfig.torqueLoss; + forceSign = -1; // Force will be applied in opposite direction of module + if (prevSpeed > 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } + + // Limit torque to prevent wheel slip + moduleTorque = Math.min(moduleTorque, config.maxTorqueFriction); + + double forceAtCarpet = moduleTorque / config.moduleConfig.wheelRadiusMeters; + Translation2d moduleForceVec = new Translation2d(forceAtCarpet * forceSign, forceAngle); + + // Add the module force vector to the chassis force vector + chassisForceVec = chassisForceVec.plus(moduleForceVec); + + // Calculate the torque this module will apply to the chassis + if (!epsilonEquals(0, moduleForceVec.getNorm())) { + Rotation2d angleToModule = config.moduleLocations[m].getAngle(); + Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule); + chassisTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.getSin(); + } + } + + Translation2d chassisAccelVec = chassisForceVec.div(config.massKG); + double chassisAngularAccel = chassisTorque / config.MOI; + + // Use kinematics to convert chassis accelerations to module accelerations + ChassisSpeeds chassisAccel = + new ChassisSpeeds(chassisAccelVec.getX(), chassisAccelVec.getY(), chassisAngularAccel); + var accelStates = config.toSwerveModuleStates(chassisAccel); + + for (int m = 0; m < config.numModules; m++) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + + double maxVelStep = Math.abs(accelStates[m].speedMetersPerSecond * dt); + + double vx_min_s = + min_s == 1.0 ? desired_vx[m] : (desired_vx[m] - prev_vx[m]) * min_s + prev_vx[m]; + double vy_min_s = + min_s == 1.0 ? desired_vy[m] : (desired_vy[m] - prev_vy[m]) * min_s + prev_vy[m]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster than that. + double s = + findDriveMaxS( + prev_vx[m], + prev_vy[m], + Math.hypot(prev_vx[m], prev_vy[m]), + vx_min_s, + vy_min_s, + Math.hypot(vx_min_s, vy_min_s), + maxVelStep); + min_s = Math.min(min_s, s); + } + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond + min_s * dtheta); + retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); + + double prevVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; + double prevVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; + double chassisAccelX = (retSpeeds.vxMetersPerSecond - prevVelX) / dt; + double chassisAccelY = (retSpeeds.vyMetersPerSecond - prevVelY) / dt; + double chassisForceX = chassisAccelX * config.massKG; + double chassisForceY = chassisAccelY * config.massKG; + + double angularAccel = + (retSpeeds.omegaRadiansPerSecond - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond) + / dt; + double angTorque = angularAccel * config.MOI; + ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque); + + Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces); + + var retStates = config.toSwerveModuleStates(retSpeeds); + double[] accelFF = new double[config.numModules]; + double[] linearForceFF = new double[config.numModules]; + double[] torqueCurrentFF = new double[config.numModules]; + double[] forceXFF = new double[config.numModules]; + double[] forceYFF = new double[config.numModules]; + for (int m = 0; m < config.numModules; m++) { + double wheelForceDist = wheelForces[m].getNorm(); + double appliedForce = + wheelForceDist > 1e-6 + ? wheelForceDist * wheelForces[m].getAngle().minus(retStates[m].angle).getCos() + : 0.0; + double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters; + double torqueCurrent = config.moduleConfig.driveMotor.getCurrent(wheelTorque); + + final var maybeOverride = overrideSteering.get(m); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[m].angle.unaryMinus().rotateBy(override))) { + retStates[m].speedMetersPerSecond *= -1.0; + appliedForce *= -1.0; + torqueCurrent *= -1.0; + } + retStates[m].angle = override; + } + final var deltaRotation = + prevSetpoint.moduleStates()[m].angle.unaryMinus().rotateBy(retStates[m].angle); + if (flipHeading(deltaRotation)) { + retStates[m].angle = retStates[m].angle.rotateBy(Rotation2d.k180deg); + retStates[m].speedMetersPerSecond *= -1.0; + appliedForce *= -1.0; + torqueCurrent *= -1.0; + } + + accelFF[m] = + (retStates[m].speedMetersPerSecond - prevSetpoint.moduleStates()[m].speedMetersPerSecond) + / dt; + linearForceFF[m] = appliedForce; + torqueCurrentFF[m] = torqueCurrent; + forceXFF[m] = wheelForces[m].getX(); + forceYFF[m] = wheelForces[m].getY(); + } + + return new SwerveSetpoint( + retSpeeds, + retStates, + new DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF, forceYFF)); + } + + /** + * Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds + * passed into or returned from this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks or + * a path following algorithm. + * @param dt The loop time. + * @param inputVoltage The input voltage of the drive motor controllers, in volts. This can also + * be a static nominal voltage if you do not want the setpoint generator to react to changes + * in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input + * voltage will be clamped to a minimum of the robot controller's brownout voltage. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredStateRobotRelative, + Time dt, + Voltage inputVoltage) { + return generateSetpoint( + prevSetpoint, desiredStateRobotRelative, dt.in(Seconds), inputVoltage.in(Volts)); + } + + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + *

Note: This method will automatically use the current robot controller input voltage. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks or + * a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + SwerveSetpoint prevSetpoint, ChassisSpeeds desiredStateRobotRelative, double dt) { + return generateSetpoint( + prevSetpoint, desiredStateRobotRelative, dt, RobotController.getInputVoltage()); + } + + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + *

Note: This method will automatically use the current robot controller input voltage. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks or + * a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + SwerveSetpoint prevSetpoint, ChassisSpeeds desiredStateRobotRelative, Time dt) { + return generateSetpoint( + prevSetpoint, + desiredStateRobotRelative, + dt.in(Seconds), + RobotController.getBatteryVoltage()); + } + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + private static boolean flipHeading(Rotation2d prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; + } + + private static double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + @FunctionalInterface + private interface Function2d { + double f(double x, double y); + } + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private static double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + + if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { + return s_guess; + } + + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + private static double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = (x, y) -> unwrapAngle(f_0, Math.atan2(y, x)) - offset; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_STEER_ITERATIONS); + } + + private static double findDriveMaxS( + double x_0, double y_0, double f_0, double x_1, double y_1, double f_1, double max_vel_step) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = (x, y) -> Math.hypot(x, y) - offset; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_DRIVE_ITERATIONS); + } + + private static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + private static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } + + private static boolean epsilonEquals(ChassisSpeeds s1, ChassisSpeeds s2) { + return epsilonEquals(s1.vxMetersPerSecond, s2.vxMetersPerSecond) + && epsilonEquals(s1.vyMetersPerSecond, s2.vyMetersPerSecond) + && epsilonEquals(s1.omegaRadiansPerSecond, s2.omegaRadiansPerSecond); + } +} From c9bed7e6057be38fc041953e043029ae305c8159 Mon Sep 17 00:00:00 2001 From: mmilunicmobile Date: Sat, 21 Dec 2024 17:52:41 -0500 Subject: [PATCH 5/5] applied formatting and modified moments of inertia to be consistent --- src/main/deploy/pathplanner/settings.json | 2 +- .../util/swerve/SwerveSetpointGenerator.java | 1125 +++++++++-------- .../frc/robot/constants/GlobalConstants.java | 85 +- .../frc/robot/constants/TunerConstants.java | 156 ++- .../subsystems/CommandSwerveDrivetrain.java | 10 +- .../FieldOrientedOrbitSwerveRequest.java | 3 +- 6 files changed, 738 insertions(+), 643 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 42c00dd5..35a007c5 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -10,7 +10,7 @@ "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, "robotMass": 68.0389, - "robotMOI": 6.883, + "robotMOI": 11.61, "robotTrackwidth": 0.508, "driveWheelRadius": 0.0508, "driveGearing": 5.142857, diff --git a/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java b/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java index 30912d1d..1234d564 100644 --- a/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java +++ b/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java @@ -25,576 +25,621 @@ * forces acting on a module's wheel from exceeding the force of friction. */ public class SwerveSetpointGenerator { - private static final double kEpsilon = 1E-8; - private static final int MAX_STEER_ITERATIONS = 8; - private static final int MAX_DRIVE_ITERATIONS = 10; - - private final RobotConfig config; - private final double maxSteerVelocityRadsPerSec; - private final double brownoutVoltage; - - /** - * Create a new swerve setpoint generator - * - * @param config The robot configuration - * @param maxSteerVelocityRadsPerSec The maximum rotation velocity of a swerve module, in radians - * per second - */ - public SwerveSetpointGenerator(RobotConfig config, double maxSteerVelocityRadsPerSec) { - this.config = config; - this.maxSteerVelocityRadsPerSec = maxSteerVelocityRadsPerSec; - this.brownoutVoltage = RobotController.getBrownoutVoltage(); - } - - /** - * Create a new swerve setpoint generator - * - * @param config The robot configuration - * @param maxSteerVelocity The maximum rotation velocity of a swerve module - */ - public SwerveSetpointGenerator(RobotConfig config, AngularVelocity maxSteerVelocity) { - this(config, maxSteerVelocity.in(RadiansPerSecond)); - } - - /** - * Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds - * passed into or returned from this method. This method will discretize the speeds for you. - * - * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous - * iteration setpoint instead of the actual measured/estimated kinematic state. - * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks or - * a path following algorithm. - * @param dt The loop time. - * @param inputVoltage The input voltage of the drive motor controllers, in volts. This can also - * be a static nominal voltage if you do not want the setpoint generator to react to changes - * in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input - * voltage will be clamped to a minimum of the robot controller's brownout voltage. - * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to - * desiredState quickly. - */ - public SwerveSetpoint generateSetpoint( - final SwerveSetpoint prevSetpoint, - ChassisSpeeds desiredStateRobotRelative, - double dt, - double inputVoltage) { - if (Double.isNaN(inputVoltage)) { - inputVoltage = 12.0; - } else { - inputVoltage = Math.max(inputVoltage, brownoutVoltage); + private static final double kEpsilon = 1E-8; + private static final int MAX_STEER_ITERATIONS = 8; + private static final int MAX_DRIVE_ITERATIONS = 10; + + private final RobotConfig config; + private final double maxSteerVelocityRadsPerSec; + private final double brownoutVoltage; + + /** + * Create a new swerve setpoint generator + * + * @param config The robot configuration + * @param maxSteerVelocityRadsPerSec The maximum rotation velocity of a swerve module, in + * radians per second + */ + public SwerveSetpointGenerator(RobotConfig config, double maxSteerVelocityRadsPerSec) { + this.config = config; + this.maxSteerVelocityRadsPerSec = maxSteerVelocityRadsPerSec; + this.brownoutVoltage = RobotController.getBrownoutVoltage(); } - SwerveModuleState[] desiredModuleStates = - config.toSwerveModuleStates(desiredStateRobotRelative); - // Make sure desiredState respects velocity limits. - SwerveDriveKinematics.desaturateWheelSpeeds( - desiredModuleStates, config.moduleConfig.maxDriveVelocityMPS); - desiredStateRobotRelative = config.toChassisSpeeds(desiredModuleStates); - - // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so - // just use the previous angle. - boolean need_to_steer = true; - if (epsilonEquals(desiredStateRobotRelative, new ChassisSpeeds())) { - need_to_steer = false; - for (int m = 0; m < config.numModules; m++) { - desiredModuleStates[m].angle = prevSetpoint.moduleStates()[m].angle; - desiredModuleStates[m].speedMetersPerSecond = 0.0; - } + /** + * Create a new swerve setpoint generator + * + * @param config The robot configuration + * @param maxSteerVelocity The maximum rotation velocity of a swerve module + */ + public SwerveSetpointGenerator(RobotConfig config, AngularVelocity maxSteerVelocity) { + this(config, maxSteerVelocity.in(RadiansPerSecond)); } - // For each module, compute local Vx and Vy vectors. - double[] prev_vx = new double[config.numModules]; - double[] prev_vy = new double[config.numModules]; - Rotation2d[] prev_heading = new Rotation2d[config.numModules]; - double[] desired_vx = new double[config.numModules]; - double[] desired_vy = new double[config.numModules]; - Rotation2d[] desired_heading = new Rotation2d[config.numModules]; - boolean all_modules_should_flip = true; - for (int m = 0; m < config.numModules; m++) { - prev_vx[m] = - prevSetpoint.moduleStates()[m].angle.getCos() - * prevSetpoint.moduleStates()[m].speedMetersPerSecond; - prev_vy[m] = - prevSetpoint.moduleStates()[m].angle.getSin() - * prevSetpoint.moduleStates()[m].speedMetersPerSecond; - prev_heading[m] = prevSetpoint.moduleStates()[m].angle; - if (prevSetpoint.moduleStates()[m].speedMetersPerSecond < 0.0) { - prev_heading[m] = prev_heading[m].rotateBy(Rotation2d.k180deg); - } - desired_vx[m] = - desiredModuleStates[m].angle.getCos() * desiredModuleStates[m].speedMetersPerSecond; - desired_vy[m] = - desiredModuleStates[m].angle.getSin() * desiredModuleStates[m].speedMetersPerSecond; - desired_heading[m] = desiredModuleStates[m].angle; - if (desiredModuleStates[m].speedMetersPerSecond < 0.0) { - desired_heading[m] = desired_heading[m].rotateBy(Rotation2d.k180deg); - } - if (all_modules_should_flip) { - double required_rotation_rad = - Math.abs(prev_heading[m].unaryMinus().rotateBy(desired_heading[m]).getRadians()); - if (required_rotation_rad < Math.PI / 2.0) { - all_modules_should_flip = false; + /** + * Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds + * passed into or returned from this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @param inputVoltage The input voltage of the drive motor controllers, in volts. This can also + * be a static nominal voltage if you do not want the setpoint generator to react to changes + * in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input + * voltage will be clamped to a minimum of the robot controller's brownout voltage. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging + * to desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredStateRobotRelative, + double dt, + double inputVoltage) { + if (Double.isNaN(inputVoltage)) { + inputVoltage = 12.0; + } else { + inputVoltage = Math.max(inputVoltage, brownoutVoltage); } - } - } - if (all_modules_should_flip - && !epsilonEquals(prevSetpoint.robotRelativeSpeeds(), new ChassisSpeeds()) - && !epsilonEquals(desiredStateRobotRelative, new ChassisSpeeds())) { - // It will (likely) be faster to stop the robot, rotate the modules in place to the complement - // of the desired angle, and accelerate again. - return generateSetpoint(prevSetpoint, new ChassisSpeeds(), dt, inputVoltage); - } - // Compute the deltas between start and goal. We can then interpolate from the start state to - // the goal state; then find the amount we can move from start towards goal in this cycle such - // that no kinematic limit is exceeded. - double dx = - desiredStateRobotRelative.vxMetersPerSecond - - prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; - double dy = - desiredStateRobotRelative.vyMetersPerSecond - - prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; - double dtheta = - desiredStateRobotRelative.omegaRadiansPerSecond - - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond; - - // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at - // desiredState. - double min_s = 1.0; - - // In cases where an individual module is stopped, we want to remember the right steering angle - // to command (since inverse kinematics doesn't care about angle, we can be opportunistically - // lazy). - List> overrideSteering = new ArrayList<>(config.numModules); - // Enforce steering velocity limits. We do this by taking the derivative of steering angle at - // the current angle, and then backing out the maximum interpolant between start and goal - // states. We remember the minimum across all modules, since that is the active constraint. - for (int m = 0; m < config.numModules; m++) { - if (!need_to_steer) { - overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[m].angle)); - continue; - } - overrideSteering.add(Optional.empty()); - - double max_theta_step = dt * maxSteerVelocityRadsPerSec; - - if (epsilonEquals(prevSetpoint.moduleStates()[m].speedMetersPerSecond, 0.0)) { - // If module is stopped, we know that we will need to move straight to the final steering - // angle, so limit based purely on rotation in place. - if (epsilonEquals(desiredModuleStates[m].speedMetersPerSecond, 0.0)) { - // Goal angle doesn't matter. Just leave module at its current angle. - overrideSteering.set(m, Optional.of(prevSetpoint.moduleStates()[m].angle)); - continue; + SwerveModuleState[] desiredModuleStates = + config.toSwerveModuleStates(desiredStateRobotRelative); + // Make sure desiredState respects velocity limits. + SwerveDriveKinematics.desaturateWheelSpeeds( + desiredModuleStates, config.moduleConfig.maxDriveVelocityMPS); + desiredStateRobotRelative = config.toChassisSpeeds(desiredModuleStates); + + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, + // so + // just use the previous angle. + boolean need_to_steer = true; + if (epsilonEquals(desiredStateRobotRelative, new ChassisSpeeds())) { + need_to_steer = false; + for (int m = 0; m < config.numModules; m++) { + desiredModuleStates[m].angle = prevSetpoint.moduleStates()[m].angle; + desiredModuleStates[m].speedMetersPerSecond = 0.0; + } } - var necessaryRotation = - prevSetpoint - .moduleStates()[m] - .angle - .unaryMinus() - .rotateBy(desiredModuleStates[m].angle); - if (flipHeading(necessaryRotation)) { - necessaryRotation = necessaryRotation.rotateBy(Rotation2d.kPi); + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[config.numModules]; + double[] prev_vy = new double[config.numModules]; + Rotation2d[] prev_heading = new Rotation2d[config.numModules]; + double[] desired_vx = new double[config.numModules]; + double[] desired_vy = new double[config.numModules]; + Rotation2d[] desired_heading = new Rotation2d[config.numModules]; + boolean all_modules_should_flip = true; + for (int m = 0; m < config.numModules; m++) { + prev_vx[m] = + prevSetpoint.moduleStates()[m].angle.getCos() + * prevSetpoint.moduleStates()[m].speedMetersPerSecond; + prev_vy[m] = + prevSetpoint.moduleStates()[m].angle.getSin() + * prevSetpoint.moduleStates()[m].speedMetersPerSecond; + prev_heading[m] = prevSetpoint.moduleStates()[m].angle; + if (prevSetpoint.moduleStates()[m].speedMetersPerSecond < 0.0) { + prev_heading[m] = prev_heading[m].rotateBy(Rotation2d.k180deg); + } + desired_vx[m] = + desiredModuleStates[m].angle.getCos() + * desiredModuleStates[m].speedMetersPerSecond; + desired_vy[m] = + desiredModuleStates[m].angle.getSin() + * desiredModuleStates[m].speedMetersPerSecond; + desired_heading[m] = desiredModuleStates[m].angle; + if (desiredModuleStates[m].speedMetersPerSecond < 0.0) { + desired_heading[m] = desired_heading[m].rotateBy(Rotation2d.k180deg); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs( + prev_heading[m] + .unaryMinus() + .rotateBy(desired_heading[m]) + .getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !epsilonEquals(prevSetpoint.robotRelativeSpeeds(), new ChassisSpeeds()) + && !epsilonEquals(desiredStateRobotRelative, new ChassisSpeeds())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the + // complement + // of the desired angle, and accelerate again. + return generateSetpoint(prevSetpoint, new ChassisSpeeds(), dt, inputVoltage); } - // getRadians() bounds to +/- Pi. - final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; + // Compute the deltas between start and goal. We can then interpolate from the start state + // to + // the goal state; then find the amount we can move from start towards goal in this cycle + // such + // that no kinematic limit is exceeded. + double dx = + desiredStateRobotRelative.vxMetersPerSecond + - prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; + double dy = + desiredStateRobotRelative.vyMetersPerSecond + - prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; + double dtheta = + desiredStateRobotRelative.omegaRadiansPerSecond + - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond; + + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; + + // In cases where an individual module is stopped, we want to remember the right steering + // angle + // to command (since inverse kinematics doesn't care about angle, we can be + // opportunistically + // lazy). + List> overrideSteering = new ArrayList<>(config.numModules); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle + // at + // the current angle, and then backing out the maximum interpolant between start and goal + // states. We remember the minimum across all modules, since that is the active constraint. + for (int m = 0; m < config.numModules; m++) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[m].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + + double max_theta_step = dt * maxSteerVelocityRadsPerSec; + + if (epsilonEquals(prevSetpoint.moduleStates()[m].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final + // steering + // angle, so limit based purely on rotation in place. + if (epsilonEquals(desiredModuleStates[m].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(m, Optional.of(prevSetpoint.moduleStates()[m].angle)); + continue; + } + + var necessaryRotation = + prevSetpoint + .moduleStates()[m] + .angle + .unaryMinus() + .rotateBy(desiredModuleStates[m].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d.kPi); + } + + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = + Math.abs(necessaryRotation.getRadians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(m, Optional.of(desiredModuleStates[m].angle)); + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + m, + Optional.of( + prevSetpoint.moduleStates()[m].angle.rotateBy( + Rotation2d.fromRadians( + Math.signum(necessaryRotation.getRadians()) + * max_theta_step)))); + min_s = 0.0; + } + continue; + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + // Enforce centripetal force limits to prevent sliding. + // We do this by changing max_theta_step to the maximum change in heading over dt + // that would create a large enough radius to keep the centripetal force under the + // friction force. + double maxHeadingChange = + (dt * config.wheelFrictionForce) + / ((config.massKG / config.numModules) + * Math.abs( + prevSetpoint.moduleStates()[m].speedMetersPerSecond)); + max_theta_step = Math.min(max_theta_step, maxHeadingChange); + + double s = + findSteeringMaxS( + prev_vx[m], + prev_vy[m], + prev_heading[m].getRadians(), + desired_vx[m], + desired_vy[m], + desired_heading[m].getRadians(), + max_theta_step); + min_s = Math.min(min_s, s); + } - if (numStepsNeeded <= 1.0) { - // Steer directly to goal angle. - overrideSteering.set(m, Optional.of(desiredModuleStates[m].angle)); - } else { - // Adjust steering by max_theta_step. - overrideSteering.set( - m, - Optional.of( - prevSetpoint.moduleStates()[m].angle.rotateBy( - Rotation2d.fromRadians( - Math.signum(necessaryRotation.getRadians()) * max_theta_step)))); - min_s = 0.0; + // Enforce drive wheel torque limits + Translation2d chassisForceVec = new Translation2d(); + double chassisTorque = 0.0; + for (int m = 0; m < config.numModules; m++) { + double lastVelRadPerSec = + prevSetpoint.moduleStates()[m].speedMetersPerSecond + / config.moduleConfig.wheelRadiusMeters; + // Use the current battery voltage since we won't be able to supply 12v if the + // battery is sagging down to 11v, which will affect the max torque output + double currentDraw = + config.moduleConfig.driveMotor.getCurrent( + Math.abs(lastVelRadPerSec), inputVoltage); + double reverseCurrentDraw = + Math.abs( + config.moduleConfig.driveMotor.getCurrent( + Math.abs(lastVelRadPerSec), -inputVoltage)); + currentDraw = Math.min(currentDraw, config.moduleConfig.driveCurrentLimit); + reverseCurrentDraw = + Math.min(reverseCurrentDraw, config.moduleConfig.driveCurrentLimit); + double forwardModuleTorque = config.moduleConfig.driveMotor.getTorque(currentDraw); + double reverseModuleTorque = + config.moduleConfig.driveMotor.getTorque(reverseCurrentDraw); + + double prevSpeed = prevSetpoint.moduleStates()[m].speedMetersPerSecond; + desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); + double desiredSpeed = desiredModuleStates[m].speedMetersPerSecond; + + int forceSign; + Rotation2d forceAngle = prevSetpoint.moduleStates()[m].angle; + double moduleTorque; + if (epsilonEquals(prevSpeed, 0.0) + || (prevSpeed > 0 && desiredSpeed >= prevSpeed) + || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { + moduleTorque = forwardModuleTorque; + // Torque loss will be fighting motor + moduleTorque -= config.moduleConfig.torqueLoss; + forceSign = 1; // Force will be applied in direction of module + if (prevSpeed < 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } else { + moduleTorque = reverseModuleTorque; + // Torque loss will be helping the motor + moduleTorque += config.moduleConfig.torqueLoss; + forceSign = -1; // Force will be applied in opposite direction of module + if (prevSpeed > 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } + + // Limit torque to prevent wheel slip + moduleTorque = Math.min(moduleTorque, config.maxTorqueFriction); + + double forceAtCarpet = moduleTorque / config.moduleConfig.wheelRadiusMeters; + Translation2d moduleForceVec = new Translation2d(forceAtCarpet * forceSign, forceAngle); + + // Add the module force vector to the chassis force vector + chassisForceVec = chassisForceVec.plus(moduleForceVec); + + // Calculate the torque this module will apply to the chassis + if (!epsilonEquals(0, moduleForceVec.getNorm())) { + Rotation2d angleToModule = config.moduleLocations[m].getAngle(); + Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule); + chassisTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.getSin(); + } } - continue; - } - if (min_s == 0.0) { - // s can't get any lower. Save some CPU. - continue; - } - - // Enforce centripetal force limits to prevent sliding. - // We do this by changing max_theta_step to the maximum change in heading over dt - // that would create a large enough radius to keep the centripetal force under the - // friction force. - double maxHeadingChange = - (dt * config.wheelFrictionForce) - / ((config.massKG / config.numModules) - * Math.abs(prevSetpoint.moduleStates()[m].speedMetersPerSecond)); - max_theta_step = Math.min(max_theta_step, maxHeadingChange); - - double s = - findSteeringMaxS( - prev_vx[m], - prev_vy[m], - prev_heading[m].getRadians(), - desired_vx[m], - desired_vy[m], - desired_heading[m].getRadians(), - max_theta_step); - min_s = Math.min(min_s, s); - } - // Enforce drive wheel torque limits - Translation2d chassisForceVec = new Translation2d(); - double chassisTorque = 0.0; - for (int m = 0; m < config.numModules; m++) { - double lastVelRadPerSec = - prevSetpoint.moduleStates()[m].speedMetersPerSecond - / config.moduleConfig.wheelRadiusMeters; - // Use the current battery voltage since we won't be able to supply 12v if the - // battery is sagging down to 11v, which will affect the max torque output - double currentDraw = - config.moduleConfig.driveMotor.getCurrent(Math.abs(lastVelRadPerSec), inputVoltage); - double reverseCurrentDraw = - Math.abs(config.moduleConfig.driveMotor.getCurrent(Math.abs(lastVelRadPerSec), -inputVoltage)); - currentDraw = Math.min(currentDraw, config.moduleConfig.driveCurrentLimit); - reverseCurrentDraw = Math.min(reverseCurrentDraw, config.moduleConfig.driveCurrentLimit); - double forwardModuleTorque = config.moduleConfig.driveMotor.getTorque(currentDraw); - double reverseModuleTorque = config.moduleConfig.driveMotor.getTorque(reverseCurrentDraw); - - double prevSpeed = prevSetpoint.moduleStates()[m].speedMetersPerSecond; - desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); - double desiredSpeed = desiredModuleStates[m].speedMetersPerSecond; - - int forceSign; - Rotation2d forceAngle = prevSetpoint.moduleStates()[m].angle; - double moduleTorque; - if (epsilonEquals(prevSpeed, 0.0) - || (prevSpeed > 0 && desiredSpeed >= prevSpeed) - || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { - moduleTorque = forwardModuleTorque; - // Torque loss will be fighting motor - moduleTorque -= config.moduleConfig.torqueLoss; - forceSign = 1; // Force will be applied in direction of module - if (prevSpeed < 0) { - forceAngle = forceAngle.plus(Rotation2d.k180deg); + Translation2d chassisAccelVec = chassisForceVec.div(config.massKG); + double chassisAngularAccel = chassisTorque / config.MOI; + + // Use kinematics to convert chassis accelerations to module accelerations + ChassisSpeeds chassisAccel = + new ChassisSpeeds( + chassisAccelVec.getX(), chassisAccelVec.getY(), chassisAngularAccel); + var accelStates = config.toSwerveModuleStates(chassisAccel); + + for (int m = 0; m < config.numModules; m++) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + + double maxVelStep = Math.abs(accelStates[m].speedMetersPerSecond * dt); + + double vx_min_s = + min_s == 1.0 + ? desired_vx[m] + : (desired_vx[m] - prev_vx[m]) * min_s + prev_vx[m]; + double vy_min_s = + min_s == 1.0 + ? desired_vy[m] + : (desired_vy[m] - prev_vy[m]) * min_s + prev_vy[m]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, + // because we + // already know we can't go faster than that. + double s = + findDriveMaxS( + prev_vx[m], + prev_vy[m], + Math.hypot(prev_vx[m], prev_vy[m]), + vx_min_s, + vy_min_s, + Math.hypot(vx_min_s, vy_min_s), + maxVelStep); + min_s = Math.min(min_s, s); } - } else { - moduleTorque = reverseModuleTorque; - // Torque loss will be helping the motor - moduleTorque += config.moduleConfig.torqueLoss; - forceSign = -1; // Force will be applied in opposite direction of module - if (prevSpeed > 0) { - forceAngle = forceAngle.plus(Rotation2d.k180deg); + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond + min_s * dtheta); + retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); + + double prevVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; + double prevVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; + double chassisAccelX = (retSpeeds.vxMetersPerSecond - prevVelX) / dt; + double chassisAccelY = (retSpeeds.vyMetersPerSecond - prevVelY) / dt; + double chassisForceX = chassisAccelX * config.massKG; + double chassisForceY = chassisAccelY * config.massKG; + + double angularAccel = + (retSpeeds.omegaRadiansPerSecond + - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond) + / dt; + double angTorque = angularAccel * config.MOI; + ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque); + + Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces); + + var retStates = config.toSwerveModuleStates(retSpeeds); + double[] accelFF = new double[config.numModules]; + double[] linearForceFF = new double[config.numModules]; + double[] torqueCurrentFF = new double[config.numModules]; + double[] forceXFF = new double[config.numModules]; + double[] forceYFF = new double[config.numModules]; + for (int m = 0; m < config.numModules; m++) { + double wheelForceDist = wheelForces[m].getNorm(); + double appliedForce = + wheelForceDist > 1e-6 + ? wheelForceDist + * wheelForces[m].getAngle().minus(retStates[m].angle).getCos() + : 0.0; + double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters; + double torqueCurrent = config.moduleConfig.driveMotor.getCurrent(wheelTorque); + + final var maybeOverride = overrideSteering.get(m); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[m].angle.unaryMinus().rotateBy(override))) { + retStates[m].speedMetersPerSecond *= -1.0; + appliedForce *= -1.0; + torqueCurrent *= -1.0; + } + retStates[m].angle = override; + } + final var deltaRotation = + prevSetpoint.moduleStates()[m].angle.unaryMinus().rotateBy(retStates[m].angle); + if (flipHeading(deltaRotation)) { + retStates[m].angle = retStates[m].angle.rotateBy(Rotation2d.k180deg); + retStates[m].speedMetersPerSecond *= -1.0; + appliedForce *= -1.0; + torqueCurrent *= -1.0; + } + + accelFF[m] = + (retStates[m].speedMetersPerSecond + - prevSetpoint.moduleStates()[m].speedMetersPerSecond) + / dt; + linearForceFF[m] = appliedForce; + torqueCurrentFF[m] = torqueCurrent; + forceXFF[m] = wheelForces[m].getX(); + forceYFF[m] = wheelForces[m].getY(); } - } - // Limit torque to prevent wheel slip - moduleTorque = Math.min(moduleTorque, config.maxTorqueFriction); + return new SwerveSetpoint( + retSpeeds, + retStates, + new DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF, forceYFF)); + } - double forceAtCarpet = moduleTorque / config.moduleConfig.wheelRadiusMeters; - Translation2d moduleForceVec = new Translation2d(forceAtCarpet * forceSign, forceAngle); + /** + * Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds + * passed into or returned from this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @param inputVoltage The input voltage of the drive motor controllers, in volts. This can also + * be a static nominal voltage if you do not want the setpoint generator to react to changes + * in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input + * voltage will be clamped to a minimum of the robot controller's brownout voltage. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging + * to desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredStateRobotRelative, + Time dt, + Voltage inputVoltage) { + return generateSetpoint( + prevSetpoint, desiredStateRobotRelative, dt.in(Seconds), inputVoltage.in(Volts)); + } - // Add the module force vector to the chassis force vector - chassisForceVec = chassisForceVec.plus(moduleForceVec); + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + *

Note: This method will automatically use the current robot controller input voltage. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging + * to desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + SwerveSetpoint prevSetpoint, ChassisSpeeds desiredStateRobotRelative, double dt) { + return generateSetpoint( + prevSetpoint, desiredStateRobotRelative, dt, RobotController.getInputVoltage()); + } - // Calculate the torque this module will apply to the chassis - if (!epsilonEquals(0, moduleForceVec.getNorm())) { - Rotation2d angleToModule = config.moduleLocations[m].getAngle(); - Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule); - chassisTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.getSin(); - } + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + *

Note: This method will automatically use the current robot controller input voltage. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging + * to desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + SwerveSetpoint prevSetpoint, ChassisSpeeds desiredStateRobotRelative, Time dt) { + return generateSetpoint( + prevSetpoint, + desiredStateRobotRelative, + dt.in(Seconds), + RobotController.getBatteryVoltage()); } - Translation2d chassisAccelVec = chassisForceVec.div(config.massKG); - double chassisAngularAccel = chassisTorque / config.MOI; - - // Use kinematics to convert chassis accelerations to module accelerations - ChassisSpeeds chassisAccel = - new ChassisSpeeds(chassisAccelVec.getX(), chassisAccelVec.getY(), chassisAngularAccel); - var accelStates = config.toSwerveModuleStates(chassisAccel); - - for (int m = 0; m < config.numModules; m++) { - if (min_s == 0.0) { - // No need to carry on. - break; - } - - double maxVelStep = Math.abs(accelStates[m].speedMetersPerSecond * dt); - - double vx_min_s = - min_s == 1.0 ? desired_vx[m] : (desired_vx[m] - prev_vx[m]) * min_s + prev_vx[m]; - double vy_min_s = - min_s == 1.0 ? desired_vy[m] : (desired_vy[m] - prev_vy[m]) * min_s + prev_vy[m]; - // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we - // already know we can't go faster than that. - double s = - findDriveMaxS( - prev_vx[m], - prev_vy[m], - Math.hypot(prev_vx[m], prev_vy[m]), - vx_min_s, - vy_min_s, - Math.hypot(vx_min_s, vy_min_s), - maxVelStep); - min_s = Math.min(min_s, s); + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + private static boolean flipHeading(Rotation2d prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; } - ChassisSpeeds retSpeeds = - new ChassisSpeeds( - prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond + min_s * dx, - prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond + min_s * dy, - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond + min_s * dtheta); - retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt); - - double prevVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond; - double prevVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond; - double chassisAccelX = (retSpeeds.vxMetersPerSecond - prevVelX) / dt; - double chassisAccelY = (retSpeeds.vyMetersPerSecond - prevVelY) / dt; - double chassisForceX = chassisAccelX * config.massKG; - double chassisForceY = chassisAccelY * config.massKG; - - double angularAccel = - (retSpeeds.omegaRadiansPerSecond - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond) - / dt; - double angTorque = angularAccel * config.MOI; - ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque); - - Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces); - - var retStates = config.toSwerveModuleStates(retSpeeds); - double[] accelFF = new double[config.numModules]; - double[] linearForceFF = new double[config.numModules]; - double[] torqueCurrentFF = new double[config.numModules]; - double[] forceXFF = new double[config.numModules]; - double[] forceYFF = new double[config.numModules]; - for (int m = 0; m < config.numModules; m++) { - double wheelForceDist = wheelForces[m].getNorm(); - double appliedForce = - wheelForceDist > 1e-6 - ? wheelForceDist * wheelForces[m].getAngle().minus(retStates[m].angle).getCos() - : 0.0; - double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters; - double torqueCurrent = config.moduleConfig.driveMotor.getCurrent(wheelTorque); - - final var maybeOverride = overrideSteering.get(m); - if (maybeOverride.isPresent()) { - var override = maybeOverride.get(); - if (flipHeading(retStates[m].angle.unaryMinus().rotateBy(override))) { - retStates[m].speedMetersPerSecond *= -1.0; - appliedForce *= -1.0; - torqueCurrent *= -1.0; + private static double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; } - retStates[m].angle = override; - } - final var deltaRotation = - prevSetpoint.moduleStates()[m].angle.unaryMinus().rotateBy(retStates[m].angle); - if (flipHeading(deltaRotation)) { - retStates[m].angle = retStates[m].angle.rotateBy(Rotation2d.k180deg); - retStates[m].speedMetersPerSecond *= -1.0; - appliedForce *= -1.0; - torqueCurrent *= -1.0; - } - - accelFF[m] = - (retStates[m].speedMetersPerSecond - prevSetpoint.moduleStates()[m].speedMetersPerSecond) - / dt; - linearForceFF[m] = appliedForce; - torqueCurrentFF[m] = torqueCurrent; - forceXFF[m] = wheelForces[m].getX(); - forceYFF[m] = wheelForces[m].getY(); } - return new SwerveSetpoint( - retSpeeds, - retStates, - new DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF, forceYFF)); - } - - /** - * Generate a new setpoint with explicit battery voltage. Note: Do not discretize ChassisSpeeds - * passed into or returned from this method. This method will discretize the speeds for you. - * - * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous - * iteration setpoint instead of the actual measured/estimated kinematic state. - * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks or - * a path following algorithm. - * @param dt The loop time. - * @param inputVoltage The input voltage of the drive motor controllers, in volts. This can also - * be a static nominal voltage if you do not want the setpoint generator to react to changes - * in input voltage. If the given voltage is NaN, it will be assumed to be 12v. The input - * voltage will be clamped to a minimum of the robot controller's brownout voltage. - * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to - * desiredState quickly. - */ - public SwerveSetpoint generateSetpoint( - final SwerveSetpoint prevSetpoint, - ChassisSpeeds desiredStateRobotRelative, - Time dt, - Voltage inputVoltage) { - return generateSetpoint( - prevSetpoint, desiredStateRobotRelative, dt.in(Seconds), inputVoltage.in(Volts)); - } - - /** - * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from - * this method. This method will discretize the speeds for you. - * - *

Note: This method will automatically use the current robot controller input voltage. - * - * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous - * iteration setpoint instead of the actual measured/estimated kinematic state. - * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks or - * a path following algorithm. - * @param dt The loop time. - * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to - * desiredState quickly. - */ - public SwerveSetpoint generateSetpoint( - SwerveSetpoint prevSetpoint, ChassisSpeeds desiredStateRobotRelative, double dt) { - return generateSetpoint( - prevSetpoint, desiredStateRobotRelative, dt, RobotController.getInputVoltage()); - } - - /** - * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from - * this method. This method will discretize the speeds for you. - * - *

Note: This method will automatically use the current robot controller input voltage. - * - * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous - * iteration setpoint instead of the actual measured/estimated kinematic state. - * @param desiredStateRobotRelative The desired state of motion, such as from the driver sticks or - * a path following algorithm. - * @param dt The loop time. - * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to - * desiredState quickly. - */ - public SwerveSetpoint generateSetpoint( - SwerveSetpoint prevSetpoint, ChassisSpeeds desiredStateRobotRelative, Time dt) { - return generateSetpoint( - prevSetpoint, - desiredStateRobotRelative, - dt.in(Seconds), - RobotController.getBatteryVoltage()); - } - - /** - * Check if it would be faster to go to the opposite of the goal heading (and reverse drive - * direction). - * - * @param prevToGoal The rotation from the previous state to the goal state (i.e. - * prev.inverse().rotateBy(goal)). - * @return True if the shortest path to achieve this rotation involves flipping the drive - * direction. - */ - private static boolean flipHeading(Rotation2d prevToGoal) { - return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; - } - - private static double unwrapAngle(double ref, double angle) { - double diff = angle - ref; - if (diff > Math.PI) { - return angle - 2.0 * Math.PI; - } else if (diff < -Math.PI) { - return angle + 2.0 * Math.PI; - } else { - return angle; + @FunctionalInterface + private interface Function2d { + double f(double x, double y); } - } - - @FunctionalInterface - private interface Function2d { - double f(double x, double y); - } - - /** - * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. - * This is a pretty naive way to do root finding, but it's usually faster than simple bisection - * while being robust in ways that e.g. the Newton-Raphson method isn't. - * - * @param func The Function2d to take the root of. - * @param x_0 x value of the lower bracket. - * @param y_0 y value of the lower bracket. - * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during - * recursion) - * @param x_1 x value of the upper bracket. - * @param y_1 y value of the upper bracket. - * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during - * recursion) - * @param iterations_left Number of iterations of root finding left. - * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the - * (approximate) root. - */ - private static double findRoot( - Function2d func, - double x_0, - double y_0, - double f_0, - double x_1, - double y_1, - double f_1, - int iterations_left) { - var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); - - if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { - return s_guess; + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private static double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + + if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { + return s_guess; + } + + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot( + func, + x_guess, + y_guess, + f_guess, + x_1, + y_1, + f_1, + iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + private static double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = (x, y) -> unwrapAngle(f_0, Math.atan2(y, x)) - offset; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_STEER_ITERATIONS); + } + + private static double findDriveMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_vel_step) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = (x, y) -> Math.hypot(x, y) - offset; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_DRIVE_ITERATIONS); } - var x_guess = (x_1 - x_0) * s_guess + x_0; - var y_guess = (y_1 - y_0) * s_guess + y_0; - var f_guess = func.f(x_guess, y_guess); - if (Math.signum(f_0) == Math.signum(f_guess)) { - // 0 and guess on same side of root, so use upper bracket. - return s_guess - + (1.0 - s_guess) - * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); - } else { - // Use lower bracket. - return s_guess - * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + private static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); } - } - - private static double findSteeringMaxS( - double x_0, - double y_0, - double f_0, - double x_1, - double y_1, - double f_1, - double max_deviation) { - f_1 = unwrapAngle(f_0, f_1); - double diff = f_1 - f_0; - if (Math.abs(diff) <= max_deviation) { - // Can go all the way to s=1. - return 1.0; + + private static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); } - double offset = f_0 + Math.signum(diff) * max_deviation; - Function2d func = (x, y) -> unwrapAngle(f_0, Math.atan2(y, x)) - offset; - return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_STEER_ITERATIONS); - } - - private static double findDriveMaxS( - double x_0, double y_0, double f_0, double x_1, double y_1, double f_1, double max_vel_step) { - double diff = f_1 - f_0; - if (Math.abs(diff) <= max_vel_step) { - // Can go all the way to s=1. - return 1.0; + + private static boolean epsilonEquals(ChassisSpeeds s1, ChassisSpeeds s2) { + return epsilonEquals(s1.vxMetersPerSecond, s2.vxMetersPerSecond) + && epsilonEquals(s1.vyMetersPerSecond, s2.vyMetersPerSecond) + && epsilonEquals(s1.omegaRadiansPerSecond, s2.omegaRadiansPerSecond); } - double offset = f_0 + Math.signum(diff) * max_vel_step; - Function2d func = (x, y) -> Math.hypot(x, y) - offset; - return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, MAX_DRIVE_ITERATIONS); - } - - private static boolean epsilonEquals(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); - } - - private static boolean epsilonEquals(double a, double b) { - return epsilonEquals(a, b, kEpsilon); - } - - private static boolean epsilonEquals(ChassisSpeeds s1, ChassisSpeeds s2) { - return epsilonEquals(s1.vxMetersPerSecond, s2.vxMetersPerSecond) - && epsilonEquals(s1.vyMetersPerSecond, s2.vyMetersPerSecond) - && epsilonEquals(s1.omegaRadiansPerSecond, s2.omegaRadiansPerSecond); - } } diff --git a/src/main/java/frc/robot/constants/GlobalConstants.java b/src/main/java/frc/robot/constants/GlobalConstants.java index 604dc944..efd9e523 100644 --- a/src/main/java/frc/robot/constants/GlobalConstants.java +++ b/src/main/java/frc/robot/constants/GlobalConstants.java @@ -3,8 +3,9 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; - +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; @@ -12,19 +13,21 @@ import edu.wpi.first.units.measure.MomentOfInertia; public class GlobalConstants { - + private static final SwerveModuleConstants EXAMPLE_MODULE = TunerConstants.FrontLeft; public static final LinearVelocity MAX_TRANSLATIONAL_SPEED = TunerConstants.kSpeedAt12Volts; - public static final AngularVelocity MAX_ROTATIONAL_SPEED = RotationsPerSecond.of(1); + public static final AngularVelocity MAX_ROTATIONAL_SPEED = RotationsPerSecond.of(1); public static final Mass ROBOT_MASS = Pounds.of(150); - public static final MomentOfInertia ROBOT_MOI = KilogramSquareMeters.of(6.883); + public static final MomentOfInertia ROBOT_MOI = KilogramSquareMeters.of(11.61); public static final double COEFFICIENT_OF_FRICTION = 1.0; - public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.DriveMotorGearRatio); - public static final DCMotor STEER_MOTOR = DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.SteerMotorGearRatio); + public static final DCMotor DRIVE_MOTOR = + DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.DriveMotorGearRatio); + public static final DCMotor STEER_MOTOR = + DCMotor.getKrakenX60Foc(1).withReduction(EXAMPLE_MODULE.SteerMotorGearRatio); public static class ControllerConstants { public static final int LEFT_DRIVE_CONTROLLER = 0; @@ -34,27 +37,48 @@ public static class ControllerConstants { private static RobotConfig robotConfigPathplanner; + public static RobotConfig getRobotConfigPathplanner() { + if (robotConfigPathplanner == null) { + try { + Translation2d[] moduleOffsets = new Translation2d[4]; + SwerveModuleConstants[] constants = + new SwerveModuleConstants[] { + TunerConstants.FrontLeft, + TunerConstants.FrontRight, + TunerConstants.BackLeft, + TunerConstants.BackRight + }; + for (int i = 0; i < constants.length; i++) { + moduleOffsets[i] = + new Translation2d(constants[i].LocationX, constants[i].LocationY); + } + + robotConfigPathplanner = + new RobotConfig( + ROBOT_MASS, + ROBOT_MOI, + new ModuleConfig( + Meters.of(EXAMPLE_MODULE.WheelRadius), + MAX_TRANSLATIONAL_SPEED, + COEFFICIENT_OF_FRICTION, + DRIVE_MOTOR, + EXAMPLE_MODULE.DriveMotorInitialConfigs.CurrentLimits + .getStatorCurrentLimitMeasure(), + 1), + moduleOffsets); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + throw new RuntimeException("Failed to load robot config from pathplanner."); + } + } + return robotConfigPathplanner; + } + // public static RobotConfig getRobotConfigPathplanner() { // if (robotConfigPathplanner == null) { // try{ - // Translation2d[] moduleOffsets = new Translation2d[4]; - // SwerveModuleConstants[] constants = new SwerveModuleConstants[]{ - // TunerConstants.FrontLeft, TunerConstants.FrontRight, TunerConstants.BackLeft, TunerConstants.BackRight - // }; - // for (int i = 0; i < constants.length; i++) { - // moduleOffsets[i] = new Translation2d(constants[i].LocationX, constants[i].LocationY); - // } - - // robotConfigPathplanner = new RobotConfig(ROBOT_MASS, ROBOT_MOI, - // new ModuleConfig( - // Meters.of(EXAMPLE_MODULE.WheelRadius), - // MAX_TRANSLATIONAL_SPEED, - // COEFFICIENT_OF_FRICTION, - // DRIVE_MOTOR, - // EXAMPLE_MODULE.DriveMotorInitialConfigs.CurrentLimits.getStatorCurrentLimitMeasure(), - // 1 - // ), - // moduleOffsets); + // robotConfigPathplanner = RobotConfig.fromGUISettings(); // } catch (Exception e) { // // Handle exception as needed // e.printStackTrace(); @@ -63,17 +87,4 @@ public static class ControllerConstants { // } // return robotConfigPathplanner; // } - - public static RobotConfig getRobotConfigPathplanner() { - if (robotConfigPathplanner == null) { - try{ - robotConfigPathplanner = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - throw new RuntimeException("Failed to load robot config from pathplanner."); - } - } - return robotConfigPathplanner; - } } diff --git a/src/main/java/frc/robot/constants/TunerConstants.java b/src/main/java/frc/robot/constants/TunerConstants.java index 356855c6..fafebfd7 100644 --- a/src/main/java/frc/robot/constants/TunerConstants.java +++ b/src/main/java/frc/robot/constants/TunerConstants.java @@ -14,7 +14,6 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; - import edu.wpi.first.units.measure.*; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -25,16 +24,20 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(40).withKI(0).withKD(0.0) - .withKS(0.00).withKV(0).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(40) + .withKI(0) + .withKD(0.0) + .withKS(0.00) + .withKV(0) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.0).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); // private static final Slot0Configs driveGains = new Slot0Configs() // .withKP(0.1).withKI(0).withKD(0) // .withKS(0).withKV(0.124); @@ -57,14 +60,16 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can + // set a relatively low + // stator current limit to help avoid brownouts without + // impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; @@ -92,36 +97,37 @@ public class TunerConstants { // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.0439); // Simulated voltage necessary to overcome friction private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withCANcoderInitialConfigs(cancoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withCANcoderInitialConfigs(cancoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left private static final int kFrontLeftDriveMotorId = 1; @@ -167,27 +173,57 @@ public class TunerConstants { private static final Distance kBackRightXPos = Inches.of(-11.5); private static final Distance kBackRightYPos = Inches.of(-11.5); - - public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftCANcoderInverted); - public static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightCANcoderInverted); - public static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftCANcoderInverted); - public static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightCANcoderInverted); + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftCANcoderInverted); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightCANcoderInverted); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftCANcoderInverted); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightCANcoderInverted); /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. + * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot + * program,. */ public static CommandSwerveDrivetrain createDrivetrain() { return new CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index cde73001..26b73205 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -347,17 +347,19 @@ public void periodic() { "Drive/setpointChassisSpeeds", m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds()); - ChassisSpeeds speedsPreview = m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds(); + ChassisSpeeds speedsPreview = + m_applyFieldSpeedsOrbit.getPreviousSetpoint().robotRelativeSpeeds(); - double currentSpeed = Math.hypot(speedsPreview.vxMetersPerSecond, speedsPreview.vyMetersPerSecond); + double currentSpeed = + Math.hypot(speedsPreview.vxMetersPerSecond, speedsPreview.vyMetersPerSecond); double acceleration = (currentSpeed - lastSpeed) / 0.02; lastSpeed = currentSpeed; Logger.recordOutput("Drive/Velocity", currentSpeed); Logger.recordOutput("Drive/Acceleration", acceleration); - Logger.recordOutput("Drive/NotZero", m_applyFieldSpeedsOrbit.getChassisSpeeds().vxMetersPerSecond != 0); - + Logger.recordOutput( + "Drive/NotZero", m_applyFieldSpeedsOrbit.getChassisSpeeds().vxMetersPerSecond != 0); Logger.recordOutput("Drive/ModuleTargets", getState().ModuleTargets); Logger.recordOutput("Drive/ModulePositions", getState().ModulePositions); diff --git a/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java b/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java index f0360830..df782bf8 100644 --- a/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java +++ b/src/main/java/frc/robot/subsystems/FieldOrientedOrbitSwerveRequest.java @@ -143,7 +143,8 @@ public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modu // Apply all other limits previousSetpoint = - setpointGenerator.generateSetpoint(previousSetpoint, robotRelativeSpeeds, timestep, 12.0); + setpointGenerator.generateSetpoint( + previousSetpoint, robotRelativeSpeeds, timestep, 12.0); DriveFeedforwards feedforwards = previousSetpoint.feedforwards();