diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3821fc44..12accfb0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,6 +24,7 @@ public final class Constants public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag + public static final double MAX_SPEED = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration. public static final class AutonConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34dceccd..2fedde3a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,6 +42,15 @@ public RobotContainer() // Configure the trigger bindings configureBindings(); + + + // Applies deadbands and inverts controls because joysticks + // are back-right positive while robot + // controls are front-left positive + // left stick controls translation + // right stick controls the rotational velocity + // buttons are quick rotation positions to different ways to face + // WARNING: default buttons are on the same buttons as the ones defined in configureBindings AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index b3c88a81..1513f2aa 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import frc.robot.Constants; import frc.robot.Constants.AutonConstants; import java.io.File; import java.util.function.DoubleSupplier; @@ -48,10 +49,6 @@ public class SwerveSubsystem extends SubsystemBase * Swerve drive object. */ private final SwerveDrive swerveDrive; - /** - * Maximum speed of the robot in meters per second, used to limit acceleration. - */ - public double maximumSpeed = Units.feetToMeters(14.5); /** * Initialize {@link SwerveDrive} with the directory provided. @@ -78,7 +75,7 @@ public SwerveSubsystem(File directory) SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed); + swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor); } catch (Exception e) @@ -98,7 +95,7 @@ public SwerveSubsystem(File directory) */ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, maximumSpeed); + swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED); } /** @@ -516,7 +513,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin headingX, headingY, getHeading().getRadians(), - maximumSpeed); + Constants.MAX_SPEED); } /** @@ -536,7 +533,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an yInput, angle.getRadians(), getHeading().getRadians(), - maximumSpeed); + Constants.MAX_SPEED); } /** diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 0aa2fbd9..1745f2cc 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -101,6 +101,10 @@ public class SwerveDrive * Whether to correct heading when driving translationally. Set to true to enable. */ public boolean headingCorrection = false; + /** + * Amount of seconds the duration of the timestep the speeds should be applied for. + */ + private double discretizationdtSeconds = 0.02; /** * Deadband for speeds in heading correction. */ @@ -342,6 +346,19 @@ public void setHeadingCorrection(boolean state, double deadband) HEADING_CORRECTION_DEADBAND = deadband; } + /** + * Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same time. + * The inputs are added together so this is not intneded to be used to give the driver both methods of control. + * + * @param fieldOrientedVelocity The field oriented velocties to use + * @param robotOrientedVelocity The robot oriented velocties to use + */ + public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity, ChassisSpeeds robotOrientedVelocity) + { + ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()).plus(robotOrientedVelocity); + drive(TotalVelocties); + } + /** * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. * @@ -464,7 +481,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 if (chassisVelocityCorrection) { - velocity = ChassisSpeeds.discretize(velocity, 0.02); + velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds); } // Heading Angular Velocity Deadband, might make a configuration option later. @@ -502,7 +519,6 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent setRawModuleStates(swerveModuleStates, isOpenLoop); } - /** * Set the maximum speeds for desaturation. * @@ -1122,11 +1138,11 @@ public void resetDriveEncoders() * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the * internal offsets to prevent double offsetting. */ - public void pushOffsetsToControllers() + public void pushOffsetsToEncoders() { for (SwerveModule module : swerveModules) { - module.pushOffsetsToControllers(); + module.pushOffsetsToEncoders(); } } @@ -1156,4 +1172,15 @@ public void setCosineCompensator(boolean enabled) } } + /** + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction + * + * @param enable + * @param dtSeconds + */ + public void setChassisDiscretization(boolean enable, double dtSeconds){ + chassisVelocityCorrection = enable; + discretizationdtSeconds = dtSeconds; + } + } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 60a3a5c1..8f71e615 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -257,7 +257,7 @@ public void setAntiJitter(boolean antiJitter) this.antiJitterEnabled = antiJitter; if (antiJitter) { - pushOffsetsToControllers(); + pushOffsetsToEncoders(); } else { restoreInternalOffset(); @@ -585,7 +585,7 @@ public SwerveModuleConfiguration getConfiguration() /** * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset. */ - public void pushOffsetsToControllers() + public void pushOffsetsToEncoders() { if (absoluteEncoder != null && angleOffset == configuration.angleOffset) {