From dd28a657b2c9fae2337217cfde712a3c6211f470 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 20 Feb 2023 20:59:31 -0600 Subject: [PATCH] Addressing issue #7 by reading CANCoder values until successful with 10ms delay between readings. Fall back to reading relative encoder. --- swervelib/SwerveController.java | 26 +++- swervelib/SwerveDrive.java | 144 ++++++++++-------- swervelib/SwerveModule.java | 59 ++++--- swervelib/encoders/CANCoderSwerve.java | 48 +++++- swervelib/encoders/SwerveAbsoluteEncoder.java | 5 + swervelib/encoders/package-info.java | 2 +- swervelib/imu/ADIS16448Swerve.java | 1 - swervelib/imu/ADXRS450Swerve.java | 1 - swervelib/imu/AnalogGyroSwerve.java | 3 +- swervelib/imu/Pigeon2Swerve.java | 1 - swervelib/imu/package-info.java | 2 +- swervelib/math/SwerveKinematics2.java | 42 ++--- swervelib/math/SwerveMath.java | 127 +++++++++------ swervelib/math/SwerveModuleState2.java | 3 +- swervelib/math/package-info.java | 2 +- swervelib/motors/SparkMaxSwerve.java | 26 +++- swervelib/motors/TalonFXSwerve.java | 34 ++--- swervelib/motors/TalonSRXSwerve.java | 29 ++-- swervelib/motors/package-info.java | 2 +- swervelib/package-info.java | 2 +- swervelib/parser/PIDFConfig.java | 1 - .../parser/SwerveControllerConfiguration.java | 18 ++- .../parser/SwerveDriveConfiguration.java | 7 +- .../parser/SwerveModuleConfiguration.java | 97 +++++++----- .../SwerveModulePhysicalCharacteristics.java | 48 ++++-- swervelib/parser/SwerveParser.java | 58 ++++--- .../parser/deserializer/package-info.java | 2 +- .../parser/json/ControllerPropertiesJson.java | 6 +- swervelib/parser/json/DeviceJson.java | 4 +- swervelib/parser/json/ModuleJson.java | 27 +++- .../parser/json/PhysicalPropertiesJson.java | 22 ++- .../parser/json/modules/LocationJson.java | 1 - .../parser/json/modules/package-info.java | 2 +- swervelib/parser/json/package-info.java | 2 +- swervelib/parser/package-info.java | 2 +- swervelib/simulation/SwerveIMUSimulation.java | 7 +- .../simulation/SwerveModuleSimulation.java | 3 +- swervelib/simulation/ctre/PhysicsSim.java | 25 ++- .../simulation/ctre/TalonFXSimProfile.java | 16 +- .../simulation/ctre/TalonSRXSimProfile.java | 14 +- .../simulation/ctre/VictorSPXSimProfile.java | 8 +- swervelib/simulation/ctre/package-info.java | 2 +- swervelib/simulation/package-info.java | 2 +- 43 files changed, 570 insertions(+), 363 deletions(-) diff --git a/swervelib/SwerveController.java b/swervelib/SwerveController.java index b4b183f..d9309e5 100644 --- a/swervelib/SwerveController.java +++ b/swervelib/SwerveController.java @@ -72,11 +72,14 @@ public class SwerveController * @param currentHeadingAngleRadians The current robot heading in radians. * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double angle, double currentHeadingAngleRadians) + public ChassisSpeeds getTargetSpeeds( + double xInput, double yInput, double angle, double currentHeadingAngleRadians) { - // Calculates an angular rate using a PIDController and the commanded angle. This is then scaled by + // Calculates an angular rate using a PIDController and the commanded angle. This is then + // scaled by // the drivebase's maximum angular velocity. - double omega = thetaController.calculate(currentHeadingAngleRadians, angle) * config.maxAngularVelocity; + double omega = + thetaController.calculate(currentHeadingAngleRadians, angle) * config.maxAngularVelocity; // 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 = Math.pow(xInput, 3) * config.maxSpeed; @@ -96,13 +99,20 @@ public class SwerveController * @param currentHeadingAngleRadians The current robot heading in radians. * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive. */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY, - double currentHeadingAngleRadians) + public ChassisSpeeds getTargetSpeeds( + double xInput, + double yInput, + double headingX, + double headingY, + double currentHeadingAngleRadians) { - // Converts the horizontal and vertical components to the commanded angle, in radians, unless the joystick is near - // the center (i. e. has been released), in which case the angle is held at the last valid joystick input (hold + // Converts the horizontal and vertical components to the commanded angle, in radians, unless + // the joystick is near + // the center (i. e. has been released), in which case the angle is held at the last valid + // joystick input (hold // position when stick released). - double angle = withinHypotDeadband(headingX, headingY) ? lastAngle : Math.atan2(headingX, headingY); + double angle = + withinHypotDeadband(headingX, headingY) ? lastAngle : Math.atan2(headingX, headingY); ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians); // Used for the position hold feature diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index 959ae54..7e7e3a8 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -11,10 +11,10 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Robot; import java.util.ArrayList; import java.util.List; import swervelib.imu.SwerveIMU; @@ -68,8 +68,8 @@ public class SwerveDrive private int moduleSynchronizationCounter = 0; /** - * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the - * {@link SwerveDrive#setModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics— it takes + * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the + * {@link SwerveDrive#setModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics-- it takes * a translation and rotation, as well as parameters for field-centric and closed-loop velocity control. * {@link SwerveDrive#setModuleStates} takes a list of SwerveModuleStates and directly passes them to the modules. * This subsystem also handles odometry. @@ -78,7 +78,8 @@ public class SwerveDrive * @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the * {@link SwerveController}. */ - public SwerveDrive(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig) + public SwerveDrive( + SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig) { swerveDriveConfiguration = config; swerveController = new SwerveController(controllerConfig); @@ -87,7 +88,7 @@ public class SwerveDrive // Create an integrator for angle if the robot is being simulated to emulate an IMU // If the robot is real, instantiate the IMU instead. - if (!Robot.isReal()) + if (RobotBase.isSimulation()) { simIMU = new SwerveIMUSimulation(); } else @@ -98,44 +99,48 @@ public class SwerveDrive this.swerveModules = config.modules; -// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions()); - swerveDrivePoseEstimator = new SwerveDrivePoseEstimator( - kinematics, - getYaw(), - getModulePositions(), - new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), - VecBuilder.fill(0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight - VecBuilder.fill(0.9, 0.9, 0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight + // odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions()); + swerveDrivePoseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + getYaw(), + getModulePositions(), + new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)), + VecBuilder.fill( + 0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight + VecBuilder.fill( + 0.9, 0.9, + 0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight zeroGyro(); SmartDashboard.putData("Field", field); } /** - * 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. + * 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. * * @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 + * 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 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) + 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); + // 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); // Display commanded speed for testing SmartDashboard.putString("RobotVelocity", velocity.toString()); @@ -147,10 +152,10 @@ public class SwerveDrive } /** - * Set the module states (azimuth and velocity) directly. Used primarily for auto pathing. + * Set the module states (azimuth and velocity) directly. Used primarily for auto pathing. * * @param desiredStates A list of SwerveModuleStates to send to the modules. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. */ public void setModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop) { @@ -161,10 +166,12 @@ public class SwerveDrive for (SwerveModule module : swerveModules) { module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop); - SmartDashboard.putNumber("Module " + module.moduleNumber + " Speed Setpoint: ", - desiredStates[module.moduleNumber].speedMetersPerSecond); - SmartDashboard.putNumber("Module " + module.moduleNumber + " Angle Setpoint: ", - desiredStates[module.moduleNumber].angle.getDegrees()); + SmartDashboard.putNumber( + "Module " + module.moduleNumber + " Speed Setpoint: ", + desiredStates[module.moduleNumber].speedMetersPerSecond); + SmartDashboard.putNumber( + "Module " + module.moduleNumber + " Angle Setpoint: ", + desiredStates[module.moduleNumber].angle.getDegrees()); } } @@ -175,8 +182,7 @@ public class SwerveDrive */ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), - false); + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false); } /** @@ -197,9 +203,11 @@ public class SwerveDrive public ChassisSpeeds getFieldVelocity() { // ChassisSpeeds has a method to convert from field-relative to robot-relative speeds, - // but not the reverse. However, because this transform is a simple rotation, negating the angle + // but not the reverse. However, because this transform is a simple rotation, negating the + // angle // given as the robot angle reverses the direction of rotation, and the conversion is reversed. - return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus()); + return ChassisSpeeds.fromFieldRelativeSpeeds( + kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus()); } /** @@ -212,10 +220,9 @@ public class SwerveDrive return kinematics.toChassisSpeeds(getStates()); } - /** * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this - * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to + * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to * keep working. * * @param pose The pose to set the odometry to @@ -257,7 +264,8 @@ public class SwerveDrive */ public SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] positions = new SwerveModulePosition[swerveDriveConfiguration.moduleCount]; + SwerveModulePosition[] positions = + new SwerveModulePosition[swerveDriveConfiguration.moduleCount]; for (SwerveModule module : swerveModules) { positions[module.moduleNumber] = module.getPosition(); @@ -270,8 +278,9 @@ public class SwerveDrive */ public void zeroGyro() { - // Resets the real gyro or the angle accumulator, depending on whether the robot is being simulated - if (Robot.isReal()) + // Resets the real gyro or the angle accumulator, depending on whether the robot is being + // simulated + if (!RobotBase.isSimulation()) { imu.setYaw(0); } else @@ -283,14 +292,14 @@ public class SwerveDrive } /** - * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped. + * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped. * * @return The yaw as a {@link Rotation2d} angle */ public Rotation2d getYaw() { // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (Robot.isReal()) + if (!RobotBase.isSimulation()) { double[] ypr = new double[3]; imu.getYawPitchRoll(ypr); @@ -309,7 +318,7 @@ public class SwerveDrive public Rotation2d getPitch() { // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (Robot.isReal()) + if (!RobotBase.isSimulation()) { double[] ypr = new double[3]; imu.getYawPitchRoll(ypr); @@ -328,7 +337,7 @@ public class SwerveDrive public Rotation2d getRoll() { // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (Robot.isReal()) + if (!RobotBase.isSimulation()) { double[] ypr = new double[3]; imu.getYawPitchRoll(ypr); @@ -347,7 +356,7 @@ public class SwerveDrive public Rotation3d getGyroRotation3d() { // Read the imu if the robot is real or the accumulator if the robot is simulated. - if (Robot.isReal()) + if (!RobotBase.isSimulation()) { double[] ypr = new double[3]; imu.getYawPitchRoll(ypr); @@ -382,9 +391,8 @@ public class SwerveDrive { for (SwerveModule swerveModule : swerveModules) { - swerveModule.setDesiredState(new SwerveModuleState2(0, - swerveModule.configuration.moduleLocation.getAngle(), - 0), true); + swerveModule.setDesiredState( + new SwerveModuleState2(0, swerveModule.configuration.moduleLocation.getAngle(), 0), true); } } @@ -400,7 +408,9 @@ public class SwerveDrive List poses = new ArrayList<>(); for (SwerveModule module : swerveModules) { - poses.add(robotPose.plus(new Transform2d(module.configuration.moduleLocation, module.getState().angle))); + poses.add( + robotPose.plus( + new Transform2d(module.configuration.moduleLocation, module.getState().angle))); } return poses.toArray(poseArr); } @@ -429,10 +439,13 @@ public class SwerveDrive swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); // Update angle accumulator if the robot is simulated - if (!Robot.isReal()) + if (RobotBase.isSimulation()) { - simIMU.updateOdometry(kinematics, getStates(), - getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()), field); + simIMU.updateOdometry( + kinematics, + getStates(), + getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()), + field); } field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); @@ -446,19 +459,21 @@ public class SwerveDrive moduleStates[module.moduleNumber + 1] = moduleState.speedMetersPerSecond; sumOmega += Math.abs(moduleState.omegaRadPerSecond); - SmartDashboard.putNumber("Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition()); - SmartDashboard.putNumber("Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition()); + SmartDashboard.putNumber( + "Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition()); + SmartDashboard.putNumber( + "Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition()); } SmartDashboard.putNumberArray("moduleStates", moduleStates); - // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS lib) + // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS + // lib) // To ensure that everytime we initialize it works. if (sumOmega <= .01 && ++moduleSynchronizationCounter > 5) { synchronizeModuleEncoders(); moduleSynchronizationCounter = 0; } - } /** @@ -492,10 +507,11 @@ public class SwerveDrive swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); } else { - swerveDrivePoseEstimator.resetPosition(robotPose.getRotation(), getModulePositions(), robotPose); + swerveDrivePoseEstimator.resetPosition( + robotPose.getRotation(), getModulePositions(), robotPose); } - if (Robot.isReal()) + if (!RobotBase.isSimulation()) { imu.setYaw(swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getDegrees()); // Yaw reset recommended by Team 1622 diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index 8daf3d6..fba0122 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -4,8 +4,8 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Robot; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveModuleState2; import swervelib.motors.SwerveMotor; @@ -59,10 +59,10 @@ public class SwerveModule */ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration) { -// angle = 0; -// speed = 0; -// omega = 0; -// fakePos = 0; + // angle = 0; + // speed = 0; + // omega = 0; + // fakePos = 0; this.moduleNumber = moduleNumber; configuration = moduleConfiguration; angleOffset = moduleConfiguration.angleOffset; @@ -90,7 +90,8 @@ public class SwerveModule { absoluteEncoder.factoryDefault(); absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted); - angleMotor.configureIntegratedEncoder(moduleConfiguration.getPositionEncoderConversion(false)); + angleMotor.configureIntegratedEncoder( + moduleConfiguration.getPositionEncoderConversion(false)); angleMotor.setPosition(absoluteEncoder.getAbsolutePosition() - angleOffset); } @@ -109,7 +110,7 @@ public class SwerveModule driveMotor.burnFlash(); angleMotor.burnFlash(); - if (!Robot.isReal()) + if (RobotBase.isSimulation()) { simModule = new SwerveModuleSimulation(); } @@ -124,7 +125,7 @@ public class SwerveModule { if (absoluteEncoder != null) { - angleMotor.setPosition(absoluteEncoder.getAbsolutePosition() - angleOffset); + angleMotor.setPosition(getAbsolutePosition() - angleOffset); } } @@ -136,14 +137,19 @@ public class SwerveModule */ public void setDesiredState(SwerveModuleState2 desiredState, boolean isOpenLoop) { - SwerveModuleState simpleState = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); + SwerveModuleState simpleState = + new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); simpleState = SwerveModuleState.optimize(simpleState, getState().angle); - desiredState = new SwerveModuleState2(simpleState.speedMetersPerSecond, simpleState.angle, - desiredState.omegaRadPerSecond); + desiredState = + new SwerveModuleState2( + simpleState.speedMetersPerSecond, simpleState.angle, desiredState.omegaRadPerSecond); - SmartDashboard.putNumber("Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond); - SmartDashboard.putNumber("Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees()); - SmartDashboard.putNumber("Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond)); + SmartDashboard.putNumber( + "Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond); + SmartDashboard.putNumber( + "Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees()); + SmartDashboard.putNumber( + "Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond)); if (isOpenLoop) { @@ -156,17 +162,18 @@ public class SwerveModule } // Prevents module rotation if speed is less than 1% - double angle = (Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) ? - lastAngle : - desiredState.angle.getDegrees()); - angleMotor.setReference(angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV); + double angle = + (Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) + ? lastAngle + : desiredState.angle.getDegrees()); + angleMotor.setReference( + angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV); lastAngle = angle; - if (!Robot.isReal()) + if (RobotBase.isSimulation()) { simModule.updateStateAndPosition(desiredState); } - } /** @@ -190,7 +197,7 @@ public class SwerveModule double velocity; Rotation2d azimuth; double omega; - if (Robot.isReal()) + if (!RobotBase.isSimulation()) { velocity = driveMotor.getVelocity(); azimuth = Rotation2d.fromDegrees(angleMotor.getPosition()); @@ -211,7 +218,7 @@ public class SwerveModule { double position; Rotation2d azimuth; - if (Robot.isReal()) + if (!RobotBase.isSimulation()) { position = driveMotor.getPosition(); azimuth = Rotation2d.fromDegrees(angleMotor.getPosition()); @@ -230,7 +237,13 @@ public class SwerveModule */ public double getAbsolutePosition() { - return absoluteEncoder.getAbsolutePosition(); + double angle = absoluteEncoder.getAbsolutePosition(); + if (absoluteEncoder.readingError) + { + angle = getRelativePosition(); + } + + return angle; } /** diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java index ed16754..23e937a 100644 --- a/swervelib/encoders/CANCoderSwerve.java +++ b/swervelib/encoders/CANCoderSwerve.java @@ -1,5 +1,6 @@ package swervelib.encoders; +import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoderConfiguration; import com.ctre.phoenix.sensors.MagnetFieldStrength; @@ -69,24 +70,61 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); canCoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; canCoderConfiguration.sensorDirection = inverted; - canCoderConfiguration.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition; + canCoderConfiguration.initializationStrategy = + SensorInitializationStrategy.BootToAbsolutePosition; canCoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond; encoder.configAllSettings(canCoderConfiguration); } /** - * Get the absolute position of the encoder. + * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on erroneous readings. * * @return Absolute position in degrees from [0, 360). */ @Override public double getAbsolutePosition() { - if (encoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED) + readingError = false; + MagnetFieldStrength strength = encoder.getMagnetFieldStrength(); + + if (strength != MagnetFieldStrength.Good_GreenLED) { - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); + DriverStation.reportWarning( + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); } - return encoder.getAbsolutePosition(); + if (strength == MagnetFieldStrength.Invalid_Unknown || strength == MagnetFieldStrength.BadRange_RedLED) + { + readingError = true; + return 0; + } + double angle = encoder.getAbsolutePosition(); + + // Taken from democat's library. + // Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 + ErrorCode code = encoder.getLastError(); + int ATTEMPTS = 3; + for (int i = 0; i < ATTEMPTS; i++) + { + if (code == ErrorCode.OK) + { + break; + } + try + { + Thread.sleep(10); + } catch (InterruptedException e) + { + } + angle = encoder.getAbsolutePosition(); + code = encoder.getLastError(); + } + if (code != ErrorCode.OK) + { + readingError = true; + DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false); + } + + return angle; } /** diff --git a/swervelib/encoders/SwerveAbsoluteEncoder.java b/swervelib/encoders/SwerveAbsoluteEncoder.java index 7091782..bec2bf0 100644 --- a/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -6,6 +6,11 @@ package swervelib.encoders; public abstract class SwerveAbsoluteEncoder { + /** + * Last angle reading was faulty. + */ + public boolean readingError = false; + /** * Reset the encoder to factory defaults. */ diff --git a/swervelib/encoders/package-info.java b/swervelib/encoders/package-info.java index 9535788..ab96808 100644 --- a/swervelib/encoders/package-info.java +++ b/swervelib/encoders/package-info.java @@ -1,4 +1,4 @@ /** * Absolute encoders for the swerve drive, all implement {@link swervelib.encoders.SwerveAbsoluteEncoder}. */ -package swervelib.encoders; \ No newline at end of file +package swervelib.encoders; diff --git a/swervelib/imu/ADIS16448Swerve.java b/swervelib/imu/ADIS16448Swerve.java index 7f722b0..6313ea8 100644 --- a/swervelib/imu/ADIS16448Swerve.java +++ b/swervelib/imu/ADIS16448Swerve.java @@ -81,4 +81,3 @@ public class ADIS16448Swerve extends SwerveIMU return imu; } } - diff --git a/swervelib/imu/ADXRS450Swerve.java b/swervelib/imu/ADXRS450Swerve.java index e32ba7c..9ce3cb9 100644 --- a/swervelib/imu/ADXRS450Swerve.java +++ b/swervelib/imu/ADXRS450Swerve.java @@ -80,5 +80,4 @@ public class ADXRS450Swerve extends SwerveIMU { return imu; } - } diff --git a/swervelib/imu/AnalogGyroSwerve.java b/swervelib/imu/AnalogGyroSwerve.java index 3b5cdc9..15a63d4 100644 --- a/swervelib/imu/AnalogGyroSwerve.java +++ b/swervelib/imu/AnalogGyroSwerve.java @@ -27,7 +27,8 @@ public class AnalogGyroSwerve extends SwerveIMU { if (!(channel == 0 || channel == 1)) { - throw new RuntimeException("Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n"); + throw new RuntimeException( + "Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n"); } gyro = new AnalogGyro(channel); SmartDashboard.putData(gyro); diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java index c2dd63f..e03701c 100644 --- a/swervelib/imu/Pigeon2Swerve.java +++ b/swervelib/imu/Pigeon2Swerve.java @@ -15,7 +15,6 @@ public class Pigeon2Swerve extends SwerveIMU */ WPI_Pigeon2 imu; - /** * Generate the SwerveIMU for pigeon. * diff --git a/swervelib/imu/package-info.java b/swervelib/imu/package-info.java index 40e1ea1..2b02abc 100644 --- a/swervelib/imu/package-info.java +++ b/swervelib/imu/package-info.java @@ -1,4 +1,4 @@ /** * IMUs used for controlling the robot heading. All implement {@link swervelib.imu.SwerveIMU}. */ -package swervelib.imu; \ No newline at end of file +package swervelib.imu; diff --git a/swervelib/math/SwerveKinematics2.java b/swervelib/math/SwerveKinematics2.java index 8179af0..c0ea077 100644 --- a/swervelib/math/SwerveKinematics2.java +++ b/swervelib/math/SwerveKinematics2.java @@ -76,8 +76,10 @@ public class SwerveKinematics2 extends SwerveDriveKinematics { m_inverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY()); m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX()); - bigInverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY()); - bigInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX()); + bigInverseKinematics.setRow( + i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY()); + bigInverseKinematics.setRow( + i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX()); } m_forwardKinematics = m_inverseKinematics.pseudoInverse(); @@ -197,11 +199,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics for (int i = 0; i < m_numModules; i++) { m_inverseKinematics.setRow( - i * 2, - 0, /* Start Data */ - 1, - 0, - -m_modules[i].getY() + centerOfRotationMeters.getY()); + i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotationMeters.getY()); m_inverseKinematics.setRow( i * 2 + 1, 0, /* Start Data */ @@ -237,14 +235,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics var moduleVelocityStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); var accelerationVector = new SimpleMatrix(4, 1); - accelerationVector.setColumn( - 0, - 0, - 0, - 0, - Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2), - 0 - ); + accelerationVector.setColumn(0, 0, 0, 0, Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2), 0); var moduleAccelerationStatesMatrix = bigInverseKinematics.mult(accelerationVector); @@ -260,26 +251,11 @@ public class SwerveKinematics2 extends SwerveDriveKinematics Rotation2d angle = new Rotation2d(x, y); var trigThetaAngle = new SimpleMatrix(2, 2); - trigThetaAngle.setColumn( - 0, - 0, - angle.getCos(), - -angle.getSin() - ); - trigThetaAngle.setColumn( - 1, - 0, - angle.getSin(), - angle.getCos() - ); + trigThetaAngle.setColumn(0, 0, angle.getCos(), -angle.getSin()); + trigThetaAngle.setColumn(1, 0, angle.getSin(), angle.getCos()); var accelVector = new SimpleMatrix(2, 1); - accelVector.setColumn( - 0, - 0, - ax, - ay - ); + accelVector.setColumn(0, 0, ax, ay); var omegaVector = trigThetaAngle.mult(accelVector); diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java index 79533df..27e080e 100644 --- a/swervelib/math/SwerveMath.java +++ b/swervelib/math/SwerveMath.java @@ -19,14 +19,15 @@ public class SwerveMath /** * Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds / - * degree <=> (maxVolts) / (maxSpeed) + * degree == (maxVolts) / (maxSpeed) * * @param optimalVoltage Optimal voltage to use when calculating the angle kV. * @param motorFreeSpeedRPM Motor free speed in Rotations per Minute. * @param angleGearRatio Angle gear ratio, the amount of times the motor as to turn for the wheel rotation. * @return angle kV for feedforward. */ - public static double calculateAngleKV(double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio) + public static double calculateAngleKV( + double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio) { double maxAngularVelocity = 360 * (motorFreeSpeedRPM / angleGearRatio) / 60; // deg/s return optimalVoltage / maxAngularVelocity; @@ -41,7 +42,8 @@ public class SwerveMath * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated encoder. * @return Meters per rotation for the drive motor. */ - public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio, double pulsePerRotation) + public static double calculateMetersPerRotation( + double wheelDiameter, double driveGearRatio, double pulsePerRotation) { return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation); } @@ -69,18 +71,21 @@ public class SwerveMath public static double applyDeadband(double value, boolean scaled, double deadband) { value = Math.abs(value) > deadband ? value : 0; - return scaled ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) : value; + return scaled + ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) + : value; } /** - * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts + * 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. * * @param angleGearRatio The gear ratio of the steering motor. * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if integrated. * @return Degrees per steering rotation for the angle motor. */ - public static double calculateDegreesPerSteeringRotation(double angleGearRatio, double pulsePerRotation) + public static double calculateDegreesPerSteeringRotation( + double angleGearRatio, double pulsePerRotation) { return 360 / (angleGearRatio * pulsePerRotation); } @@ -93,7 +98,8 @@ public class SwerveMath * @param furthestModuleY Y of the furthest module in meters. * @return Maximum angular velocity in rad/s. */ - public static double calculateMaxAngularVelocity(double maxSpeed, double furthestModuleX, double furthestModuleY) + public static double calculateMaxAngularVelocity( + double maxSpeed, double furthestModuleX, double furthestModuleY) { return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY); } @@ -119,8 +125,12 @@ public class SwerveMath * @param robotMass Mass of the robot in kg. * @return Theoretical maximum acceleration in m/s/s. */ - public static double calculateMaxAcceleration(double stallTorqueNm, double gearRatio, double moduleCount, - double wheelDiameter, double robotMass) + public static double calculateMaxAcceleration( + double stallTorqueNm, + double gearRatio, + double moduleCount, + double wheelDiameter, + double robotMass) { return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass); } @@ -137,50 +147,60 @@ public class SwerveMath * @param config The swerve drive configuration. * @return Maximum acceleration allowed in the robot direction. */ - private static double calcMaxAccel(Rotation2d angle, double chassisMass, double robotMass, - Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config) + private static double calcMaxAccel( + Rotation2d angle, + double chassisMass, + double robotMass, + Translation3d chassisCenterOfGravity, + SwerveDriveConfiguration config) { double xMoment = (chassisCenterOfGravity.getX() * chassisMass); double yMoment = (chassisCenterOfGravity.getY() * chassisMass); - // Calculate the vertical mass moment using the floor as the datum. This will be used later to calculate max + // Calculate the vertical mass moment using the floor as the datum. This will be used later to + // calculate max // acceleration double zMoment = (chassisCenterOfGravity.getZ() * (chassisMass)); Translation3d robotCG = new Translation3d(xMoment, yMoment, zMoment).div(robotMass); Translation2d horizontalCG = robotCG.toTranslation2d(); - Translation2d projectedHorizontalCg = new Translation2d( - (angle.getSin() * angle.getCos() * horizontalCG.getY()) + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()), - (angle.getSin() * angle.getCos() * horizontalCG.getX()) + (Math.pow(angle.getSin(), 2) * horizontalCG.getY()) - ); + Translation2d projectedHorizontalCg = + new Translation2d( + (angle.getSin() * angle.getCos() * horizontalCG.getY()) + + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()), + (angle.getSin() * angle.getCos() * horizontalCG.getX()) + + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())); - // Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is rectangular. - // Because a line is being projected, rather than a point, one of the coordinates of the projected point is + // Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is + // rectangular. + // Because a line is being projected, rather than a point, one of the coordinates of the + // projected point is // already known. Translation2d projectedWheelbaseEdge; double angDeg = angle.getDegrees(); if (angDeg <= 45 && angDeg >= -45) { SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); - projectedWheelbaseEdge = new Translation2d(conf.moduleLocation.getX(), - conf.moduleLocation.getX() * angle.getTan()); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); } else if (135 >= angDeg && angDeg > 45) { SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); - projectedWheelbaseEdge = new Translation2d( - conf.moduleLocation.getY() / angle.getTan(), - conf.moduleLocation.getY()); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); } else if (-135 <= angDeg && angDeg < -45) { SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false); - projectedWheelbaseEdge = new Translation2d( - conf.moduleLocation.getY() / angle.getTan(), - conf.moduleLocation.getY()); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); } else { SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true); - projectedWheelbaseEdge = new Translation2d( - conf.moduleLocation.getX(), - conf.moduleLocation.getX() * angle.getTan()); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); } double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm(); @@ -192,7 +212,7 @@ public class SwerveMath /** * Limits a commanded velocity to prevent exceeding the maximum acceleration given by - * {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}. Note that + * {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}. Note that * this takes and returns field-relative velocities. * * @param commandedVelocity The desired velocity @@ -204,12 +224,18 @@ public class SwerveMath * @param robotMass The weight of the robot in kg. (Including manipulators, etc). * @param chassisCenterOfGravity Chassis center of gravity. * @param config The swerve drive configuration. - * @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable + * @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable * velocity. */ - public static Translation2d limitVelocity(Translation2d commandedVelocity, ChassisSpeeds fieldVelocity, - Pose2d robotPose, double loopTime, double chassisMass, double robotMass, - Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config) + public static Translation2d limitVelocity( + Translation2d commandedVelocity, + ChassisSpeeds fieldVelocity, + Pose2d robotPose, + double loopTime, + double chassisMass, + double robotMass, + Translation3d chassisCenterOfGravity, + SwerveDriveConfiguration config) { // Get the robot's current field-relative velocity Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity); @@ -222,12 +248,18 @@ public class SwerveMath // Creates an acceleration vector with the direction of delta V and a magnitude // of the maximum allowed acceleration in that direction - Translation2d maxAccel = new Translation2d( - calcMaxAccel(deltaV - // Rotates the velocity vector to convert from field-relative to robot-relative - .rotateBy(robotPose.getRotation().unaryMinus()) - .getAngle(), chassisMass, robotMass, chassisCenterOfGravity, config), - deltaV.getAngle()); + Translation2d maxAccel = + new Translation2d( + calcMaxAccel( + deltaV + // Rotates the velocity vector to convert from field-relative to robot-relative + .rotateBy(robotPose.getRotation().unaryMinus()) + .getAngle(), + chassisMass, + robotMass, + chassisCenterOfGravity, + config), + deltaV.getAngle()); // Calculate the maximum achievable velocity by the next loop cycle. // delta V = Vf - Vi = at @@ -251,17 +283,22 @@ public class SwerveMath * @param left True = furthest left, False = furthest right. * @return Module location which is the furthest from center and abides by parameters. */ - public static SwerveModuleConfiguration getSwerveModule(SwerveModule[] modules, boolean front, boolean left) + public static SwerveModuleConfiguration getSwerveModule( + SwerveModule[] modules, boolean front, boolean left) { Translation2d target = modules[0].configuration.moduleLocation, current, temp; SwerveModuleConfiguration configuration = modules[0].configuration; for (SwerveModule module : modules) { current = module.configuration.moduleLocation; - temp = front ? (target.getY() >= current.getY() ? current : target) - : (target.getY() <= current.getY() ? current : target); - target = left ? (target.getX() >= temp.getX() ? temp : target) - : (target.getX() <= temp.getX() ? temp : target); + temp = + front + ? (target.getY() >= current.getY() ? current : target) + : (target.getY() <= current.getY() ? current : target); + target = + left + ? (target.getX() >= temp.getX() ? temp : target) + : (target.getX() <= temp.getX() ? temp : target); configuration = current.equals(target) ? module.configuration : configuration; } return configuration; diff --git a/swervelib/math/SwerveModuleState2.java b/swervelib/math/SwerveModuleState2.java index 5e8d475..2f88e9d 100644 --- a/swervelib/math/SwerveModuleState2.java +++ b/swervelib/math/SwerveModuleState2.java @@ -36,7 +36,8 @@ public class SwerveModuleState2 extends SwerveModuleState * @param angle The angle of the module. * @param omegaRadPerSecond The angular velocity of the module. */ - public SwerveModuleState2(double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond) + public SwerveModuleState2( + double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond) { this.speedMetersPerSecond = speedMetersPerSecond; this.angle = angle; diff --git a/swervelib/math/package-info.java b/swervelib/math/package-info.java index 2aa3099..d95b8e0 100644 --- a/swervelib/math/package-info.java +++ b/swervelib/math/package-info.java @@ -1,4 +1,4 @@ /** * Mathematics for swerve drives. */ -package swervelib.math; \ No newline at end of file +package swervelib.math; diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java index 9906732..d7ffd67 100644 --- a/swervelib/motors/SparkMaxSwerve.java +++ b/swervelib/motors/SparkMaxSwerve.java @@ -38,7 +38,6 @@ public class SparkMaxSwerve extends SwerveMotor */ private boolean factoryDefaultOccurred = false; - /** * Initialize the swerve motor. * @@ -54,7 +53,8 @@ public class SparkMaxSwerve extends SwerveMotor encoder = motor.getEncoder(); pid = motor.getPIDController(); - pid.setFeedbackDevice(encoder); // Configure feedback of the PID controller as the integrated encoder. + pid.setFeedbackDevice( + encoder); // Configure feedback of the PID controller as the integrated encoder. motor.setCANTimeout(0); // Spin off configurations in a different thread. } @@ -179,12 +179,17 @@ public class SparkMaxSwerve extends SwerveMotor encoder.setPositionConversionFactor(positionConversionFactor); encoder.setVelocityConversionFactor(positionConversionFactor / 60); - // Taken from https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 configureCANStatusFrames(10, 20, 20, 500, 500); } else { absoluteEncoder.setPositionConversionFactor(positionConversionFactor); absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60); + if (!isAttachedAbsoluteEncoder()) + { + configureCANStatusFrames(10, 20, 20, 500, 500); + } } } @@ -196,7 +201,8 @@ public class SparkMaxSwerve extends SwerveMotor @Override public void configurePIDF(PIDFConfig config) { - int pidSlot = isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); + int pidSlot = + isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); pid.setP(config.p, pidSlot); pid.setI(config.i, pidSlot); pid.setD(config.d, pidSlot); @@ -228,7 +234,8 @@ public class SparkMaxSwerve extends SwerveMotor * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position */ - public void configureCANStatusFrames(int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) + public void configureCANStatusFrames( + int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) { motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0); motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1); @@ -290,8 +297,13 @@ public class SparkMaxSwerve extends SwerveMotor @Override public void setReference(double setpoint, double feedforward) { - int pidSlot = isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - pid.setReference(setpoint, isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, pidSlot, feedforward); + int pidSlot = + isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); + pid.setReference( + setpoint, + isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, + pidSlot, + feedforward); } /** diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java index 7619986..8c1e393 100644 --- a/swervelib/motors/TalonFXSwerve.java +++ b/swervelib/motors/TalonFXSwerve.java @@ -5,7 +5,7 @@ import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import frc.robot.Robot; +import edu.wpi.first.wpilibj.RobotBase; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.simulation.ctre.PhysicsSim; @@ -55,7 +55,7 @@ public class TalonFXSwerve extends SwerveMotor factoryDefaults(); clearStickyFaults(); - if (!Robot.isReal()) + if (RobotBase.isSimulation()) { PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); } @@ -106,7 +106,6 @@ public class TalonFXSwerve extends SwerveMotor motor.clearStickyFaults(); } - /** * Set the absolute encoder to be a compatible absolute encoder. * @@ -122,15 +121,17 @@ public class TalonFXSwerve extends SwerveMotor /** * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. * - * @param positionConversionFactor The conversion factor to apply for position.


