diff --git a/README.md b/README.md index be42744..25bff00 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,4 @@ +Buy Me a Robot at ko-fi.com # Yet Another Generic Swerve Library * In early 2023 `swerve-lib` created by SwerveDriveSpecialties officially became unmaintained after not being updated in 2022. * This library aims to simplify Swerve Drive implementations while not sacrificing speed or processing power. @@ -6,7 +7,9 @@ * The swerve drive is configurable via JSON files, and you can initialize the entire swerve drive with a similar line as the following. * Simulation support. ```java -SwerveDrive swerveDrive = new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")).createSwerveDrive(); +import edu.wpi.first.math.util.Units; + +SwerveDrive swerveDrive=new SwerveParser(new File(Filesystem.getDeployDirectory(),"swerve")).createSwerveDrive(Units.feetToMeters(14.5)); ``` * The library is located in [swervelib/](./swervelib) with documentation in [docs/](./docs) and example JSON in [deploy](./deploy). diff --git a/swervelib/SwerveController.java b/swervelib/SwerveController.java index c1dccef..0bb1fa7 100644 --- a/swervelib/SwerveController.java +++ b/swervelib/SwerveController.java @@ -94,19 +94,21 @@ public class SwerveController /** * Get the chassis speeds based on controller input of 1 joystick [-1,1] and an angle. * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. + * @param xInput X joystick input for the robot to move in the X direction. X = xInput * maxSpeed + * @param yInput Y joystick input for the robot to move in the Y direction. Y = yInput * + * maxSpeed; * @param angle The desired angle of the robot in radians. * @param currentHeadingAngleRadians The current robot heading in radians. + * @param maxSpeed Maximum speed in meters per second. * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. */ public ChassisSpeeds getTargetSpeeds( - double xInput, double yInput, double angle, double currentHeadingAngleRadians) + double xInput, double yInput, double angle, double currentHeadingAngleRadians, double maxSpeed) { // Convert joystick inputs to m/s by scaling by max linear speed. Also uses a cubic function // to allow for precise control and fast movement. - double x = xInput * config.maxSpeed; - double y = yInput * config.maxSpeed; + double x = xInput * maxSpeed; + double y = yInput * maxSpeed; return getRawTargetSpeeds(x, y, angle, currentHeadingAngleRadians); } @@ -134,6 +136,8 @@ public class SwerveController * @param headingX X joystick which controls the angle of the robot. * @param headingY Y joystick which controls the angle of the robot. * @param currentHeadingAngleRadians The current robot heading in radians. + * @param maxSpeed Maximum speed of the drive motors in meters per second, multiplier of the xInput + * and yInput. * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. */ public ChassisSpeeds getTargetSpeeds( @@ -141,7 +145,8 @@ public class SwerveController double yInput, double headingX, double headingY, - double currentHeadingAngleRadians) + double currentHeadingAngleRadians, + double maxSpeed) { // Converts the horizontal and vertical components to the commanded angle, in radians, unless // the joystick is near @@ -150,7 +155,7 @@ public class SwerveController // position when stick released). double angle = withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY); - ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians); + ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians, maxSpeed); // Used for the position hold feature lastAngleScalar = angle; diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index dc70b50..5d759fd 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -21,7 +21,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -45,7 +44,7 @@ public class SwerveDrive { /** - * Swerve Kinematics object utilizing second order kinematics. + * Swerve Kinematics object. */ public final SwerveDriveKinematics kinematics; /** @@ -67,34 +66,44 @@ public class SwerveDrive /** * Odometry lock to ensure thread safety. */ - private final Lock odometryLock = new ReentrantLock(); + private final Lock odometryLock = new ReentrantLock(); /** * Field object. */ - public Field2d field = new Field2d(); + public Field2d field = new Field2d(); /** * Swerve controller for controlling heading of the robot. */ public SwerveController swerveController; /** - * Trustworthiness of the internal model of how motors should be moving Measured in expected standard deviation - * (meters of position and degrees of rotation) - */ - public Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); - /** - * Trustworthiness of the vision system Measured in expected standard deviation (meters of position and degrees of + * Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of * rotation) */ - public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, 0.9, 0.9); + public Matrix stateStdDevs = VecBuilder.fill(0.1, + 0.1, + 0.1); + /** + * The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more + * points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)} + * with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get + * vision accurate to inches instead of feet. + */ + public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, + 0.9, + 0.9); /** * Invert odometry readings of drive motor positions, used as a patch for debugging currently. */ - public boolean invertOdometry = false; + public boolean invertOdometry = false; /** * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's * correction. */ - public boolean chassisVelocityCorrection = true; + public boolean chassisVelocityCorrection = true; + /** + * Whether to correct heading when driving translationally. Set to true to enable. + */ + public boolean headingCorrection = false; /** * Swerve IMU device for sensing the heading of the robot. */ @@ -106,11 +115,23 @@ public class SwerveDrive /** * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ - private int moduleSynchronizationCounter = 0; + private int moduleSynchronizationCounter = 0; /** * The last heading set in radians. */ - private double lastHeadingRadians = 0; + private double lastHeadingRadians = 0; + /** + * The absolute max speed that your robot can reach while translating in meters per second. + */ + private double attainableMaxTranslationalSpeedMetersPerSecond = 0; + /** + * The absolute max speed the robot can reach while rotating radians per second. + */ + private double attainableMaxRotationalVelocityRadiansPerSecond = 0; + /** + * Maximum speed of the robot in meters per second. + */ + private double maxSpeedMPS; /** * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the @@ -122,10 +143,13 @@ public class SwerveDrive * @param config The {@link SwerveDriveConfiguration} configuration to base the swerve drive off of. * @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the * {@link SwerveController}. + * @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link Units#feetToMeters(double)} if + * you have feet per second! */ public SwerveDrive( - SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig) + SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS) { + this.maxSpeedMPS = maxSpeedMPS; swerveDriveConfiguration = config; swerveController = new SwerveController(controllerConfig); // Create Kinematics from swerve module locations. @@ -165,7 +189,7 @@ public class SwerveDrive if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { - SwerveDriveTelemetry.maxSpeed = swerveDriveConfiguration.maxSpeed; + SwerveDriveTelemetry.maxSpeed = maxSpeedMPS; SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity; SwerveDriveTelemetry.moduleCount = swerveModules.length; SwerveDriveTelemetry.sizeFrontBack = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, true, @@ -213,53 +237,75 @@ public class SwerveDrive } /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and calculates - * and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel - * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. This method - * defaults to no heading correction. + * Set the conversion factor for the angle/azimuth motor controller. * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per - * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is - * torwards port (left). In field-relative mode, positive x is away from the alliance wall (field - * North) and positive y is torwards the left wall when looking through the driver station glass - * (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot - * relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param conversionFactor Angle motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. */ - public void drive( - Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) + public void setAngleMotorConversionFactor(double conversionFactor) { - drive(translation, rotation, fieldRelative, isOpenLoop, false); + for (SwerveModule module : swerveModules) + { + module.setAngleMotorConversionFactor(conversionFactor); + } } /** - * The primary method for controlling the drivebase. Takes a Translation2d and a rotation rate, and calculates and - * commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel - * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. + * Set the conversion factor for the drive motor controller. * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per - * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is - * torwards port (left). In field-relative mode, positive x is away from the alliance wall - * (field North) and positive y is torwards the left wall when looking through the driver - * station glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot - * relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. - * @param headingCorrection Whether to correct heading when driving translationally. Set to true to enable. + * @param conversionFactor Drive motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. */ - public void drive( - Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, boolean headingCorrection) + public void setDriveMotorConversionFactor(double conversionFactor) + { + for (SwerveModule module : swerveModules) + { + module.setDriveMotorConversionFactor(conversionFactor); + } + } + + /** + * Set the heading correction capabilities of YAGSL. + * + * @param state {@link SwerveDrive#headingCorrection} state. + */ + public void setHeadingCorrection(boolean state) + { + headingCorrection = state; + } + + /** + * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. + * + * @param velocity Velocity of the robot desired. + */ + public void driveFieldOriented(ChassisSpeeds velocity) + { + ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw()); + drive(fieldOrientedVelocity); + } + + /** + * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the swerve module states, + * to achieve the goal. + * + * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. + */ + public void drive(ChassisSpeeds velocity) + { + drive(velocity, false); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link ChassisSpeeds}, and calculates and commands module + * states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Also has + * field- and robot-relative modes, which affect how the translation vector is used. + * + * @param velocity The chassis speeds to set the robot to achieve. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + */ + public void drive(ChassisSpeeds velocity, boolean isOpenLoop) { - // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if - // necessary. - ChassisSpeeds velocity = - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getYaw()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); // Thank you to Jared Russell FRC254 for Open Loop Compensation Code // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 @@ -279,7 +325,7 @@ public class SwerveDrive // Originally made by Team 1466 Webb Robotics. if (headingCorrection) { - if (Math.abs(rotation) < 0.01) + if (Math.abs(velocity.omegaRadiansPerSecond) < 0.01) { velocity.omegaRadiansPerSecond = swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians()); @@ -307,6 +353,35 @@ public class SwerveDrive setRawModuleStates(swerveModuleStates, isOpenLoop); } + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and calculates + * and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel + * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per + * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is + * torwards port (left). In field-relative mode, positive x is away from the alliance wall (field + * North) and positive y is torwards the left wall when looking through the driver station glass + * (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot + * relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + */ + public void drive( + Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) + { + // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if + // necessary. + ChassisSpeeds velocity = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + translation.getX(), translation.getY(), rotation, getYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + + drive(velocity, isOpenLoop); + } + /** * Set the maximum speeds for desaturation. * @@ -323,10 +398,8 @@ public class SwerveDrive double attainableMaxRotationalVelocityRadiansPerSecond) { setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond); - swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond = - attainableMaxTranslationalSpeedMetersPerSecond; - swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond = - attainableMaxRotationalVelocityRadiansPerSecond; + this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond; + this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond; } /** @@ -338,13 +411,12 @@ public class SwerveDrive private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { // Desaturates wheel speeds - if (swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond != 0 || - swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond != 0) + if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(), - swerveDriveConfiguration.maxSpeed, - swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond, - swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond); + maxSpeedMPS, + attainableMaxTranslationalSpeedMetersPerSecond, + attainableMaxRotationalVelocityRadiansPerSecond); } // Sets states @@ -384,7 +456,7 @@ public class SwerveDrive } /** - * Set chassis speeds with closed-loop velocity control and second order kinematics. + * Set chassis speeds with closed-loop velocity control. * * @param chassisSpeeds Chassis speeds to set. */ @@ -625,8 +697,7 @@ public class SwerveDrive } /** - * Set the maximum speed of the drive motors, modified {@link SwerveControllerConfiguration#maxSpeed} and - * {@link SwerveDriveConfiguration#maxSpeed} which is used for the + * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. @@ -635,27 +706,25 @@ public class SwerveDrive * @param updateModuleFeedforward Update the swerve module feedforward to account for the new maximum speed. This * should be true unless you have replaced the drive motor feedforward with * {@link SwerveDrive#replaceSwerveModuleFeedforward(SimpleMotorFeedforward)} + * @param optimalVoltage Optimal voltage to use for the feedforward. */ - public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward) + public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward, double optimalVoltage) { - swerveDriveConfiguration.maxSpeed = maximumSpeed; -// swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond = maximumSpeed; - swerveController.config.maxSpeed = maximumSpeed; -// swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond = swerveController.config -// .maxAngularVelocity; + maxSpeedMPS = maximumSpeed; + swerveDriveConfiguration.physicalCharacteristics.optimalVoltage = optimalVoltage; for (SwerveModule module : swerveModules) { - module.configuration.maxSpeed = maximumSpeed; if (updateModuleFeedforward) { - module.feedforward = module.configuration.createDriveFeedforward(); + module.feedforward = SwerveMath.createDriveFeedforward(optimalVoltage, + maximumSpeed, + swerveDriveConfiguration.physicalCharacteristics.wheelGripCoefficientOfFriction); } } } /** - * Set the maximum speed of the drive motors, modified {@link SwerveControllerConfiguration#maxSpeed} and - * {@link SwerveDriveConfiguration#maxSpeed} which is used for the + * Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the * {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the @@ -665,7 +734,7 @@ public class SwerveDrive */ public void setMaximumSpeed(double maximumSpeed) { - setMaximumSpeed(maximumSpeed, true); + setMaximumSpeed(maximumSpeed, true, swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); } /** @@ -838,29 +907,14 @@ public class SwerveDrive * AFTER USING THIS FUNCTION!
To update your gyroscope readings directly use * {@link SwerveDrive#setGyroOffset(Rotation3d)}. * - * @param robotPose Robot {@link Pose2d} as measured by vision. - * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from - * {@link Timer#getFPGATimestamp()} or similar sources. - * @param soft Add vision estimate using the - * {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)} function, or hard - * reset odometry with the given position with - * {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry#resetPosition(Rotation2d, - * SwerveModulePosition[], Pose2d)}. - * @param trustWorthiness Trust level of vision reading when using a soft measurement, used to multiply the standard - * deviation. Set to 1 for full trust. Higher = Less Weight. + * @param robotPose Robot {@link Pose2d} as measured by vision. + * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from + * {@link Timer#getFPGATimestamp()} or similar sources. */ - public void addVisionMeasurement(Pose2d robotPose, double timestamp, boolean soft, double trustWorthiness) + public void addVisionMeasurement(Pose2d robotPose, double timestamp) { odometryLock.lock(); - if (soft) - { - swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp, - visionMeasurementStdDevs.times(1.0 / trustWorthiness)); - } else - { - swerveDrivePoseEstimator.resetPosition( - robotPose.getRotation(), getModulePositions(), robotPose); - } + swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), robotPose.getRotation()); odometryLock.unlock(); @@ -876,19 +930,18 @@ public class SwerveDrive * @param robotPose Robot {@link Pose2d} as measured by vision. * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from * {@link Timer#getFPGATimestamp()} or similar sources. - * @param soft Add vision estimate using the - * {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)} function, or - * hard reset odometry with the given position with - * {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry#resetPosition(Rotation2d, - * SwerveModulePosition[], Pose2d)}. * @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the - * {@link SwerveDrivePoseEstimator}. + * {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement, + * for best accuracy calculate the standard deviation at 2 or more points and fit a + * line to it with the calculated optimal standard deviation. (Units should be meters + * per pixel). By optimizing this you can get * vision accurate to inches instead of + * feet. */ - public void addVisionMeasurement(Pose2d robotPose, double timestamp, boolean soft, + public void addVisionMeasurement(Pose2d robotPose, double timestamp, Matrix visionMeasurementStdDevs) { this.visionMeasurementStdDevs = visionMeasurementStdDevs; - addVisionMeasurement(robotPose, timestamp, soft, 1); + addVisionMeasurement(robotPose, timestamp); } @@ -913,7 +966,7 @@ public class SwerveDrive * Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to * generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply * {@link edu.wpi.first.math.filter.SlewRateLimiter} to given inputs. Important functions to look at are - * {@link SwerveController#getTargetSpeeds(double, double, double, double)}, + * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)}, * {@link SwerveController#addSlewRateLimiters(SlewRateLimiter, SlewRateLimiter, SlewRateLimiter)}, * {@link SwerveController#getRawTargetSpeeds(double, double, double)}. * @@ -959,53 +1012,4 @@ public class SwerveDrive } } - /** - * Enable second order kinematics for simulation and modifying the feedforward. Second order kinematics could increase - * accuracy in odometry. - * - * @param moduleFeedforward Module feedforward to apply should be between [-1, 1] excluding 0. - */ - public void enableSecondOrderKinematics(double moduleFeedforward) - { - for (SwerveModule module : swerveModules) - { - module.configuration.moduleSteerFFCL = moduleFeedforward; - } - } - - /** - * Enable second order kinematics with calculated values for the feedforward and return the value used. - * - * @param motorFreeSpeedRPM Steering motor free speed RPM. - * @return The feedforward value used for the last swerve module. - */ - public double enableSecondOrderKinematics(int motorFreeSpeedRPM) - { - double feedforwardValue = 0; - for (SwerveModule module : swerveModules) - { - feedforwardValue = module.configuration.moduleSteerFFCL = RobotController.getBatteryVoltage() / - (2 * Math.PI * (((double) motorFreeSpeedRPM) / - module.configuration.physicalCharacteristics.angleGearRatio) / - 60); - } - return feedforwardValue; - } - - /** - * Enable second order kinematics for tracking purposes but completely untuned. - */ - public void enableSecondOrderKinematics() - { - enableSecondOrderKinematics(-0.00000000000000001); - } - - /** - * Disable second order kinematics. - */ - public void disableSecondOrderKinematics() - { - enableSecondOrderKinematics(0); - } - } diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index da0b7bf..28744bd 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -43,6 +43,10 @@ public class SwerveModule * Feedforward for drive motor during closed loop control. */ public SimpleMotorFeedforward feedforward; + /** + * Maximum speed of the drive motors in meters per second. + */ + public double maxSpeed; /** * Last swerve module state applied. */ @@ -66,8 +70,11 @@ public class SwerveModule * * @param moduleNumber Module number for kinematics. * @param moduleConfiguration Module constants containing CAN ID's and offsets. + * @param driveFeedforward Drive motor feedforward created by + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. */ - public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration) + public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration, + SimpleMotorFeedforward driveFeedforward) { // angle = 0; // speed = 0; @@ -78,7 +85,7 @@ public class SwerveModule angleOffset = moduleConfiguration.angleOffset; // Initialize Feedforward for drive motor. - feedforward = configuration.createDriveFeedforward(); + feedforward = driveFeedforward; // Create motors from configuration and reset them to defaults. angleMotor = moduleConfiguration.angleMotor; @@ -104,14 +111,14 @@ public class SwerveModule } // Config angle motor/controller - angleMotor.configureIntegratedEncoder(moduleConfiguration.getPositionEncoderConversion(false)); + angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle); angleMotor.configurePIDF(moduleConfiguration.anglePIDF); angleMotor.configurePIDWrapping(-180, 180); angleMotor.setInverted(moduleConfiguration.angleMotorInverted); angleMotor.setMotorBrake(false); // Config drive motor/controller - driveMotor.configureIntegratedEncoder(moduleConfiguration.getPositionEncoderConversion(true)); + driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive); driveMotor.configurePIDF(moduleConfiguration.velocityPIDF); driveMotor.setInverted(moduleConfiguration.driveMotorInverted); driveMotor.setMotorBrake(true); @@ -127,6 +134,27 @@ public class SwerveModule lastState = getState(); } + /** + * Set the voltage compensation for the swerve module motor. + * + * @param optimalVoltage Nominal voltage for operation to output to. + */ + public void setAngleMotorVoltageCompensation(double optimalVoltage) + { + angleMotor.setVoltageCompensation(optimalVoltage); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param optimalVoltage Nominal voltage for operation to output to. + */ + public void setDriveMotorVoltageCompensation(double optimalVoltage) + { + driveMotor.setVoltageCompensation(optimalVoltage); + } + + /** * Queue synchronization of the integrated angle encoder with the absolute encoder. */ @@ -157,7 +185,7 @@ public class SwerveModule if (isOpenLoop) { - double percentOutput = desiredState.speedMetersPerSecond / configuration.maxSpeed; + double percentOutput = desiredState.speedMetersPerSecond / maxSpeed; driveMotor.set(percentOutput); } else { @@ -172,7 +200,7 @@ public class SwerveModule if (!force) { // Prevents module rotation if speed is less than 1% - SwerveMath.antiJitter(desiredState, lastState, Math.min(configuration.maxSpeed, 4)); + SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4)); } if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) @@ -225,12 +253,10 @@ public class SwerveModule { double velocity; Rotation2d azimuth; - double omega; if (!SwerveDriveTelemetry.isSimulation) { velocity = driveMotor.getVelocity(); azimuth = Rotation2d.fromDegrees(angleMotor.getPosition()); - omega = Math.toRadians(angleMotor.getVelocity()); } else { return simModule.getState(); @@ -310,6 +336,28 @@ public class SwerveModule driveMotor.setMotorBrake(brake); } + /** + * Set the conversion factor for the angle/azimuth motor controller. + * + * @param conversionFactor Angle motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. + */ + public void setAngleMotorConversionFactor(double conversionFactor) + { + angleMotor.configureIntegratedEncoder(conversionFactor); + } + + /** + * Set the conversion factor for the drive motor controller. + * + * @param conversionFactor Drive motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. + */ + public void setDriveMotorConversionFactor(double conversionFactor) + { + driveMotor.configureIntegratedEncoder(conversionFactor); + } + /** * Get the angle {@link SwerveMotor} for the {@link SwerveModule}. * diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java index a55d9e9..b150b16 100644 --- a/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -20,7 +20,7 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder public AbsoluteEncoder encoder; /** - * Create the {@link AbsoluteEncoder} object as a duty cycle. from the {@link CANSparkMax} motor. + * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. * * @param motor Motor to create the encoder from. * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java index e524ff3..9ec700c 100644 --- a/swervelib/math/SwerveMath.java +++ b/swervelib/math/SwerveMath.java @@ -1,5 +1,6 @@ package swervelib.math; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -65,6 +66,27 @@ public class SwerveMath : value; } + /** + * Create the drive feedforward for swerve modules. + * + * @param optimalVoltage Optimal voltage to calculate kV (voltage/max Velocity) + * @param maxSpeed Maximum velocity in meters per second to use for the feed forward, should be + * as close to physical max as possible. + * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction for kA (voltage/(cof*9.81)) + * @return Drive feedforward for drive motor on a swerve module. + */ + public static SimpleMotorFeedforward createDriveFeedforward(double optimalVoltage, double maxSpeed, + double wheelGripCoefficientOfFriction) + { + double kv = optimalVoltage / maxSpeed; + /// ^ Volt-seconds per meter (max voltage divided by max speed) + double ka = + optimalVoltage + / calculateMaxAcceleration(wheelGripCoefficientOfFriction); + /// ^ Volt-seconds^2 per meter (max voltage divided by max accel) + return new SimpleMotorFeedforward(0, kv, ka); + } + /** * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts * motor rotations to linear wheel distance and steering converts motor rotations to module azimuth. @@ -368,7 +390,7 @@ public class SwerveMath * * @param moduleState Current {@link SwerveModuleState} requested. * @param lastModuleState Previous {@link SwerveModuleState} used. - * @param maxSpeed Maximum speed of the modules, should be in {@link SwerveDriveConfiguration#maxSpeed}. + * @param maxSpeed Maximum speed of the modules. */ public static void antiJitter(SwerveModuleState moduleState, SwerveModuleState lastModuleState, double maxSpeed) { diff --git a/swervelib/motors/SwerveMotor.java b/swervelib/motors/SwerveMotor.java index c7470eb..7d585ca 100644 --- a/swervelib/motors/SwerveMotor.java +++ b/swervelib/motors/SwerveMotor.java @@ -12,11 +12,11 @@ public abstract class SwerveMotor /** * The maximum amount of times the swerve motor will attempt to configure a motor if failures occur. */ - public final int maximumRetries = 5; + public final int maximumRetries = 5; /** * Whether the swerve motor is a drive motor. */ - protected boolean isDriveMotor; + protected boolean isDriveMotor; /** * Configure the factory defaults. diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java index 53da69b..8afc6bf 100644 --- a/swervelib/motors/TalonFXSwerve.java +++ b/swervelib/motors/TalonFXSwerve.java @@ -258,7 +258,7 @@ public class TalonFXSwerve extends SwerveMotor { if (configChanged) { - motor.configAllSettings(configuration, 250); + motor.configAllSettings(configuration, 0); configChanged = false; } } @@ -367,7 +367,7 @@ public class TalonFXSwerve extends SwerveMotor if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { position = position < 0 ? (position % 360) + 360 : position; - motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 250); + motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 0); } } diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java index 60a5899..9a469fa 100644 --- a/swervelib/motors/TalonSRXSwerve.java +++ b/swervelib/motors/TalonSRXSwerve.java @@ -356,7 +356,7 @@ public class TalonSRXSwerve extends SwerveMotor { if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { - motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 250); + motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 0); } } diff --git a/swervelib/parser/SwerveControllerConfiguration.java b/swervelib/parser/SwerveControllerConfiguration.java index e1533ef..3266a45 100644 --- a/swervelib/parser/SwerveControllerConfiguration.java +++ b/swervelib/parser/SwerveControllerConfiguration.java @@ -17,10 +17,6 @@ public class SwerveControllerConfiguration */ public final double angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick. - /** - * Maximum robot speed in meters per second. - */ - public double maxSpeed; /** * Maximum angular velocity in rad/s */ @@ -29,19 +25,20 @@ public class SwerveControllerConfiguration /** * Construct the swerve controller configuration. * - * @param driveCfg Drive configuration. * @param headingPIDF Heading PIDF configuration. * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick. + * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you have + * feet per second use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. */ public SwerveControllerConfiguration( SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF, - double angleJoyStickRadiusDeadband) + double angleJoyStickRadiusDeadband, + double maxSpeedMPS) { - this.maxSpeed = driveCfg.maxSpeed; this.maxAngularVelocity = calculateMaxAngularVelocity( - driveCfg.maxSpeed, + maxSpeedMPS, Math.abs(driveCfg.moduleLocationsMeters[0].getX()), Math.abs(driveCfg.moduleLocationsMeters[0].getY())); this.headingPIDF = headingPIDF; @@ -54,9 +51,11 @@ public class SwerveControllerConfiguration * * @param driveCfg Drive configuration. * @param headingPIDF Heading PIDF configuration. + * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you have feet per second + * use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}. */ - public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF) + public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF, double maxSpeedMPS) { - this(driveCfg, headingPIDF, 0.5); + this(driveCfg, headingPIDF, 0.5, maxSpeedMPS); } } diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java index 27c568e..6e618e0 100644 --- a/swervelib/parser/SwerveDriveConfiguration.java +++ b/swervelib/parser/SwerveDriveConfiguration.java @@ -1,5 +1,6 @@ package swervelib.parser; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Translation2d; import swervelib.SwerveModule; import swervelib.imu.SwerveIMU; @@ -13,69 +14,69 @@ public class SwerveDriveConfiguration /** * Swerve Module locations. */ - public Translation2d[] moduleLocationsMeters; + public Translation2d[] moduleLocationsMeters; /** * Swerve IMU */ - public SwerveIMU imu; + public SwerveIMU imu; /** * Invert the imu measurements. */ - public boolean invertedIMU = false; - /** - * Max module speed in meters per second. - */ - public double maxSpeed, attainableMaxTranslationalSpeedMetersPerSecond, - attainableMaxRotationalVelocityRadiansPerSecond; + public boolean invertedIMU = false; /** * Number of modules on the robot. */ - public int moduleCount; + public int moduleCount; /** * Swerve Modules. */ - public SwerveModule[] modules; + public SwerveModule[] modules; + /** + * Physical characteristics of the swerve drive from physicalproperties.json. + */ + public SwerveModulePhysicalCharacteristics physicalCharacteristics; /** * Create swerve drive configuration. * - * @param moduleConfigs Module configuration. - * @param swerveIMU Swerve IMU. - * @param maxSpeed Max speed of the robot in meters per second. - * @param invertedIMU Invert the IMU. + * @param moduleConfigs Module configuration. + * @param swerveIMU Swerve IMU. + * @param invertedIMU Invert the IMU. + * @param driveFeedforward The drive motor feedforward to use for the {@link SwerveModule}. */ public SwerveDriveConfiguration( SwerveModuleConfiguration[] moduleConfigs, SwerveIMU swerveIMU, - double maxSpeed, - boolean invertedIMU) + boolean invertedIMU, + SimpleMotorFeedforward driveFeedforward, + SwerveModulePhysicalCharacteristics physicalCharacteristics) { this.moduleCount = moduleConfigs.length; this.imu = swerveIMU; - this.maxSpeed = maxSpeed; - this.attainableMaxRotationalVelocityRadiansPerSecond = 0; - this.attainableMaxTranslationalSpeedMetersPerSecond = 0; this.invertedIMU = invertedIMU; - this.modules = createModules(moduleConfigs); + this.modules = createModules(moduleConfigs, driveFeedforward); this.moduleLocationsMeters = new Translation2d[moduleConfigs.length]; for (SwerveModule module : modules) { this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation; } + this.physicalCharacteristics = physicalCharacteristics; } /** * Create modules based off of the SwerveModuleConfiguration. * - * @param swerves Swerve constants. + * @param swerves Swerve constants. + * @param driveFeedforward Drive feedforward created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. * @return Swerve Modules. */ - public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves) + public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves, SimpleMotorFeedforward driveFeedforward) { SwerveModule[] modArr = new SwerveModule[swerves.length]; for (int i = 0; i < swerves.length; i++) { - modArr[i] = new SwerveModule(i, swerves[i]); + modArr[i] = new SwerveModule(i, swerves[i], driveFeedforward); } return modArr; } diff --git a/swervelib/parser/SwerveModuleConfiguration.java b/swervelib/parser/SwerveModuleConfiguration.java index 4fd2a94..91d1fe7 100644 --- a/swervelib/parser/SwerveModuleConfiguration.java +++ b/swervelib/parser/SwerveModuleConfiguration.java @@ -1,13 +1,9 @@ package swervelib.parser; -import static swervelib.math.SwerveMath.calculateDegreesPerSteeringRotation; -import static swervelib.math.SwerveMath.calculateMaxAcceleration; -import static swervelib.math.SwerveMath.calculateMetersPerRotation; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Translation2d; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.motors.SwerveMotor; +import swervelib.parser.json.MotorConfigDouble; /** * Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}. @@ -15,6 +11,13 @@ import swervelib.motors.SwerveMotor; public class SwerveModuleConfiguration { + /** + * Conversion factor for drive motor onboard PID's and angle PID's. Use + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to calculate the + * conversion factors. + */ + public final MotorConfigDouble conversionFactors; /** * Angle offset in degrees for the Swerve Module. */ @@ -31,10 +34,6 @@ public class SwerveModuleConfiguration * State of inversion of the angle motor. */ public final boolean angleMotorInverted; - /** - * Maximum robot speed in meters per second. - */ - public double maxSpeed; /** * PIDF configuration options for the angle motor closed-loop PID controller. */ @@ -43,14 +42,6 @@ public class SwerveModuleConfiguration * PIDF configuration options for the drive motor closed-loop PID controller. */ public PIDFConfig velocityPIDF; - /** - * Angle volt-meter-per-second. - */ - public double moduleSteerFFCL; - /** - * The integrated encoder pulse per revolution. - */ - public double angleMotorEncoderPulsePerRevolution = 0; /** * Swerve module location relative to the robot. */ @@ -75,41 +66,39 @@ public class SwerveModuleConfiguration /** * Construct a configuration object for swerve modules. * - * @param driveMotor Drive {@link SwerveMotor}. - * @param angleMotor Angle {@link SwerveMotor} - * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. - * @param angleOffset Absolute angle offset to 0. - * @param absoluteEncoderInverted Absolute encoder inverted. - * @param angleMotorInverted State of inversion of the angle motor. - * @param driveMotorInverted Drive motor inverted. - * @param xMeters Module location in meters from the center horizontally. - * @param yMeters Module location in meters from center vertically. - * @param anglePIDF Angle PIDF configuration. - * @param velocityPIDF Velocity PIDF configuration. - * @param maxSpeed Maximum speed in meters per second. - * @param physicalCharacteristics Physical characteristics of the swerve module. - * @param angleMotorEncoderPulsePerRevolution The encoder pulse per revolution for the angle motor encoder. - * @param name The name for the swerve module. + * @param driveMotor Drive {@link SwerveMotor}. + * @param angleMotor Angle {@link SwerveMotor} + * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. + * @param angleOffset Absolute angle offset to 0. + * @param absoluteEncoderInverted Absolute encoder inverted. + * @param angleMotorInverted State of inversion of the angle motor. + * @param driveMotorInverted Drive motor inverted. + * @param xMeters Module location in meters from the center horizontally. + * @param yMeters Module location in meters from center vertically. + * @param anglePIDF Angle PIDF configuration. + * @param velocityPIDF Velocity PIDF configuration. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name The name for the swerve module. */ public SwerveModuleConfiguration( SwerveMotor driveMotor, SwerveMotor angleMotor, + MotorConfigDouble conversionFactors, SwerveAbsoluteEncoder absoluteEncoder, double angleOffset, double xMeters, double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, - double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics, boolean absoluteEncoderInverted, boolean driveMotorInverted, boolean angleMotorInverted, - double angleMotorEncoderPulsePerRevolution, String name) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; + this.conversionFactors = conversionFactors; this.absoluteEncoder = absoluteEncoder; this.angleOffset = angleOffset; this.absoluteEncoderInverted = absoluteEncoderInverted; @@ -118,10 +107,7 @@ public class SwerveModuleConfiguration this.moduleLocation = new Translation2d(xMeters, yMeters); this.anglePIDF = anglePIDF; this.velocityPIDF = velocityPIDF; - this.maxSpeed = maxSpeed; - this.moduleSteerFFCL = physicalCharacteristics.moduleSteerFFCL; this.physicalCharacteristics = physicalCharacteristics; - this.angleMotorEncoderPulsePerRevolution = angleMotorEncoderPulsePerRevolution; this.name = name; } @@ -131,78 +117,45 @@ public class SwerveModuleConfiguration * * @param driveMotor Drive {@link SwerveMotor}. * @param angleMotor Angle {@link SwerveMotor} + * @param conversionFactors Conversion factors for angle/azimuth motors drive factors. * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. * @param angleOffset Absolute angle offset to 0. * @param xMeters Module location in meters from the center horizontally. * @param yMeters Module location in meters from center vertically. * @param anglePIDF Angle PIDF configuration. * @param velocityPIDF Velocity PIDF configuration. - * @param maxSpeed Maximum robot speed in meters per second. * @param physicalCharacteristics Physical characteristics of the swerve module. * @param name Name for the module. */ public SwerveModuleConfiguration( SwerveMotor driveMotor, SwerveMotor angleMotor, + MotorConfigDouble conversionFactors, SwerveAbsoluteEncoder absoluteEncoder, double angleOffset, double xMeters, double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, - double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics, String name) { this( driveMotor, angleMotor, + conversionFactors, absoluteEncoder, angleOffset, xMeters, yMeters, anglePIDF, velocityPIDF, - maxSpeed, physicalCharacteristics, false, false, false, - physicalCharacteristics.angleEncoderPulsePerRotation, name); } - /** - * Create the drive feedforward for swerve modules. - * - * @return Drive feedforward for drive motor on a swerve module. - */ - public SimpleMotorFeedforward createDriveFeedforward() - { - double kv = physicalCharacteristics.optimalVoltage / maxSpeed; - /// ^ Volt-seconds per meter (max voltage divided by max speed) - double ka = - physicalCharacteristics.optimalVoltage - / calculateMaxAcceleration(physicalCharacteristics.wheelGripCoefficientOfFriction); - /// ^ Volt-seconds^2 per meter (max voltage divided by max accel) - return new SimpleMotorFeedforward(0, kv, ka); - } - /** - * Get the encoder conversion for position encoders. - * - * @param isDriveMotor For the drive motor. - * @return Position encoder conversion factor. - */ - public double getPositionEncoderConversion(boolean isDriveMotor) - { - return isDriveMotor - ? calculateMetersPerRotation( - physicalCharacteristics.wheelDiameter, - physicalCharacteristics.driveGearRatio, - physicalCharacteristics.driveEncoderPulsePerRotation) - : calculateDegreesPerSteeringRotation( - physicalCharacteristics.angleGearRatio, - angleMotorEncoderPulsePerRevolution); - } } diff --git a/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 67eace3..87e13cf 100644 --- a/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -1,35 +1,13 @@ package swervelib.parser; +import swervelib.parser.json.MotorConfigDouble; + /** * Configuration class which stores physical characteristics shared between every swerve module. */ public class SwerveModulePhysicalCharacteristics { - /** - * Wheel diameter in meters. - */ - public final double wheelDiameter; - /** - * Drive gear ratio. How many times the motor has to spin before the wheel makes a complete rotation. - */ - public final double driveGearRatio; - /** - * Angle gear ratio. How many times the motor has to spin before the wheel makes a full rotation. - */ - public final double angleGearRatio; - /** - * Drive motor encoder pulse per rotation. 1 if integrated encoder. - */ - public final int driveEncoderPulsePerRotation; - /** - * Angle motor encoder pulse per rotation. 1 for Neo encoder. 2048 for Falcons. - */ - public final int angleEncoderPulsePerRotation; - /** - * Optimal voltage of the robot. - */ - public final double optimalVoltage; /** * Current limits for the Swerve Module. */ @@ -41,21 +19,26 @@ public class SwerveModulePhysicalCharacteristics /** * Wheel grip tape coefficient of friction on carpet, as described by the vendor. */ - public final double wheelGripCoefficientOfFriction; + public final double wheelGripCoefficientOfFriction; /** - * Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that - * your drive train does not drift towards the direction you are rotating while you translate. When set to 0 the - * calculated kV will be used. + * The voltage to use for the smart motor voltage compensation. */ - public final double moduleSteerFFCL; + public double optimalVoltage; + /** + * The conversion factors for the drive and angle motors, created by + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + */ + public MotorConfigDouble conversionFactor; /** * Construct the swerve module physical characteristics. * - * @param driveGearRatio Gear ratio of the drive motor. Number of motor rotations to rotate the - * wheel. - * @param angleGearRatio Gear ratio of the angle motor. Number of motor rotations to spin the wheel. - * @param wheelDiameter Wheel diameter in meters. + * @param conversionFactors The conversion factors for the drive and angle motors, created by + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, + * double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, + * double)}. * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction on carpet given by manufacturer. * @param optimalVoltage Optimal robot voltage. * @param driveMotorCurrentLimit Current limit for the drive motor. @@ -64,39 +47,33 @@ public class SwerveModulePhysicalCharacteristics * over drawing power from battery) * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents * overdrawing power and power loss). - * @param driveEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders. - * @param angleEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders. - * @param moduleSteerFFCL The kV applied to the steering motor to ensure your drivetrain does not drift - * towards a direction when rotating while translating. */ public SwerveModulePhysicalCharacteristics( - double driveGearRatio, - double angleGearRatio, - double wheelDiameter, + MotorConfigDouble conversionFactors, double wheelGripCoefficientOfFriction, double optimalVoltage, int driveMotorCurrentLimit, int angleMotorCurrentLimit, double driveMotorRampRate, - double angleMotorRampRate, - int driveEncoderPulsePerRotation, - int angleEncoderPulsePerRotation, - double moduleSteerFFCL) + double angleMotorRampRate) { this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; this.optimalVoltage = optimalVoltage; - this.angleGearRatio = angleGearRatio; - this.driveGearRatio = driveGearRatio; - this.angleEncoderPulsePerRotation = angleEncoderPulsePerRotation; - this.driveEncoderPulsePerRotation = driveEncoderPulsePerRotation; - this.wheelDiameter = wheelDiameter; + this.conversionFactor = conversionFactors; + // Set the conversion factors to null if they are both 0. + if (conversionFactors != null) + { + if (conversionFactors.angle == 0 && conversionFactors.drive == 0) + { + this.conversionFactor = null; + } + } this.driveMotorCurrentLimit = driveMotorCurrentLimit; this.angleMotorCurrentLimit = angleMotorCurrentLimit; this.driveMotorRampRate = driveMotorRampRate; this.angleMotorRampRate = angleMotorRampRate; - this.moduleSteerFFCL = moduleSteerFFCL; } /** @@ -104,36 +81,26 @@ public class SwerveModulePhysicalCharacteristics * nitrile on carpet from Studica) and optimal voltage is 12v. Assumes the drive motor current limit is 40A, and the * angle motor current limit is 20A. * - * @param driveGearRatio Gear ratio of the drive motor. Number of motor rotations to rotate the wheel. - * @param angleGearRatio Gear ratio of the angle motor. Number of motor rotations to spin the wheel. - * @param wheelDiameter Wheel diameter in meters. - * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents over - * drawing power from battery) - * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents - * overdrawing power and power loss). - * @param driveEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders. - * @param angleEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders. + * @param conversionFactors The conversion factors for the drive and angle motors, created by + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents over drawing + * power from battery) + * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents overdrawing + * power and power loss). */ public SwerveModulePhysicalCharacteristics( - double driveGearRatio, - double angleGearRatio, - double wheelDiameter, + MotorConfigDouble conversionFactors, double driveMotorRampRate, - double angleMotorRampRate, - int driveEncoderPulsePerRotation, - int angleEncoderPulsePerRotation) + double angleMotorRampRate) { this( - driveGearRatio, - angleGearRatio, - wheelDiameter, + conversionFactors, 1.19, 12, 40, 20, driveMotorRampRate, - angleMotorRampRate, - driveEncoderPulsePerRotation, - angleEncoderPulsePerRotation, -0.30); + angleMotorRampRate); } } diff --git a/swervelib/parser/SwerveParser.java b/swervelib/parser/SwerveParser.java index 0ec95c3..573fdd8 100644 --- a/swervelib/parser/SwerveParser.java +++ b/swervelib/parser/SwerveParser.java @@ -2,12 +2,13 @@ package swervelib.parser; import com.fasterxml.jackson.databind.JsonNode; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import java.io.File; import java.io.IOException; import java.util.HashMap; import swervelib.SwerveDrive; import swervelib.SwerveModule; +import swervelib.math.SwerveMath; import swervelib.parser.json.ControllerPropertiesJson; import swervelib.parser.json.ModuleJson; import swervelib.parser.json.PIDFPropertiesJson; @@ -127,11 +128,78 @@ public class SwerveParser /** * Create {@link SwerveDrive} from JSON configuration directory. * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular acceleration used in + * {@link swervelib.SwerveController} and drive feedforward in + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. * @return {@link SwerveDrive} instance. */ - public SwerveDrive createSwerveDrive() + public SwerveDrive createSwerveDrive(double maxSpeed) + { + return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular + * acceleration used in {@link swervelib.SwerveController} and drive feedforward in + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop + * units to degrees, usually created using + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to + * meters per rotation, usually created using + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) + { + physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, + * double)}. + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration + * in {@link swervelib.SwerveController} of the robot. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop + * units to degrees, usually created using + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to + * meters per rotation, usually created using + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed, + double angleMotorConversionFactor, double driveMotorConversion) + { + physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + return createSwerveDrive(driveFeedforward, maxSpeed); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param driveFeedforward Drive feedforward to use for swerve modules, should be created using + * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in + * {@link swervelib.SwerveController} of the robot + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed) { - double maxSpeedMPS = Units.feetToMeters(swerveDriveJson.maxSpeed); SwerveModuleConfiguration[] moduleConfigurations = new SwerveModuleConfiguration[moduleJsons.length]; for (int i = 0; i < moduleConfigurations.length; i++) @@ -141,19 +209,19 @@ public class SwerveParser module.createModuleConfiguration( pidfPropertiesJson.angle, pidfPropertiesJson.drive, - maxSpeedMPS, - physicalPropertiesJson.createPhysicalProperties(swerveDriveJson.optimalVoltage), + physicalPropertiesJson.createPhysicalProperties(), swerveDriveJson.modules[i]); } SwerveDriveConfiguration swerveDriveConfiguration = new SwerveDriveConfiguration( moduleConfigurations, swerveDriveJson.imu.createIMU(), - maxSpeedMPS, - swerveDriveJson.invertedIMU); + swerveDriveJson.invertedIMU, + driveFeedforward, + physicalPropertiesJson.createPhysicalProperties()); return new SwerveDrive( swerveDriveConfiguration, - controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration)); + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed); } } diff --git a/swervelib/parser/json/ControllerPropertiesJson.java b/swervelib/parser/json/ControllerPropertiesJson.java index f3a49e8..103107d 100644 --- a/swervelib/parser/json/ControllerPropertiesJson.java +++ b/swervelib/parser/json/ControllerPropertiesJson.java @@ -23,12 +23,13 @@ public class ControllerPropertiesJson * Create the {@link SwerveControllerConfiguration} based on parsed and given data. * * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration. + * @param maxSpeedMPS Maximum speed in meters per second for the angular acceleration of the robot. * @return {@link SwerveControllerConfiguration} object based on parsed data. */ public SwerveControllerConfiguration createControllerConfiguration( - SwerveDriveConfiguration driveConfiguration) + SwerveDriveConfiguration driveConfiguration, double maxSpeedMPS) { return new SwerveControllerConfiguration( - driveConfiguration, heading, angleJoystickRadiusDeadband); + driveConfiguration, heading, angleJoystickRadiusDeadband, maxSpeedMPS); } } diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java index 0cc1428..72c79de 100644 --- a/swervelib/parser/json/DeviceJson.java +++ b/swervelib/parser/json/DeviceJson.java @@ -187,24 +187,4 @@ public class DeviceJson throw new RuntimeException( "Could not create absolute encoder from data port of " + type + " id " + id); } - - /** - * Get the encoder pulse per rotation based off of the encoder type. - * - * @param angleEncoderPulsePerRotation The configured pulse per rotation. - * @return The correct pulse per rotation based off of the encoder type. - */ - public int getPulsePerRotation(int angleEncoderPulsePerRotation) - { - switch (type) - { - case "canandcoder": - return 360; - case "falcon": - case "talonfx": - return 2048; - default: - return angleEncoderPulsePerRotation; - } - } } diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java index 17cea4a..7dcbe4c 100644 --- a/swervelib/parser/json/ModuleJson.java +++ b/swervelib/parser/json/ModuleJson.java @@ -18,42 +18,45 @@ public class ModuleJson /** * Drive motor device configuration. */ - public DeviceJson drive; + public DeviceJson drive; /** * Angle motor device configuration. */ - public DeviceJson angle; + public DeviceJson angle; + /** + * Conversion factor for the module, if different from the one in swervedrive.json + *

+ * Conversion factor applied to the motor controllers PID loops. Can be calculated with + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. + */ + public MotorConfigDouble conversionFactor; /** * Absolute encoder device configuration. */ - public DeviceJson encoder; + public DeviceJson encoder; /** * Defines which motors are inverted. */ - public BoolMotorJson inverted; + public BoolMotorJson inverted; /** * Absolute encoder offset from 0 in degrees. */ - public double absoluteEncoderOffset; + public double absoluteEncoderOffset; /** * Absolute encoder inversion state. */ - public boolean absoluteEncoderInverted = false; - /** - * The angle encoder pulse per revolution override. 1 for Neo encoder. 2048 for Falcons. - */ - public double angleEncoderPulsePerRevolution = 0; + public boolean absoluteEncoderInverted = false; /** * The location of the swerve module from the center of the robot in inches. */ - public LocationJson location; + public LocationJson location; /** * Create the swerve module configuration based off of parsed data. * * @param anglePIDF The PIDF values for the angle motor. * @param velocityPIDF The velocity PIDF values for the drive motor. - * @param maxSpeed The maximum speed of the robot in meters per second. * @param physicalCharacteristics Physical characteristics of the swerve module. * @param name Module json filename. * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data. @@ -61,21 +64,12 @@ public class ModuleJson public SwerveModuleConfiguration createModuleConfiguration( PIDFConfig anglePIDF, PIDFConfig velocityPIDF, - double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics, String name) { SwerveMotor angleMotor = angle.createMotor(false); SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); - // Override angle encoder pulse per rotation based on the encoder and motor type. - int encoderPulseOverride = encoder.getPulsePerRotation(physicalCharacteristics.angleEncoderPulsePerRotation); - int motorPulseOverride = angle.getPulsePerRotation(physicalCharacteristics.angleEncoderPulsePerRotation); - - int angleEncoderPulsePerRotation = - motorPulseOverride != physicalCharacteristics.angleEncoderPulsePerRotation ? motorPulseOverride - : encoderPulseOverride; - // If the absolute encoder is attached. if (absEncoder == null) { @@ -83,22 +77,57 @@ public class ModuleJson angleMotor.setAbsoluteEncoder(absEncoder); } + // Set the conversion factors to null if they are both 0. + if (this.conversionFactor != null) + { + if (this.conversionFactor.angle == 0 && this.conversionFactor.drive == 0) + { + this.conversionFactor = null; + } + } + + if (this.conversionFactor == null && physicalCharacteristics.conversionFactor == null) + { + throw new RuntimeException("No Conversion Factor configured! Please create SwerveDrive using \n" + + "SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + + "OR\n" + + "SwerveParser.createSwerveDrive(maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + + "OR\n" + + "Set the conversion factor in physicalproperties.json OR the module JSON file." + + "REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n"); + } else if (physicalCharacteristics.conversionFactor != null && this.conversionFactor == null) + { + this.conversionFactor = physicalCharacteristics.conversionFactor; + } else if (physicalCharacteristics.conversionFactor != + null) // If both are defined, override 0 with the physical characterstics input. + { + this.conversionFactor.angle = this.conversionFactor.angle == 0 ? physicalCharacteristics.conversionFactor.angle + : this.conversionFactor.angle; + this.conversionFactor.drive = this.conversionFactor.drive == 0 ? physicalCharacteristics.conversionFactor.drive + : this.conversionFactor.drive; + } + + if (this.conversionFactor.drive == 0 || this.conversionFactor.angle == 0) + { + throw new RuntimeException( + "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files."); + } + System.out.println(conversionFactor.drive); + return new SwerveModuleConfiguration( drive.createMotor(true), angleMotor, + conversionFactor, absEncoder, absoluteEncoderOffset, Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x), Units.inchesToMeters(Math.round(location.y) == 0 ? location.left : location.y), anglePIDF, velocityPIDF, - maxSpeed, physicalCharacteristics, absoluteEncoderInverted, inverted.drive, inverted.angle, - angleEncoderPulsePerRevolution == 0 ? angleEncoderPulsePerRotation - : angleEncoderPulsePerRevolution, name.replaceAll("\\.json", "")); } } diff --git a/swervelib/parser/json/MotorConfigDouble.java b/swervelib/parser/json/MotorConfigDouble.java new file mode 100644 index 0000000..86766b3 --- /dev/null +++ b/swervelib/parser/json/MotorConfigDouble.java @@ -0,0 +1,36 @@ +package swervelib.parser.json; + +/** + * Used to store doubles for motor configuration. + */ +public class MotorConfigDouble +{ + + /** + * Drive motor. + */ + public double drive; + /** + * Angle motor. + */ + public double angle; + + /** + * Default constructor. + */ + public MotorConfigDouble() + { + } + + /** + * Default constructor. + * + * @param angle Angle data. + * @param drive Drive data. + */ + public MotorConfigDouble(double angle, double drive) + { + this.angle = angle; + this.drive = drive; + } +} diff --git a/swervelib/parser/json/MotorConfigInt.java b/swervelib/parser/json/MotorConfigInt.java new file mode 100644 index 0000000..d31726c --- /dev/null +++ b/swervelib/parser/json/MotorConfigInt.java @@ -0,0 +1,36 @@ +package swervelib.parser.json; + +/** + * Used to store ints for motor configuration. + */ +public class MotorConfigInt +{ + + /** + * Drive motor. + */ + public int drive; + /** + * Angle motor. + */ + public int angle; + + /** + * Default constructor. + */ + public MotorConfigInt() + { + } + + /** + * Default constructor with values. + * + * @param drive Drive data. + * @param angle Angle data. + */ + public MotorConfigInt(int drive, int angle) + { + this.angle = angle; + this.drive = drive; + } +} diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java index 64fbc0e..0e6b722 100644 --- a/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/swervelib/parser/json/PhysicalPropertiesJson.java @@ -1,8 +1,6 @@ package swervelib.parser.json; -import edu.wpi.first.math.util.Units; import swervelib.parser.SwerveModulePhysicalCharacteristics; -import swervelib.telemetry.SwerveDriveTelemetry; /** * {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the SwerveModule. @@ -10,18 +8,13 @@ import swervelib.telemetry.SwerveDriveTelemetry; public class PhysicalPropertiesJson { + /** - * Wheel diameter in inches. + * Conversion factor applied to the motor controllers PID loops. Can be calculated with + * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or + * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. */ - public double wheelDiameter; - /** - * Gear ratio for the motors, number of times the motor has to spin before the wheel rotates a single time. - */ - public MotorConfigDouble gearRatio; - /** - * Encoder pulse per rotation for non-integrated encoders. 1 for integrated encoders. - */ - public MotorConfigInt encoderPulsePerRotation = new MotorConfigInt(1, 1); + public MotorConfigDouble conversionFactor; /** * The current limit in AMPs to apply to the motors. */ @@ -35,106 +28,25 @@ public class PhysicalPropertiesJson */ public double wheelGripCoefficientOfFriction = 1.19; /** - * Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that - * your drive train does not drift towards the direction you are rotating while you translate. Default value is 0. If - * robot arcs while translating and rotating negate this. + * The voltage to use for the smart motor voltage compensation, default is 12. */ - public double moduleFeedForwardClosedLoop = SwerveDriveTelemetry.isSimulation ? 0.33 : 0; - /** - * DEPRECATED: No longer needed, tune {@link PhysicalPropertiesJson#moduleFeedForwardClosedLoop} instead. - */ - public double angleMotorFreeSpeedRPM = 0; + public double optimalVoltage = 12; /** * Create the physical characteristics based off the parsed data. * - * @param optimalVoltage Optimal voltage to compensate for and use to calculate drive motor feedforward. * @return {@link SwerveModulePhysicalCharacteristics} based on parsed data. */ - public SwerveModulePhysicalCharacteristics createPhysicalProperties(double optimalVoltage) + public SwerveModulePhysicalCharacteristics createPhysicalProperties() { return new SwerveModulePhysicalCharacteristics( - gearRatio.drive, - gearRatio.angle, - Units.inchesToMeters(wheelDiameter), + conversionFactor, wheelGripCoefficientOfFriction, optimalVoltage, currentLimit.drive, currentLimit.angle, rampRate.drive, - rampRate.angle, - encoderPulsePerRotation.drive, - encoderPulsePerRotation.angle, - moduleFeedForwardClosedLoop); + rampRate.angle); } } -/** - * Used to store doubles for motor configuration. - */ -class MotorConfigDouble -{ - - /** - * Drive motor. - */ - public double drive; - /** - * Angle motor. - */ - public double angle; - - /** - * Default constructor. - */ - public MotorConfigDouble() - { - } - - /** - * Default constructor. - * - * @param angle Angle data. - * @param drive Drive data. - */ - public MotorConfigDouble(double angle, double drive) - { - this.angle = angle; - this.drive = drive; - } -} - -/** - * Used to store ints for motor configuration. - */ -class MotorConfigInt -{ - - /** - * Drive motor. - */ - public int drive; - /** - * Angle motor. - */ - public int angle; - - /** - * Default constructor. - */ - public MotorConfigInt() - { - } - - /** - * Default constructor with values. - * - * @param drive Drive data. - * @param angle Angle data. - */ - public MotorConfigInt(int drive, int angle) - { - this.angle = angle; - this.drive = drive; - } -} diff --git a/swervelib/parser/json/SwerveDriveJson.java b/swervelib/parser/json/SwerveDriveJson.java index b505d77..332d30d 100644 --- a/swervelib/parser/json/SwerveDriveJson.java +++ b/swervelib/parser/json/SwerveDriveJson.java @@ -6,14 +6,6 @@ package swervelib.parser.json; public class SwerveDriveJson { - /** - * Maximum robot speed in feet per second. - */ - public double maxSpeed; - /** - * Optimal voltage to compensate to and base feedforward calculations off of. - */ - public double optimalVoltage; /** * Robot IMU used to determine heading of the robot. */