Degrees:
+ * @param positionConversionFactor The conversion factor to apply for position. + *


+ * Degrees:
* * 360 / (angleGearRatio * encoderTicksPerRotation) - *

- *


Meters:
+ *
+ *


+ * Meters:
* * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) * - *

*/ @Override public void configureIntegratedEncoder(double positionConversionFactor) @@ -265,9 +266,8 @@ public class TalonFXSwerve extends SwerveMotor */ public double convertToNativeSensorUnits(double setpoint) { - setpoint = isDriveMotor ? - setpoint * .1 : - placeInAppropriate0To360Scope(getRawPosition(), setpoint); + setpoint = + isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getRawPosition(), setpoint); return setpoint / positionConversionFactor; } @@ -280,17 +280,18 @@ public class TalonFXSwerve extends SwerveMotor @Override public void setReference(double setpoint, double feedforward) { - if (!Robot.isReal()) + if (RobotBase.isSimulation()) { PhysicsSim.getInstance().run(); } burnFlash(); - motor.set(isDriveMotor ? ControlMode.Velocity : ControlMode.Position, - convertToNativeSensorUnits(setpoint), - DemandType.ArbitraryFeedForward, - feedforward * -0.3); + motor.set( + isDriveMotor ? ControlMode.Velocity : ControlMode.Position, + convertToNativeSensorUnits(setpoint), + DemandType.ArbitraryFeedForward, + feedforward * -0.3); // Credit to Team 3181 for the -0.3, I'm not sure why it works, but it does. } @@ -334,7 +335,7 @@ public class TalonFXSwerve extends SwerveMotor @Override public void setPosition(double position) { - if (!absoluteEncoder && Robot.isReal()) + if (!absoluteEncoder && !RobotBase.isSimulation()) { motor.setSelectedSensorPosition(convertToNativeSensorUnits(position)); } @@ -400,5 +401,4 @@ public class TalonFXSwerve extends SwerveMotor { return absoluteEncoder; } - } diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java index 0494a38..59a5056 100644 --- a/swervelib/motors/TalonSRXSwerve.java +++ b/swervelib/motors/TalonSRXSwerve.java @@ -6,7 +6,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import frc.robot.Robot; +import edu.wpi.first.wpilibj.RobotBase; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -20,18 +20,18 @@ public class TalonSRXSwerve extends SwerveMotor * Factory default already occurred. */ private final boolean factoryDefaultOccurred = false; + /** + * Whether the absolute encoder is integrated. + */ + private final boolean absoluteEncoder = false; /** * TalonSRX motor controller. */ WPI_TalonSRX motor; - /** - * Whether the absolute encoder is integrated. - */ - private final boolean absoluteEncoder = false; /** * The position conversion factor. */ - private double positionConversionFactor = 1; + private double positionConversionFactor = 1; /** * Constructor for TalonSRX swerve motor. @@ -82,7 +82,6 @@ public class TalonSRXSwerve extends SwerveMotor motor.clearStickyFaults(); } - /** * Set the absolute encoder to be a compatible absolute encoder. * @@ -187,10 +186,11 @@ public class TalonSRXSwerve extends SwerveMotor { burnFlash(); - motor.set(isDriveMotor ? ControlMode.Velocity : ControlMode.Position, - convertToNativeSensorUnits(setpoint), - DemandType.ArbitraryFeedForward, - feedforward * -0.3); + motor.set( + isDriveMotor ? ControlMode.Velocity : ControlMode.Position, + convertToNativeSensorUnits(setpoint), + DemandType.ArbitraryFeedForward, + feedforward * -0.3); // Credit to Team 3181 for the -0.3, I'm not sure why it works, but it does. } @@ -234,7 +234,7 @@ public class TalonSRXSwerve extends SwerveMotor @Override public void setPosition(double position) { - if (!absoluteEncoder && Robot.isReal()) + if (!absoluteEncoder && !RobotBase.isSimulation()) { motor.setSelectedSensorPosition(convertToNativeSensorUnits(position)); } @@ -354,9 +354,8 @@ public class TalonSRXSwerve extends SwerveMotor */ public double convertToNativeSensorUnits(double setpoint) { - setpoint = isDriveMotor ? - setpoint * .1 : - placeInAppropriate0To360Scope(getRawPosition(), setpoint); + setpoint = + isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getRawPosition(), setpoint); return setpoint / positionConversionFactor; } } diff --git a/swervelib/motors/package-info.java b/swervelib/motors/package-info.java index 6e1f6ea..d8bd36c 100644 --- a/swervelib/motors/package-info.java +++ b/swervelib/motors/package-info.java @@ -1,4 +1,4 @@ /** * Swerve motor controller wrappers which implement {@link swervelib.motors.SwerveMotor}. */ -package swervelib.motors; \ No newline at end of file +package swervelib.motors; diff --git a/swervelib/package-info.java b/swervelib/package-info.java index 10e78bc..a9884ca 100644 --- a/swervelib/package-info.java +++ b/swervelib/package-info.java @@ -3,4 +3,4 @@ * * @version 1.0.0 */ -package swervelib; \ No newline at end of file +package swervelib; diff --git a/swervelib/parser/PIDFConfig.java b/swervelib/parser/PIDFConfig.java index c052b66..9da0684 100644 --- a/swervelib/parser/PIDFConfig.java +++ b/swervelib/parser/PIDFConfig.java @@ -106,4 +106,3 @@ public class PIDFConfig return new PIDController(p, i, d); } } - diff --git a/swervelib/parser/SwerveControllerConfiguration.java b/swervelib/parser/SwerveControllerConfiguration.java index f77ffcb..2649544 100644 --- a/swervelib/parser/SwerveControllerConfiguration.java +++ b/swervelib/parser/SwerveControllerConfiguration.java @@ -23,7 +23,8 @@ public class SwerveControllerConfiguration /** * hypotenuse deadband for the robot angle control joystick. */ - public final double angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick. + public final double + angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick. /** * Construct the swerve controller configuration. @@ -32,13 +33,17 @@ public class SwerveControllerConfiguration * @param headingPIDF Heading PIDF configuration. * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick. */ - public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF, - double angleJoyStickRadiusDeadband) + public SwerveControllerConfiguration( + SwerveDriveConfiguration driveCfg, + PIDFConfig headingPIDF, + double angleJoyStickRadiusDeadband) { this.maxSpeed = driveCfg.maxSpeed; - this.maxAngularVelocity = calculateMaxAngularVelocity(driveCfg.maxSpeed, - Math.abs(driveCfg.moduleLocationsMeters[0].getX()), - Math.abs(driveCfg.moduleLocationsMeters[0].getY())); + this.maxAngularVelocity = + calculateMaxAngularVelocity( + driveCfg.maxSpeed, + Math.abs(driveCfg.moduleLocationsMeters[0].getX()), + Math.abs(driveCfg.moduleLocationsMeters[0].getY())); this.headingPIDF = headingPIDF; this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband; } @@ -54,5 +59,4 @@ public class SwerveControllerConfiguration { this(driveCfg, headingPIDF, 0.5); } - } diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java index 81b7b81..24bf4c0 100644 --- a/swervelib/parser/SwerveDriveConfiguration.java +++ b/swervelib/parser/SwerveDriveConfiguration.java @@ -43,8 +43,11 @@ public class SwerveDriveConfiguration * @param maxSpeed Max speed of the robot in meters per second. * @param invertedIMU Invert the IMU. */ - public SwerveDriveConfiguration(SwerveModuleConfiguration[] moduleConfigs, SwerveIMU swerveIMU, double maxSpeed, - boolean invertedIMU) + public SwerveDriveConfiguration( + SwerveModuleConfiguration[] moduleConfigs, + SwerveIMU swerveIMU, + double maxSpeed, + boolean invertedIMU) { this.moduleCount = moduleConfigs.length; this.imu = swerveIMU; diff --git a/swervelib/parser/SwerveModuleConfiguration.java b/swervelib/parser/SwerveModuleConfiguration.java index 12b6e6c..274704b 100644 --- a/swervelib/parser/SwerveModuleConfiguration.java +++ b/swervelib/parser/SwerveModuleConfiguration.java @@ -65,7 +65,6 @@ public class SwerveModuleConfiguration */ public SwerveAbsoluteEncoder absoluteEncoder; - /** * Construct a configuration object for swerve modules. * @@ -83,13 +82,20 @@ public class SwerveModuleConfiguration * @param maxSpeed Maximum speed in meters per second. * @param physicalCharacteristics Physical characteristics of the swerve module. */ - public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor, - SwerveAbsoluteEncoder absoluteEncoder, double angleOffset, - double xMeters, - double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, double maxSpeed, - SwerveModulePhysicalCharacteristics physicalCharacteristics, - boolean absoluteEncoderInverted, boolean driveMotorInverted, - boolean angleMotorInverted) + public SwerveModuleConfiguration( + SwerveMotor driveMotor, + SwerveMotor angleMotor, + SwerveAbsoluteEncoder absoluteEncoder, + double angleOffset, + double xMeters, + double yMeters, + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + double maxSpeed, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + boolean absoluteEncoderInverted, + boolean driveMotorInverted, + boolean angleMotorInverted) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; @@ -102,11 +108,12 @@ public class SwerveModuleConfiguration this.anglePIDF = anglePIDF; this.velocityPIDF = velocityPIDF; this.maxSpeed = maxSpeed; - this.angleKV = calculateAngleKV(physicalCharacteristics.optimalVoltage, - physicalCharacteristics.angleMotorFreeSpeedRPM, - physicalCharacteristics.angleGearRatio); + this.angleKV = + calculateAngleKV( + physicalCharacteristics.optimalVoltage, + physicalCharacteristics.angleMotorFreeSpeedRPM, + physicalCharacteristics.angleGearRatio); this.physicalCharacteristics = physicalCharacteristics; - } /** @@ -124,27 +131,34 @@ public class SwerveModuleConfiguration * @param maxSpeed Maximum robot speed in meters per second. * @param physicalCharacteristics Physical characteristics of the swerve module. */ - public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor, - SwerveAbsoluteEncoder absoluteEncoder, double angleOffset, - double xMeters, double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, - double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics) + public SwerveModuleConfiguration( + SwerveMotor driveMotor, + SwerveMotor angleMotor, + SwerveAbsoluteEncoder absoluteEncoder, + double angleOffset, + double xMeters, + double yMeters, + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + double maxSpeed, + SwerveModulePhysicalCharacteristics physicalCharacteristics) { - this(driveMotor, - angleMotor, - absoluteEncoder, - angleOffset, - xMeters, - yMeters, - anglePIDF, - velocityPIDF, - maxSpeed, - physicalCharacteristics, - false, - false, - false); + this( + driveMotor, + angleMotor, + absoluteEncoder, + angleOffset, + xMeters, + yMeters, + anglePIDF, + velocityPIDF, + maxSpeed, + physicalCharacteristics, + false, + false, + false); } - /** * Create the drive feedforward for swerve modules. * @@ -153,10 +167,11 @@ public class SwerveModuleConfiguration 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) + /// ^ 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); } @@ -168,11 +183,13 @@ public class SwerveModuleConfiguration */ public double getPositionEncoderConversion(boolean isDriveMotor) { - return isDriveMotor ? calculateMetersPerRotation(physicalCharacteristics.wheelDiameter, - physicalCharacteristics.driveGearRatio, - physicalCharacteristics.driveEncoderPulsePerRotation) - : calculateDegreesPerSteeringRotation( - physicalCharacteristics.angleGearRatio, - physicalCharacteristics.angleEncoderPulsePerRotation); + return isDriveMotor + ? calculateMetersPerRotation( + physicalCharacteristics.wheelDiameter, + physicalCharacteristics.driveGearRatio, + physicalCharacteristics.driveEncoderPulsePerRotation) + : calculateDegreesPerSteeringRotation( + physicalCharacteristics.angleGearRatio, + physicalCharacteristics.angleEncoderPulsePerRotation); } } diff --git a/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 3d6405e..97d4953 100644 --- a/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -66,13 +66,19 @@ public class SwerveModulePhysicalCharacteristics * @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. */ - public SwerveModulePhysicalCharacteristics(double driveGearRatio, double angleGearRatio, - double angleMotorFreeSpeedRPM, - double wheelDiameter, double wheelGripCoefficientOfFriction, - double optimalVoltage, int driveMotorCurrentLimit, - int angleMotorCurrentLimit, double driveMotorRampRate, - double angleMotorRampRate, int driveEncoderPulsePerRotation, - int angleEncoderPulsePerRotation) + public SwerveModulePhysicalCharacteristics( + double driveGearRatio, + double angleGearRatio, + double angleMotorFreeSpeedRPM, + double wheelDiameter, + double wheelGripCoefficientOfFriction, + double optimalVoltage, + int driveMotorCurrentLimit, + int angleMotorCurrentLimit, + double driveMotorRampRate, + double angleMotorRampRate, + int driveEncoderPulsePerRotation, + int angleEncoderPulsePerRotation) { this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; this.optimalVoltage = optimalVoltage; @@ -106,12 +112,28 @@ public class SwerveModulePhysicalCharacteristics * @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. */ - public SwerveModulePhysicalCharacteristics(double driveGearRatio, double angleGearRatio, - double angleMotorFreeSpeedRPM, - double wheelDiameter, double driveMotorRampRate, double angleMotorRampRate, - int driveEncoderPulsePerRotation, int angleEncoderPulsePerRotation) + public SwerveModulePhysicalCharacteristics( + double driveGearRatio, + double angleGearRatio, + double angleMotorFreeSpeedRPM, + double wheelDiameter, + double driveMotorRampRate, + double angleMotorRampRate, + int driveEncoderPulsePerRotation, + int angleEncoderPulsePerRotation) { - this(driveGearRatio, angleGearRatio, angleMotorFreeSpeedRPM, wheelDiameter, 1.19, 12, - 40, 20, driveMotorRampRate, angleMotorRampRate, driveEncoderPulsePerRotation, angleEncoderPulsePerRotation); + this( + driveGearRatio, + angleGearRatio, + angleMotorFreeSpeedRPM, + wheelDiameter, + 1.19, + 12, + 40, + 20, + driveMotorRampRate, + angleMotorRampRate, + driveEncoderPulsePerRotation, + angleEncoderPulsePerRotation); } } diff --git a/swervelib/parser/SwerveParser.java b/swervelib/parser/SwerveParser.java index 2269307..1e29b78 100644 --- a/swervelib/parser/SwerveParser.java +++ b/swervelib/parser/SwerveParser.java @@ -45,7 +45,6 @@ public class SwerveParser */ public static ModuleJson[] moduleJsons; - /** * Construct a swerve parser. Will throw an error if there is a missing file. * @@ -55,13 +54,22 @@ public class SwerveParser public SwerveParser(File directory) throws IOException { checkDirectory(directory); - swerveDriveJson = new ObjectMapper().readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class); - controllerPropertiesJson = new ObjectMapper().readValue(new File(directory, "controllerproperties.json"), - ControllerPropertiesJson.class); - pidfPropertiesJson = new ObjectMapper().readValue(new File(directory, "modules/pidfproperties.json"), - PIDFPropertiesJson.class); - physicalPropertiesJson = new ObjectMapper().readValue(new File(directory, "modules/physicalproperties.json"), - PhysicalPropertiesJson.class); + swerveDriveJson = + new ObjectMapper() + .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class); + controllerPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); + pidfPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); + physicalPropertiesJson = + new ObjectMapper() + .readValue( + new File(directory, "modules/physicalproperties.json"), + PhysicalPropertiesJson.class); moduleJsons = new ModuleJson[swerveDriveJson.modules.length]; for (int i = 0; i < moduleJsons.length; i++) { @@ -79,8 +87,8 @@ public class SwerveParser * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from. * @return {@link SwerveModuleConfiguration} based on the file. */ - public static SwerveModule getModuleConfigurationByName(String name, - SwerveDriveConfiguration driveConfiguration) + public static SwerveModule getModuleConfigurationByName( + String name, SwerveDriveConfiguration driveConfiguration) { return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; } @@ -123,22 +131,28 @@ public class SwerveParser */ public SwerveDrive createSwerveDrive() { - double maxSpeedMPS = Units.feetToMeters(swerveDriveJson.maxSpeed); - SwerveModuleConfiguration[] moduleConfigurations = new SwerveModuleConfiguration[moduleJsons.length]; + double maxSpeedMPS = Units.feetToMeters(swerveDriveJson.maxSpeed); + SwerveModuleConfiguration[] moduleConfigurations = + new SwerveModuleConfiguration[moduleJsons.length]; for (int i = 0; i < moduleConfigurations.length; i++) { ModuleJson module = moduleJsons[i]; - moduleConfigurations[i] = module.createModuleConfiguration(pidfPropertiesJson.angle, pidfPropertiesJson.drive, - maxSpeedMPS, - physicalPropertiesJson.createPhysicalProperties( - swerveDriveJson.optimalVoltage)); + moduleConfigurations[i] = + module.createModuleConfiguration( + pidfPropertiesJson.angle, + pidfPropertiesJson.drive, + maxSpeedMPS, + physicalPropertiesJson.createPhysicalProperties(swerveDriveJson.optimalVoltage)); } - SwerveDriveConfiguration swerveDriveConfiguration = new SwerveDriveConfiguration(moduleConfigurations, - swerveDriveJson.imu.createIMU(), - maxSpeedMPS, - swerveDriveJson.invertedIMU); + SwerveDriveConfiguration swerveDriveConfiguration = + new SwerveDriveConfiguration( + moduleConfigurations, + swerveDriveJson.imu.createIMU(), + maxSpeedMPS, + swerveDriveJson.invertedIMU); - return new SwerveDrive(swerveDriveConfiguration, - controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration)); + return new SwerveDrive( + swerveDriveConfiguration, + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration)); } } diff --git a/swervelib/parser/deserializer/package-info.java b/swervelib/parser/deserializer/package-info.java index 76923d8..820f2cc 100644 --- a/swervelib/parser/deserializer/package-info.java +++ b/swervelib/parser/deserializer/package-info.java @@ -1,4 +1,4 @@ /** * Deserialize specific variables for outside the parser. */ -package swervelib.parser.deserializer; \ No newline at end of file +package swervelib.parser.deserializer; diff --git a/swervelib/parser/json/ControllerPropertiesJson.java b/swervelib/parser/json/ControllerPropertiesJson.java index 9cd4426..f3a49e8 100644 --- a/swervelib/parser/json/ControllerPropertiesJson.java +++ b/swervelib/parser/json/ControllerPropertiesJson.java @@ -25,8 +25,10 @@ public class ControllerPropertiesJson * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration. * @return {@link SwerveControllerConfiguration} object based on parsed data. */ - public SwerveControllerConfiguration createControllerConfiguration(SwerveDriveConfiguration driveConfiguration) + public SwerveControllerConfiguration createControllerConfiguration( + SwerveDriveConfiguration driveConfiguration) { - return new SwerveControllerConfiguration(driveConfiguration, heading, angleJoystickRadiusDeadband); + return new SwerveControllerConfiguration( + driveConfiguration, heading, angleJoystickRadiusDeadband); } } diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java index b26547b..47232a7 100644 --- a/swervelib/parser/json/DeviceJson.java +++ b/swervelib/parser/json/DeviceJson.java @@ -102,7 +102,6 @@ public class DeviceJson return new TalonSRXSwerve(id, isDriveMotor); default: throw new RuntimeException(type + " is not a recognized absolute encoder type."); - } } @@ -121,6 +120,7 @@ public class DeviceJson case "none": return null; } - throw new RuntimeException("Could not create absolute encoder from data port of " + type + " id " + id); + throw new RuntimeException( + "Could not create absolute encoder from data port of " + type + " id " + id); } } diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java index 70a6fa3..7a0bafc 100644 --- a/swervelib/parser/json/ModuleJson.java +++ b/swervelib/parser/json/ModuleJson.java @@ -53,9 +53,11 @@ public class ModuleJson * @param physicalCharacteristics Physical characteristics of the swerve module. * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data. */ - public SwerveModuleConfiguration createModuleConfiguration(PIDFConfig anglePIDF, PIDFConfig velocityPIDF, - double maxSpeed, - SwerveModulePhysicalCharacteristics physicalCharacteristics) + public SwerveModuleConfiguration createModuleConfiguration( + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + double maxSpeed, + SwerveModulePhysicalCharacteristics physicalCharacteristics) { SwerveMotor angleMotor = angle.createMotor(false); SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(); @@ -67,10 +69,19 @@ public class ModuleJson angleMotor.setAbsoluteEncoder(absEncoder); } - return new SwerveModuleConfiguration(drive.createMotor(true), angleMotor, absEncoder, - absoluteEncoderOffset, Units.inchesToMeters(location.x), - Units.inchesToMeters(location.y), anglePIDF, velocityPIDF, maxSpeed, - physicalCharacteristics, absoluteEncoderInverted, inverted.drive, - inverted.angle); + return new SwerveModuleConfiguration( + drive.createMotor(true), + angleMotor, + absEncoder, + absoluteEncoderOffset, + Units.inchesToMeters(location.x), + Units.inchesToMeters(location.y), + anglePIDF, + velocityPIDF, + maxSpeed, + physicalCharacteristics, + absoluteEncoderInverted, + inverted.drive, + inverted.angle); } } diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java index eee440b..b8a5929 100644 --- a/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/swervelib/parser/json/PhysicalPropertiesJson.java @@ -46,12 +46,19 @@ public class PhysicalPropertiesJson */ public SwerveModulePhysicalCharacteristics createPhysicalProperties(double optimalVoltage) { - return new SwerveModulePhysicalCharacteristics(gearRatio.drive, gearRatio.angle, angleMotorFreeSpeedRPM, - Units.inchesToMeters(wheelDiameter), wheelGripCoefficientOfFriction, - optimalVoltage, - currentLimit.drive, currentLimit.angle, rampRate.drive, - rampRate.angle, encoderPulsePerRotation.drive, - encoderPulsePerRotation.angle); + return new SwerveModulePhysicalCharacteristics( + gearRatio.drive, + gearRatio.angle, + angleMotorFreeSpeedRPM, + Units.inchesToMeters(wheelDiameter), + wheelGripCoefficientOfFriction, + optimalVoltage, + currentLimit.drive, + currentLimit.angle, + rampRate.drive, + rampRate.angle, + encoderPulsePerRotation.drive, + encoderPulsePerRotation.angle); } } @@ -88,7 +95,6 @@ class MotorConfigDouble this.angle = angle; this.drive = drive; } - } /** @@ -124,4 +130,4 @@ class MotorConfigInt this.angle = angle; this.drive = drive; } -} \ No newline at end of file +} diff --git a/swervelib/parser/json/modules/LocationJson.java b/swervelib/parser/json/modules/LocationJson.java index 4c4a5f0..c0a403c 100644 --- a/swervelib/parser/json/modules/LocationJson.java +++ b/swervelib/parser/json/modules/LocationJson.java @@ -4,7 +4,6 @@ package swervelib.parser.json.modules; * Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as distances to the center of * the robot. Positive x is torwards the robot front, and * +y is torwards robot left. */ - public class LocationJson { diff --git a/swervelib/parser/json/modules/package-info.java b/swervelib/parser/json/modules/package-info.java index 4ec91de..b5ff2b7 100644 --- a/swervelib/parser/json/modules/package-info.java +++ b/swervelib/parser/json/modules/package-info.java @@ -1,4 +1,4 @@ /** * JSON Mapped Configuration types for modules. */ -package swervelib.parser.json.modules; \ No newline at end of file +package swervelib.parser.json.modules; diff --git a/swervelib/parser/json/package-info.java b/swervelib/parser/json/package-info.java index 14496ea..836fbc0 100644 --- a/swervelib/parser/json/package-info.java +++ b/swervelib/parser/json/package-info.java @@ -1,4 +1,4 @@ /** * JSON Mapped classes for parsing configuration files. */ -package swervelib.parser.json; \ No newline at end of file +package swervelib.parser.json; diff --git a/swervelib/parser/package-info.java b/swervelib/parser/package-info.java index 60f8785..ade6ed3 100644 --- a/swervelib/parser/package-info.java +++ b/swervelib/parser/package-info.java @@ -1,4 +1,4 @@ /** * JSON Parser for YAGSL configurations. */ -package swervelib.parser; \ No newline at end of file +package swervelib.parser; diff --git a/swervelib/simulation/SwerveIMUSimulation.java b/swervelib/simulation/SwerveIMUSimulation.java index 069c801..2a80f66 100644 --- a/swervelib/simulation/SwerveIMUSimulation.java +++ b/swervelib/simulation/SwerveIMUSimulation.java @@ -86,8 +86,11 @@ public class SwerveIMUSimulation * @param modulePoses {@link Pose2d} representing the swerve modules. * @param field {@link Field2d} to update. */ - public void updateOdometry(SwerveKinematics2 kinematics, SwerveModuleState2[] states, Pose2d[] modulePoses, - Field2d field) + public void updateOdometry( + SwerveKinematics2 kinematics, + SwerveModuleState2[] states, + Pose2d[] modulePoses, + Field2d field) { angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime); lastTime = timer.get(); diff --git a/swervelib/simulation/SwerveModuleSimulation.java b/swervelib/simulation/SwerveModuleSimulation.java index 8c635f6..d34f6a6 100644 --- a/swervelib/simulation/SwerveModuleSimulation.java +++ b/swervelib/simulation/SwerveModuleSimulation.java @@ -73,7 +73,8 @@ public class SwerveModuleSimulation */ public SwerveModulePosition getPosition() { - return new SwerveModulePosition(fakePos, state.angle.plus(new Rotation2d(state.omegaRadPerSecond * dt))); + return new SwerveModulePosition( + fakePos, state.angle.plus(new Rotation2d(state.omegaRadPerSecond * dt))); } /** diff --git a/swervelib/simulation/ctre/PhysicsSim.java b/swervelib/simulation/ctre/PhysicsSim.java index 373fad8..1b9122d 100644 --- a/swervelib/simulation/ctre/PhysicsSim.java +++ b/swervelib/simulation/ctre/PhysicsSim.java @@ -16,6 +16,8 @@ public class PhysicsSim /** * Gets the robot simulator instance. + * + * @return {@link PhysicsSim} instance. */ public static PhysicsSim getInstance() { @@ -25,7 +27,8 @@ public class PhysicsSim /* scales a random domain of [0, 2pi] to [min, max] while prioritizing the peaks */ static double random(double min, double max) { - return (max - min) / 2 * Math.sin(Math.IEEEremainder(Math.random(), 2 * 3.14159)) + (max + min) / 2; + return (max - min) / 2 * Math.sin(Math.IEEEremainder(Math.random(), 2 * 3.14159)) + + (max + min) / 2; } static double random(double max) @@ -53,11 +56,16 @@ public class PhysicsSim * @param fullVel The maximum motor velocity, in ticks per 100ms * @param sensorPhase The phase of the TalonSRX sensors */ - public void addTalonSRX(TalonSRX talon, final double accelToFullTime, final double fullVel, final boolean sensorPhase) + public void addTalonSRX( + TalonSRX talon, + final double accelToFullTime, + final double fullVel, + final boolean sensorPhase) { if (talon != null) { - TalonSRXSimProfile simTalon = new TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase); + TalonSRXSimProfile simTalon = + new TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase); _simProfiles.add(simTalon); } } @@ -82,11 +90,16 @@ public class PhysicsSim * @param fullVel The maximum motor velocity, in ticks per 100ms * @param sensorPhase The phase of the TalonFX sensors */ - public void addTalonFX(TalonFX falcon, final double accelToFullTime, final double fullVel, final boolean sensorPhase) + public void addTalonFX( + TalonFX falcon, + final double accelToFullTime, + final double fullVel, + final boolean sensorPhase) { if (falcon != null) { - TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase); + TalonFXSimProfile simFalcon = + new TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase); _simProfiles.add(simFalcon); } } @@ -152,4 +165,4 @@ public class PhysicsSim return period; } } -} \ No newline at end of file +} diff --git a/swervelib/simulation/ctre/TalonFXSimProfile.java b/swervelib/simulation/ctre/TalonFXSimProfile.java index 46f6b02..1c57d2e 100644 --- a/swervelib/simulation/ctre/TalonFXSimProfile.java +++ b/swervelib/simulation/ctre/TalonFXSimProfile.java @@ -31,8 +31,11 @@ class TalonFXSimProfile extends SimProfile * @param fullVel The maximum motor velocity, in ticks per 100ms * @param sensorPhase The phase of the TalonFX sensors */ - public TalonFXSimProfile(final TalonFX falcon, final double accelToFullTime, final double fullVel, - final boolean sensorPhase) + public TalonFXSimProfile( + final TalonFX falcon, + final double accelToFullTime, + final double fullVel, + final boolean sensorPhase) { this._falcon = falcon; this._accelToFullTime = accelToFullTime; @@ -42,9 +45,10 @@ class TalonFXSimProfile extends SimProfile /** * Runs the simulation profile. - *

- * This uses very rudimentary physics simulation and exists to allow users to test features of our products in - * simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation. + * + *

This uses very rudimentary physics simulation and exists to allow users to test features of + * our products in simulation using our examples out of the box. Users may modify this to utilize more accurate + * physics simulation. */ public void run() { @@ -85,4 +89,4 @@ class TalonFXSimProfile extends SimProfile _falcon.getSimCollection().setBusVoltage(12 - outPerc * outPerc * 3 / 4 * random(0.95, 1.05)); } -} \ No newline at end of file +} diff --git a/swervelib/simulation/ctre/TalonSRXSimProfile.java b/swervelib/simulation/ctre/TalonSRXSimProfile.java index 299f33c..7b55fc7 100644 --- a/swervelib/simulation/ctre/TalonSRXSimProfile.java +++ b/swervelib/simulation/ctre/TalonSRXSimProfile.java @@ -31,8 +31,11 @@ class TalonSRXSimProfile extends SimProfile * @param fullVel The maximum motor velocity, in ticks per 100ms * @param sensorPhase The phase of the TalonSRX sensors */ - public TalonSRXSimProfile(final TalonSRX talon, final double accelToFullTime, final double fullVel, - final boolean sensorPhase) + public TalonSRXSimProfile( + final TalonSRX talon, + final double accelToFullTime, + final double fullVel, + final boolean sensorPhase) { this._talon = talon; this._accelToFullTime = accelToFullTime; @@ -42,9 +45,10 @@ class TalonSRXSimProfile extends SimProfile /** * Runs the simulation profile. - *

- * This uses very rudimentary physics simulation and exists to allow users to test features of our products in - * simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation. + * + *

This uses very rudimentary physics simulation and exists to allow users to test features of + * our products in simulation using our examples out of the box. Users may modify this to utilize more accurate + * physics simulation. */ public void run() { diff --git a/swervelib/simulation/ctre/VictorSPXSimProfile.java b/swervelib/simulation/ctre/VictorSPXSimProfile.java index f1553ad..a4d1009 100644 --- a/swervelib/simulation/ctre/VictorSPXSimProfile.java +++ b/swervelib/simulation/ctre/VictorSPXSimProfile.java @@ -1,6 +1,5 @@ package swervelib.simulation.ctre; - import static swervelib.simulation.ctre.PhysicsSim.random; import com.ctre.phoenix.motorcontrol.can.VictorSPX; @@ -26,9 +25,10 @@ class VictorSPXSimProfile extends SimProfile /** * Runs the simulation profile. - *

- * This uses very rudimentary physics simulation and exists to allow users to test features of our products in - * simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation. + * + *

This uses very rudimentary physics simulation and exists to allow users to test features of + * our products in simulation using our examples out of the box. Users may modify this to utilize more accurate + * physics simulation. */ public void run() { diff --git a/swervelib/simulation/ctre/package-info.java b/swervelib/simulation/ctre/package-info.java index 3999f5a..d38353a 100644 --- a/swervelib/simulation/ctre/package-info.java +++ b/swervelib/simulation/ctre/package-info.java @@ -1,4 +1,4 @@ /** * CTRE Physics Simulator. */ -package swervelib.simulation.ctre; \ No newline at end of file +package swervelib.simulation.ctre; diff --git a/swervelib/simulation/package-info.java b/swervelib/simulation/package-info.java index e021267..9aa4aec 100644 --- a/swervelib/simulation/package-info.java +++ b/swervelib/simulation/package-info.java @@ -1,4 +1,4 @@ /** * Classes used to simulate the swerve drive. */ -package swervelib.simulation; \ No newline at end of file +package swervelib.simulation;