diff --git a/README.md b/README.md
index f158228..ebfde58 100644
--- a/README.md
+++ b/README.md
@@ -21,6 +21,7 @@ SwerveDrive swerveDrive=new SwerveParser(new File(Filesystem.getDeployDirectory(
- [ ] Install NavX Library
- [ ] Install Phoenix Library
- [ ] Install REVLib.
+- [ ] Install ReduxLib.
- [ ] Install YAGSL (`https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json`)
### [Easy Configuration File Generation](https://broncbotz3481.github.io/YAGSL-Example/)
@@ -43,6 +44,9 @@ https://user-images.githubusercontent.com/10247070/219801620-c2850078-9b58-4f32-
# Special Thanks to Team 7900! Trial N' Error
Without the debugging and aid of Team 7900 the project could never be as stable or active as it is.
+# Special Thanks to Team 6238! Popcorn Penguins!
+Without their hardwork debugging the issue with feedforwards the 2024 release would not have been possible. Thank you!
+
# Support our developers!
\ No newline at end of file
diff --git a/docs/allclasses-index.html b/docs/allclasses-index.html
index 27891eb..ac17b2f 100644
--- a/docs/allclasses-index.html
+++ b/docs/allclasses-index.html
@@ -1,18 +1,17 @@
-
-All Classes and Interfaces
+
+All Classes and Interfaces (YAGSL-Example API)
-
-
+
@@ -36,7 +35,7 @@ loadScripts(document, 'script');
DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
Encoder." attached via a PWM lane.
The VM is configured to automatically run this class, and to call the functions corresponding to each mode, as
+ described in the TimedRobot documentation.
-Starting from the Overview page, you can browse the documentation using the links in each page, and in the navigation bar at the top of each page. The Index and Search box allow you to navigate to specific declarations and summary pages, including: All Packages, All Classes and Interfaces
+Starting from the Overview page, you can browse the documentation using the links in each page, and in the navigation bar at the top of each page. The Index and Search box allow you to navigate to specific declarations and summary pages, including: All Packages, All Classes and Interfaces
Search
You can search for definitions of modules, packages, types, fields, methods, system properties and other terms defined in the API, using some or all of the name, optionally using "camelCase" abbreviations. For example:
@@ -170,7 +169,7 @@ The following sections describe the different kinds of pages in this collection.
Index
-
The Index contains an alphabetic index of all classes, interfaces, constructors, methods, and fields in the documentation, as well as summary pages such as All Packages, All Classes and Interfaces.
+
The Index contains an alphabetic index of all classes, interfaces, constructors, methods, and fields in the documentation, as well as summary pages such as All Packages, All Classes and Interfaces.
SwerveControllerConfiguration object storing data to generate the PIDController
- for controlling the robot heading, and deadband for heading joystick.
+
SwerveControllerConfiguration object storing data to generate the PIDController for controlling the
+ robot heading, and deadband for heading joystick.
Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well
- as getting rid of joystick saturation at edges of joystick.
Helper function to get the SwerveDrive.swerveController for the SwerveDrive
- which can be used to generate ChassisSpeeds for the robot to orient it correctly given
- axis or angles, and apply SlewRateLimiter to given inputs.
+
Helper function to get the SwerveDrive.swerveController for the SwerveDrive which can be used to
+ generate ChassisSpeeds for the robot to orient it correctly given axis or angles, and apply
+ SlewRateLimiter to given inputs.
Limits a commanded velocity to prevent exceeding the maximum acceleration given by SwerveMath.calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d, java.util.List<swervelib.math.Matter>, double, swervelib.parser.SwerveDriveConfiguration).
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.
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.
DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
+ Encoder." attached via a PWM lane.
The VM is configured to automatically run this class, and to call the functions corresponding to each mode, as
+ described in the TimedRobot documentation.
Trustworthiness of the internal model of how motors should be moving Measured in expected
- standard deviation (meters of position and degrees of rotation)
+
Standard deviation of encoders and gyroscopes, usually should not change.
Clone of SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to
- achieve a more accurate estimation, originally made by Team 1466.
+
Initialize SwerveDrive with the directory provided.
Trustworthiness of the vision system Measured in expected standard deviation (meters of
- position and degrees of rotation)
+
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 SwerveDrive.addVisionMeasurement(Pose2d, double, Matrix)
+ with the calculated optimal standard deviation.
DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
- Encoder." attached via a PWM lane.
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 SwerveDrive.addVisionMeasurement(Pose2d, double, Matrix)
- with the calculated optimal standard deviation.
SwerveControllerConfiguration object storing data to generate the PIDController for controlling the
- robot heading, and deadband for heading joystick.
Helper function to get the SwerveDrive.swerveController for the SwerveDrive which can be used to
- generate ChassisSpeeds for the robot to orient it correctly given axis or angles, and apply
- SlewRateLimiter to given inputs.
Updates the robot's position on the field using forward kinematics and integration of the pose over time. This
- method automatically calculates the current time to calculate period (difference between two timestamps). The
- period is used to calculate the change in distance from a velocity. This also takes in an angle parameter which is
- used instead of the angular rate that is calculated from forward kinematics. This also takes in pitch and roll to
- allow for more accurate pose estimation on angled surfaces using a rotation matrix.
-
-
Parameters:
-
gyroYaw - The yaw reported by the gyro.
-
pitch - The pitch reported by the gyro.
-
roll - The roll reported by the gyro.
-
modulePositions - The current position of all swerve modules. Please provide the positions in the same order
- in which you instantiated your SwerveDriveKinematics.
public class SwerveKinematics2
-extends edu.wpi.first.math.kinematics.SwerveDriveKinematics
-
Clone of WPI SwerveKinematics, which implements second order kinematics when calculating modules states from chassis
- speed. Makes use of SwerveModuleState2 to add the angular velocity that is required of the module as an
- output.
Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well as getting rid of
- joystick saturation at edges of joystick.
Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations as Translation2ds.
- The order in which you pass in the wheel locations is the same order that you will receive the module states when
- performing inverse kinematics. It is also expected that you pass in the module states in the same order when
- calling the forward kinematics methods.
-
-
Parameters:
-
wheelsMeters - The locations of the wheels relative to the physical center of the robot.
-
-
-
-
-
-
-
-
-
-
Method Details
-
-
-
-
desaturateWheelSpeeds
-
public staticvoiddesaturateWheelSpeeds(SwerveModuleState2[] moduleStates,
- double attainableMaxSpeedMetersPerSecond)
-
Renormalizes the wheel speeds if any individual speed is above the specified maximum.
-
-
Sometimes, after inverse kinematics, the requested speed from one or more modules may be
- above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the
- wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while
- maintaining the ratio of speeds between modules.
-
-
Parameters:
-
moduleStates - Reference to array of module states. The array will be mutated with the
- normalized speeds!
-
attainableMaxSpeedMetersPerSecond - The absolute max speed that a module can reach.
Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well as getting rid of
- joystick saturation at edges of joystick.
-
-
Sometimes, after inverse kinematics, the requested speed from one or more modules may be
- above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the
- wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while
- maintaining the ratio of speeds between modules.
-
-
Parameters:
-
moduleStates - Reference to array of module states. The array will be
- mutated with the normalized speeds!
-
currentChassisSpeed - The current speed of the robot
-
attainableMaxModuleSpeedMetersPerSecond - The absolute max speed that a module can reach
-
attainableMaxTranslationalSpeedMetersPerSecond - The absolute max speed that your robot can reach while
- translating
-
attainableMaxRotationalVelocityRadiansPerSecond - The absolute max speed the robot can reach while rotating
Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often used
- to convert joystick values into module speeds and angles.
-
-
This function also supports variable centers of rotation. During normal operations, the
- center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to
- that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or
- for any other use case, you can do so.
-
-
In the case that the desired chassis speeds are zero (i.e. the robot will be stationary),
- the previously calculated module angle will be maintained.
-
-
Overrides:
-
toSwerveModuleStates in class edu.wpi.first.math.kinematics.SwerveDriveKinematics
-
Parameters:
-
chassisSpeeds - The desired chassis speed.
-
centerOfRotationMeters - The center of rotation. For example, if you set the center of rotation at one corner
- of the robot and provide a chassis speed that only has a dtheta component, the robot
- will rotate around that corner.
-
Returns:
-
An array containing the module states. Use caution because these module states are not normalized.
- Sometimes, a user input may cause one of the module speeds to go above the attainable max velocity. Use the
- DesaturateWheelSpeeds function to rectify this issue.
Performs forward kinematics to return the resulting chassis state from the given module states. This method is
- often used for odometry -- determining the robot's position on the field using data from the real-world speed and
- angle of each module on the robot.
-
-
Parameters:
-
wheelStates - The state of the modules (as a SwerveModuleState type) as measured from respective encoders and
- gyros. The order of the swerve module states should be same as passed into the constructor of
- this class.
Performs forward kinematics to return the resulting chassis state from the given module states. This method is
- often used for odometry -- determining the robot's position on the field using data from the real-world speed and
- angle of each module on the robot.
-
-
Overrides:
-
toTwist2d in class edu.wpi.first.math.kinematics.SwerveDriveKinematics
-
Parameters:
-
wheelDeltas - The latest change in position of the modules (as a SwerveModulePosition type) as measured from
- respective encoders and gyros. The order of the swerve module states should be same as passed
- into the constructor of this class.
Limits a commanded velocity to prevent exceeding the maximum acceleration given by calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d, java.util.List<swervelib.math.Matter>, double, swervelib.parser.SwerveDriveConfiguration).
Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm position from
- NetworkTables and is passed the direction in question.
-
-
Parameters:
-
angle - The direction in which to calculate max acceleration, as a Rotation2d. Note that this is
- robot-relative.
-
matter - Matter that the robot is composed of in kg. (Includes chassis)
-
robotMass - The weight of the robot in kg. (Including manipulators, etc).
-
config - The swerve drive configuration.
-
Returns:
-
Maximum acceleration allowed in the robot direction.
-
-
-
-
PoseLog
public staticedu.wpi.first.math.geometry.Twist2dPoseLog(edu.wpi.first.math.geometry.Pose2d transform)
Limits a commanded velocity to prevent exceeding the maximum acceleration given by calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d, java.util.List<swervelib.math.Matter>, double, swervelib.parser.SwerveDriveConfiguration).
Note that this takes and returns field-relative velocities.
Minimize the change in heading the desired swerve module state would require by potentially reversing the direction
- the wheel spins. If this is used with the PIDController class's continuous input functionality, the furthest a
- wheel will ever rotate is 90 degrees.
-
-
Parameters:
-
desiredState - The desired state.
-
currentAngle - The current module angle.
-
lastState - The last state of the module.
-
moduleSteerFeedForwardClosedLoop - The module feed forward closed loop for the angle motor.
Represents an odometry record. The record contains the inputs provided as well as the pose that was observed based
- on these inputs, as well as the previous record and its inputs.
-
-
-
-
-
-
-
Field Summary
-
Fields
-
-
Modifier and Type
-
Field
-
Description
-
private final edu.wpi.first.math.geometry.Rotation2d
Clone of SwerveDrivePoseEstimator which takes into account gyroscope pitch and roll to achieve a more
- accurate estimation, originally made by Team 1466.
Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements.
-
-
The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
- and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 meters for x, 0.9
- meters for y, and 0.9 radians for heading.
-
-
Parameters:
-
kinematics - A correctly-configured kinematics object for your drivetrain.
-
gyroAngle - The current gyro angle.
-
modulePositions - The current distance measurements and rotations of the swerve modules.
kinematics - A correctly-configured kinematics object for your drivetrain.
-
gyroAngle - The current gyro angle.
-
modulePositions - The current distance and rotation measurements of the swerve modules.
-
initialPoseMeters - The starting pose estimate.
-
stateStdDevs - Standard deviations of the pose estimate (x position in meters, y position in
- meters, and heading in radians). Increase these numbers to trust your state
- estimate less.
-
visionMeasurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y
- position in meters, and heading in radians). Increase these numbers to trust the
- vision pose measurement less.
Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements
- after the autonomous period, or to change trust as distance to a vision target increases.
-
-
Parameters:
-
visionMeasurementStdDevs - Standard deviations of the vision measurements. Increase these numbers to trust
- global measurements from vision less. This matrix is in the form [x, y, theta]^T,
- with units in meters and radians.
Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
- for measurement noise.
-
-
This method can be called as infrequently as you want, as long as you are calling SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[]) every loop.
-
-
To promote stability of the pose estimate and make it robust to bad vision data, we
- recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
-
-
Parameters:
-
visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
-
timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your
- own time source by calling
- SwerveDrivePoseEstimator.updateWithTime(double, Rotation2d, SwerveModulePosition[]) then you must use a timestamp with an epoch since FPGA
- startup (i.e., the epoch of this timestamp is the same epoch as
- Timer.getFPGATimestamp().) This means that you should
- use Timer.getFPGATimestamp() as your time source or sync
- the epochs.
Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting
- for measurement noise.
-
-
This method can be called as infrequently as you want, as long as you are calling SwerveDrivePoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.SwerveModulePosition[]) every loop.
-
-
To promote stability of the pose estimate and make it robust to bad vision data, we
- recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
-
-
Note that the vision measurement standard deviations passed into this method will continue
- to apply to future measurements until a subsequent call to
- SwerveDrivePoseEstimator.setVisionMeasurementStdDevs(Matrix) or this method.
-
-
Parameters:
-
visionRobotPoseMeters - The pose of the robot as measured by the vision camera.
-
timestampSeconds - The timestamp of the vision measurement in seconds. Note that if you don't use your
- own time source by calling
- SwerveDrivePoseEstimator.updateWithTime(double, Rotation2d, SwerveModulePosition[]), then you must use a timestamp with an epoch since FPGA
- startup (i.e., the epoch of this timestamp is the same epoch as
- Timer.getFPGATimestamp()). This means that you should
- use Timer.getFPGATimestamp() as your time source in
- this case.
-
visionMeasurementStdDevs - Standard deviations of the vision pose measurement (x position in meters, y
- position in meters, and heading in radians). Increase these numbers to trust the
- vision pose measurement less.
nominalVoltage - Nominal voltage for operation to output to.
+
+
+
+
+
+
setCurrentLimit
+
publicvoidsetCurrentLimit(int currentLimit)
+
Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
+ voltage compensation. This is useful to protect the motor from current spikes.
Returns the enum constant of this class with the specified name.
-The string must match exactly an identifier used to declare an
-enum constant in this class. (Extraneous whitespace characters are
-not permitted.)
-
-
Parameters:
-
name - the name of the enum constant to be returned.
Creates a new simulation profile for a TalonFX device.
-
-
Parameters:
-
falcon - The TalonFX device
-
accelToFullTime - The time the motor takes to accelerate from 0 to full, in seconds
-
fullVel - The maximum motor velocity, in ticks per 100ms
-
sensorPhase - The phase of the TalonFX sensors
-
-
-
-
-
-
-
-
-
-
Method Details
-
-
-
-
run
-
publicvoidrun()
-
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.
Creates a new simulation profile for a TalonSRX device.
-
-
Parameters:
-
talon - The TalonSRX device
-
accelToFullTime - The time the motor takes to accelerate from 0 to full, in seconds
-
fullVel - The maximum motor velocity, in ticks per 100ms
-
sensorPhase - The phase of the TalonSRX sensors
-
-
-
-
-
-
-
-
-
-
Method Details
-
-
-
-
run
-
publicvoidrun()
-
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.
diff --git a/docs/type-search-index.js b/docs/type-search-index.js
index 52aeb53..5e0eb7f 100644
--- a/docs/type-search-index.js
+++ b/docs/type-search-index.js
@@ -1 +1 @@
-typeSearchIndex = [{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"l":"All Classes and Interfaces","u":"allclasses-index.html"},{"p":"swervelib.encoders","l":"AnalogAbsoluteEncoderSwerve"},{"p":"swervelib.imu","l":"AnalogGyroSwerve"},{"p":"swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"swervelib.encoders","l":"CanAndCoderSwerve"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"swervelib.parser.json.modules","l":"LocationJson"},{"p":"swervelib.math","l":"Matter"},{"p":"swervelib.parser.json","l":"ModuleJson"},{"p":"swervelib.parser.json","l":"MotorConfigDouble"},{"p":"swervelib.parser.json","l":"MotorConfigInt"},{"p":"swervelib.imu","l":"NavXSwerve"},{"p":"swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim"},{"p":"swervelib.parser","l":"PIDFConfig"},{"p":"swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"swervelib.parser.deserializer","l":"PIDFRange"},{"p":"swervelib.imu","l":"Pigeon2Swerve"},{"p":"swervelib.imu","l":"PigeonSwerve"},{"p":"swervelib.encoders","l":"PWMDutyCycleEncoderSwerve"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim.SimProfile"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"p":"swervelib.encoders","l":"SparkMaxAnalogEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxBrushedMotorSwerve"},{"p":"swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxSwerve"},{"p":"swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"swervelib","l":"SwerveController"},{"p":"swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"swervelib","l":"SwerveDrive"},{"p":"swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"swervelib.parser.json","l":"SwerveDriveJson"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveMath"},{"p":"swervelib","l":"SwerveModule"},{"p":"swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"swervelib.simulation","l":"SwerveModuleSimulation"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"swervelib.simulation.ctre","l":"TalonFXSimProfile"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.simulation.ctre","l":"TalonSRXSimProfile"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"},{"p":"swervelib.simulation.ctre","l":"VictorSPXSimProfile"}];updateSearchResults();
\ No newline at end of file
+typeSearchIndex = [{"p":"frc.robot.commands.swervedrive.drivebase","l":"AbsoluteDrive"},{"p":"frc.robot.commands.swervedrive.drivebase","l":"AbsoluteDriveAdv"},{"p":"frc.robot.commands.swervedrive.drivebase","l":"AbsoluteFieldDrive"},{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"l":"All Classes and Interfaces","u":"allclasses-index.html"},{"p":"swervelib.encoders","l":"AnalogAbsoluteEncoderSwerve"},{"p":"swervelib.imu","l":"AnalogGyroSwerve"},{"p":"frc.robot.commands.swervedrive.auto","l":"AutoBalanceCommand"},{"p":"frc.robot","l":"Constants.Auton"},{"p":"swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"swervelib.encoders","l":"CanAndCoderSwerve"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"frc.robot","l":"Constants"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"frc.robot","l":"Constants.Drivebase"},{"p":"swervelib.parser.json.modules","l":"LocationJson"},{"p":"frc.robot","l":"Main"},{"p":"swervelib.math","l":"Matter"},{"p":"swervelib.parser.json","l":"ModuleJson"},{"p":"swervelib.parser.json","l":"MotorConfigDouble"},{"p":"swervelib.parser.json","l":"MotorConfigInt"},{"p":"swervelib.imu","l":"NavXSwerve"},{"p":"frc.robot","l":"Constants.OperatorConstants"},{"p":"swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"swervelib.parser","l":"PIDFConfig"},{"p":"swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"swervelib.parser.deserializer","l":"PIDFRange"},{"p":"swervelib.imu","l":"Pigeon2Swerve"},{"p":"swervelib.imu","l":"PigeonSwerve"},{"p":"swervelib.encoders","l":"PWMDutyCycleEncoderSwerve"},{"p":"frc.robot","l":"Robot"},{"p":"frc.robot","l":"RobotContainer"},{"p":"swervelib.motors","l":"SparkFlexSwerve"},{"p":"swervelib.encoders","l":"SparkMaxAnalogEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxBrushedMotorSwerve"},{"p":"swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxSwerve"},{"p":"swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"swervelib","l":"SwerveController"},{"p":"swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"swervelib","l":"SwerveDrive"},{"p":"swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"swervelib.parser.json","l":"SwerveDriveJson"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveMath"},{"p":"swervelib","l":"SwerveModule"},{"p":"swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"swervelib.simulation","l":"SwerveModuleSimulation"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"frc.robot.subsystems.swervedrive","l":"SwerveSubsystem"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"},{"p":"frc.robot.commands.swervedrive.drivebase","l":"TeleopDrive"}];updateSearchResults();
\ No newline at end of file
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index d47d2f6..0915b08 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -100,6 +100,10 @@ public class SwerveDrive
* correction.
*/
public boolean chassisVelocityCorrection = true;
+ /**
+ * Whether heading correction PID is currently active.
+ */
+ private boolean correctionEnabled = false;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
@@ -116,6 +120,10 @@ public class SwerveDrive
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
+ /**
+ * Deadband for speeds in heading correction.
+ */
+ private final double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* The last heading set in radians.
*/
@@ -396,27 +404,28 @@ public class SwerveDrive
// https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
if (chassisVelocityCorrection)
{
- double dtConstant = 0.009;
- Pose2d robotPoseVel = new Pose2d(velocity.vxMetersPerSecond * dtConstant,
- velocity.vyMetersPerSecond * dtConstant,
- Rotation2d.fromRadians(velocity.omegaRadiansPerSecond * dtConstant));
- Twist2d twistVel = SwerveMath.PoseLog(robotPoseVel);
-
- velocity = new ChassisSpeeds(twistVel.dx / dtConstant, twistVel.dy / dtConstant,
- twistVel.dtheta / dtConstant);
+ velocity = ChassisSpeeds.discretize(velocity, 0.02);
}
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
+ // Modified by Team 7525 Pioneers and BoiledBurntBagel of 6036
if (headingCorrection)
{
- if (Math.abs(velocity.omegaRadiansPerSecond) < 0.01)
+ if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND
+ && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
+ || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
{
+ if (!correctionEnabled)
+ {
+ lastHeadingRadians = getYaw().getRadians();
+ correctionEnabled = true;
+ }
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
} else
{
- lastHeadingRadians = getYaw().getRadians();
+ correctionEnabled = false;
}
}
@@ -488,15 +497,6 @@ public class SwerveDrive
SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) +
1] = module.lastState.speedMetersPerSecond;
}
- if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
- {
- SmartDashboard.putNumber(
- "Module[" + module.configuration.name + "] Speed Setpoint: ",
- module.lastState.speedMetersPerSecond);
- SmartDashboard.putNumber(
- "Module[" + module.configuration.name + "] Angle Setpoint: ",
- module.lastState.angle.getDegrees());
- }
}
}
@@ -897,13 +897,8 @@ public class SwerveDrive
sumVelocity += Math.abs(moduleState.speedMetersPerSecond);
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
- SmartDashboard.putNumber(
- "Module[" + module.configuration.name + "] Relative Encoder", module.getRelativePosition());
- SmartDashboard.putNumber(
- "Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition());
- SmartDashboard.putNumber(
- "Module[" + module.configuration.name + "] Absolute Encoder Read Issue",
- module.getAbsoluteEncoderReadIssue() ? 1 : 0);
+ module.updateTelemetry();
+ SmartDashboard.putNumber("Adjusted IMU Yaw", getYaw().getDegrees());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
@@ -1051,7 +1046,7 @@ public class SwerveDrive
* Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in
* autonomous.
*/
- public void resetEncoders()
+ public void resetDriveEncoders()
{
for (SwerveModule module : swerveModules)
{
@@ -1059,19 +1054,6 @@ public class SwerveDrive
}
}
- /**
- * Configure whether the {@link SwerveModuleState} will be optimized to prevent spinning more than 90deg.
- *
- * @param optimizationEnabled Whether the module optimization is enabled.
- */
- public void setModuleStateOptimization(boolean optimizationEnabled)
- {
- for (SwerveModule module : swerveModules)
- {
- module.moduleStateOptimization = optimizationEnabled;
- }
- }
-
/**
* Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the
* internal offsets to prevent double offsetting.
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 999ccc1..82b3ca9 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -4,6 +4,7 @@ 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.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
@@ -48,15 +49,10 @@ public class SwerveModule
* Last swerve module state applied.
*/
public SwerveModuleState lastState;
- /**
- * Enable {@link SwerveModuleState} optimizations so the angle is reversed and speed is reversed to ensure the module
- * never turns more than 90deg.
- */
- public boolean moduleStateOptimization = true;
/**
* Angle offset from the absolute encoder.
*/
- private double angleOffset;
+ private double angleOffset;
/**
* Simulated swerve module.
*/
@@ -114,7 +110,7 @@ public class SwerveModule
// Config angle motor/controller
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle);
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
- angleMotor.configurePIDWrapping(-180, 180);
+ angleMotor.configurePIDWrapping(0, 180);
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
angleMotor.setMotorBrake(false);
@@ -178,11 +174,7 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
{
- if (moduleStateOptimization)
- {
- desiredState = SwerveModuleState.optimize(desiredState,
- Rotation2d.fromDegrees(getAbsolutePosition()));
- }
+ desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
if (isOpenLoop)
{
@@ -190,40 +182,44 @@ public class SwerveModule
driveMotor.set(percentOutput);
} else
{
- if (desiredState.speedMetersPerSecond != lastState.speedMetersPerSecond)
+ // Taken from the CTRE SwerveModule class.
+ // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
+ /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
+ /* To reduce the "skew" that occurs when changing direction */
+ double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition();
+ /* If error is close to 0 rotations, we're already there, so apply full power */
+ /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
+ double cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError));
+ /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
+ if (cosineScalar < 0.0)
{
- double velocity = desiredState.speedMetersPerSecond;
- driveMotor.setReference(velocity, feedforward.calculate(velocity));
+ cosineScalar = 0.0;
}
+
+ double velocity = desiredState.speedMetersPerSecond * (cosineScalar);
+ driveMotor.setReference(velocity, feedforward.calculate(velocity));
}
+ /* // Not necessary anymore.
// If we are forcing the angle
if (!force)
{
- // Prevents module rotation if speed is less than 1%
+ // Prevents module rotation if speed is less than 1%
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4));
}
-
- if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
- {
- SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint:", desiredState.speedMetersPerSecond);
- SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint:", desiredState.angle.getDegrees());
- }
+ */
// Prevent module rotation if angle is the same as the previous angle.
- if (desiredState.angle != lastState.angle || synchronizeEncoderQueued)
+ // Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
+ if (absoluteEncoder != null && synchronizeEncoderQueued)
{
- // Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
- if (absoluteEncoder != null && synchronizeEncoderQueued)
- {
- double absoluteEncoderPosition = getAbsolutePosition();
- angleMotor.setPosition(absoluteEncoderPosition);
- angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
- synchronizeEncoderQueued = false;
- } else
- {
- angleMotor.setReference(desiredState.angle.getDegrees(), 0);
- }
+ double absoluteEncoderPosition = getAbsolutePosition();
+ angleMotor.setPosition(absoluteEncoderPosition);
+ angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
+ synchronizeEncoderQueued = false;
+ } else
+ {
+ angleMotor.setReference(desiredState.angle.getDegrees(), 0);
}
lastState = desiredState;
@@ -232,6 +228,12 @@ public class SwerveModule
{
simModule.updateStateAndPosition(desiredState);
}
+
+ if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
+ {
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond);
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees());
+ }
}
/**
@@ -282,10 +284,6 @@ public class SwerveModule
{
return simModule.getPosition();
}
- if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
- {
- SmartDashboard.putNumber("Module[" + configuration.name + "] Angle", azimuth.getDegrees());
- }
return new SwerveModulePosition(position, azimuth);
}
@@ -434,4 +432,20 @@ public class SwerveModule
return absoluteEncoder.readingError;
}
}
+
+ /**
+ * Update data sent to {@link SmartDashboard}.
+ */
+ public void updateTelemetry()
+ {
+ if (absoluteEncoder != null)
+ {
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Absolute Encoder",
+ absoluteEncoder.getAbsolutePosition());
+ }
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Motor Encoder", angleMotor.getPosition());
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Adjusted Absolute Encoder", getAbsolutePosition());
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Absolute Encoder Read Issue",
+ getAbsoluteEncoderReadIssue() ? 1 : 0);
+ }
}
diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java
index 87c93cc..5d8c328 100644
--- a/swervelib/encoders/CANCoderSwerve.java
+++ b/swervelib/encoders/CANCoderSwerve.java
@@ -1,12 +1,14 @@
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;
-import com.ctre.phoenix.sensors.SensorInitializationStrategy;
-import com.ctre.phoenix.sensors.SensorTimeBase;
-import com.ctre.phoenix.sensors.WPI_CANCoder;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.CANcoderConfigurator;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.MagnetHealthValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
import edu.wpi.first.wpilibj.DriverStation;
/**
@@ -18,7 +20,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
/**
* CANCoder with WPILib sendable and support.
*/
- public WPI_CANCoder encoder;
+ public CANcoder encoder;
/**
* Initialize the CANCoder on the standard CANBus.
@@ -27,7 +29,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
*/
public CANCoderSwerve(int id)
{
- encoder = new WPI_CANCoder(id);
+ encoder = new CANcoder(id);
}
/**
@@ -38,7 +40,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
*/
public CANCoderSwerve(int id, String canbus)
{
- encoder = new WPI_CANCoder(id, canbus);
+ encoder = new CANcoder(id, canbus);
}
/**
@@ -47,7 +49,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public void factoryDefault()
{
- encoder.configFactoryDefault();
+ encoder.getConfigurator().apply(new CANcoderConfiguration());
}
/**
@@ -67,13 +69,13 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public void configure(boolean inverted)
{
- CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
- canCoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360;
- canCoderConfiguration.sensorDirection = inverted;
- canCoderConfiguration.initializationStrategy =
- SensorInitializationStrategy.BootToAbsolutePosition;
- canCoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond;
- encoder.configAllSettings(canCoderConfiguration);
+ CANcoderConfigurator cfg = encoder.getConfigurator();
+ MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs();
+ cfg.refresh(magnetSensorConfiguration);
+ cfg.apply(magnetSensorConfiguration
+ .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
+ .withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive
+ : SensorDirectionValue.CounterClockwise_Positive));
}
/**
@@ -85,46 +87,38 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
public double getAbsolutePosition()
{
readingError = false;
- MagnetFieldStrength strength = encoder.getMagnetFieldStrength();
+ MagnetHealthValue strength = encoder.getMagnetHealth().getValue();
- if (strength != MagnetFieldStrength.Good_GreenLED)
+ if (strength != MagnetHealthValue.Magnet_Green)
{
DriverStation.reportWarning(
"CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false);
}
- if (strength == MagnetFieldStrength.Invalid_Unknown || strength == MagnetFieldStrength.BadRange_RedLED)
+ if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red)
{
readingError = true;
DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false);
return 0;
}
- double angle = encoder.getAbsolutePosition();
+ StatusSignal angle = encoder.getAbsolutePosition().refresh();
// 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();
for (int i = 0; i < maximumRetries; i++)
{
- if (code == ErrorCode.OK)
+ if (angle.getStatus() == StatusCode.OK)
{
break;
}
- try
- {
- Thread.sleep(10);
- } catch (InterruptedException e)
- {
- }
- angle = encoder.getAbsolutePosition();
- code = encoder.getLastError();
+ angle = angle.waitForUpdate(0.01);
}
- if (code != ErrorCode.OK)
+ if (angle.getStatus() != StatusCode.OK)
{
readingError = true;
DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false);
}
- return angle;
+ return angle.getValue() * 360;
}
/**
@@ -139,16 +133,23 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
}
/**
- * Sets the Absolute Encoder Offset inside of the CANcoder's Memory.
+ * Sets the Absolute Encoder Offset within the CANcoder's Memory.
*
- * @param offset the offset the Absolute Encoder uses as the zero point.
+ * @param offset the offset the Absolute Encoder uses as the zero point in degrees.
* @return if setting Absolute Encoder Offset was successful or not.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
- ErrorCode error = encoder.configMagnetOffset(offset);
- if (error == ErrorCode.OK)
+ CANcoderConfigurator cfg = encoder.getConfigurator();
+ MagnetSensorConfigs magCfg = new MagnetSensorConfigs();
+ StatusCode error = cfg.refresh(magCfg);
+ if (error != StatusCode.OK)
+ {
+ return false;
+ }
+ error = cfg.apply(magCfg.withMagnetOffset(offset / 360));
+ if (error == StatusCode.OK)
{
return true;
}
@@ -165,6 +166,6 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public double getVelocity()
{
- return encoder.getVelocity();
+ return encoder.getVelocity().getValue() * 360;
}
}
diff --git a/swervelib/encoders/CanAndCoderSwerve.java b/swervelib/encoders/CanAndCoderSwerve.java
index 7e7c19d..4b21498 100644
--- a/swervelib/encoders/CanAndCoderSwerve.java
+++ b/swervelib/encoders/CanAndCoderSwerve.java
@@ -1,31 +1,31 @@
package swervelib.encoders;
-import com.reduxrobotics.sensors.canandcoder.CANandcoder;
+import com.reduxrobotics.sensors.canandcoder.Canandcoder;
import edu.wpi.first.wpilibj.DriverStation;
/**
- * HELIUM {@link CANandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus.
+ * HELIUM {@link Canandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus.
*/
public class CanAndCoderSwerve extends SwerveAbsoluteEncoder
{
/**
- * The {@link CANandcoder} representing the CANandCoder on the CAN bus.
+ * The {@link Canandcoder} representing the CANandCoder on the CAN bus.
*/
- public CANandcoder encoder;
+ public Canandcoder encoder;
/**
* Inversion state of the encoder.
*/
private boolean inverted = false;
/**
- * Create the {@link CANandcoder}
+ * Create the {@link Canandcoder}
*
* @param canid The CAN ID whenever the CANandCoder is operating on the CANBus.
*/
public CanAndCoderSwerve(int canid)
{
- encoder = new CANandcoder(canid);
+ encoder = new Canandcoder(canid);
}
/**
diff --git a/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
index 1f79cbf..0508bca 100644
--- a/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
+++ b/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
@@ -2,8 +2,8 @@ package swervelib.encoders;
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
-import com.revrobotics.SparkMaxAnalogSensor;
-import com.revrobotics.SparkMaxAnalogSensor.Mode;
+import com.revrobotics.SparkAnalogSensor;
+import com.revrobotics.SparkAnalogSensor.Mode;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.function.Supplier;
import swervelib.motors.SwerveMotor;
@@ -15,9 +15,9 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
{
/**
- * The {@link SparkMaxAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
+ * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
*/
- public SparkMaxAnalogSensor encoder;
+ public SparkAnalogSensor encoder;
/**
* Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data
diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java
index 87f9782..f95d36d 100644
--- a/swervelib/encoders/SparkMaxEncoderSwerve.java
+++ b/swervelib/encoders/SparkMaxEncoderSwerve.java
@@ -3,7 +3,7 @@ package swervelib.encoders;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
-import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
+import com.revrobotics.SparkAbsoluteEncoder.Type;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.function.Supplier;
import swervelib.motors.SwerveMotor;
diff --git a/swervelib/imu/ADIS16448Swerve.java b/swervelib/imu/ADIS16448Swerve.java
index 683ece0..d60f8c6 100644
--- a/swervelib/imu/ADIS16448Swerve.java
+++ b/swervelib/imu/ADIS16448Swerve.java
@@ -37,9 +37,8 @@ public class ADIS16448Swerve extends SwerveIMU
@Override
public void factoryDefault()
{
- offset = new Rotation3d(
- Math.toRadians(imu.getYComplementaryAngle()), Math.toRadians(imu.getXComplementaryAngle()),
- Math.toRadians(imu.getAngle()));
+ offset = new Rotation3d(0, 0, 0);
+ imu.calibrate();
}
/**
@@ -68,9 +67,9 @@ public class ADIS16448Swerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
- return new Rotation3d(
- Math.toRadians(imu.getYComplementaryAngle()), Math.toRadians(imu.getXComplementaryAngle()),
- Math.toRadians(imu.getAngle()));
+ return new Rotation3d(Math.toRadians(-imu.getGyroAngleX()),
+ Math.toRadians(-imu.getGyroAngleY()),
+ Math.toRadians(-imu.getGyroAngleZ()));
}
/**
diff --git a/swervelib/imu/ADIS16470Swerve.java b/swervelib/imu/ADIS16470Swerve.java
index 0068e4b..5a102bf 100644
--- a/swervelib/imu/ADIS16470Swerve.java
+++ b/swervelib/imu/ADIS16470Swerve.java
@@ -2,6 +2,7 @@ package swervelib.imu;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
@@ -38,9 +39,8 @@ public class ADIS16470Swerve extends SwerveIMU
@Override
public void factoryDefault()
{
- offset = new Rotation3d(
- Math.toRadians(imu.getYComplementaryAngle()), Math.toRadians(imu.getXComplementaryAngle()),
- Math.toRadians(imu.getAngle()));
+ offset = new Rotation3d(0, 0, 0);
+ imu.calibrate();
}
/**
@@ -69,9 +69,7 @@ public class ADIS16470Swerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
- return new Rotation3d(
- Math.toRadians(imu.getYComplementaryAngle()), Math.toRadians(imu.getXComplementaryAngle()),
- Math.toRadians(imu.getAngle()));
+ return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw)));
}
/**
diff --git a/swervelib/imu/ADXRS450Swerve.java b/swervelib/imu/ADXRS450Swerve.java
index ba29264..f313fb0 100644
--- a/swervelib/imu/ADXRS450Swerve.java
+++ b/swervelib/imu/ADXRS450Swerve.java
@@ -37,7 +37,8 @@ public class ADXRS450Swerve extends SwerveIMU
@Override
public void factoryDefault()
{
- offset = new Rotation3d(0, 0, Math.toRadians(imu.getAngle()));
+ imu.calibrate();
+ offset = new Rotation3d(0, 0, 0);//Math.toRadians(-imu.getAngle()));
}
/**
@@ -66,7 +67,7 @@ public class ADXRS450Swerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
- return new Rotation3d(0, 0, Math.toRadians(imu.getAngle()));
+ return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle()));
}
/**
diff --git a/swervelib/imu/AnalogGyroSwerve.java b/swervelib/imu/AnalogGyroSwerve.java
index 124bd36..590666f 100644
--- a/swervelib/imu/AnalogGyroSwerve.java
+++ b/swervelib/imu/AnalogGyroSwerve.java
@@ -44,7 +44,8 @@ public class AnalogGyroSwerve extends SwerveIMU
@Override
public void factoryDefault()
{
- offset = new Rotation3d(0, 0, Math.toRadians(gyro.getAngle()));
+ gyro.calibrate();
+ offset = new Rotation3d(0, 0, 0);
}
/**
@@ -73,7 +74,7 @@ public class AnalogGyroSwerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
- return new Rotation3d(0, 0, Math.toRadians(gyro.getAngle()));
+ return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle()));
}
/**
diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java
index e34bc26..272efaf 100644
--- a/swervelib/imu/NavXSwerve.java
+++ b/swervelib/imu/NavXSwerve.java
@@ -96,10 +96,7 @@ public class NavXSwerve extends SwerveIMU
public void factoryDefault()
{
// gyro.reset(); // Reported to be slow
- offset = new Rotation3d(new Quaternion(gyro.getQuaternionW(),
- gyro.getQuaternionX(),
- gyro.getQuaternionY(),
- gyro.getQuaternionZ()));
+ offset = gyro.getRotation3d();
}
/**
@@ -125,13 +122,10 @@ public class NavXSwerve extends SwerveIMU
*
* @return {@link Rotation3d} from the IMU.
*/
+ @Override
public Rotation3d getRawRotation3d()
{
- return new Rotation3d(new Quaternion(gyro.getQuaternionW() * 0.5,
- gyro.getQuaternionX() * 0.5,
- gyro.getQuaternionY() * 0.5,
- gyro.getQuaternionZ() *
- 0.5)); // TODO: Remove when Studica's official release is made.
+ return gyro.getRotation3d();
}
/**
@@ -142,7 +136,7 @@ public class NavXSwerve extends SwerveIMU
@Override
public Rotation3d getRotation3d()
{
- return getRawRotation3d().minus(offset);
+ return gyro.getRotation3d().minus(offset);
}
/**
diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java
index 086a3b0..61fb6f8 100644
--- a/swervelib/imu/Pigeon2Swerve.java
+++ b/swervelib/imu/Pigeon2Swerve.java
@@ -1,7 +1,9 @@
package swervelib.imu;
-import com.ctre.phoenix.sensors.Pigeon2Configuration;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import com.ctre.phoenix6.configs.Pigeon2Configurator;
+import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
@@ -17,7 +19,7 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Pigeon2 IMU device.
*/
- WPI_Pigeon2 imu;
+ Pigeon2 imu;
/**
* Offset for the Pigeon 2.
*/
@@ -31,9 +33,7 @@ public class Pigeon2Swerve extends SwerveIMU
*/
public Pigeon2Swerve(int canid, String canbus)
{
- imu = new WPI_Pigeon2(canid, canbus);
- Pigeon2Configuration config = new Pigeon2Configuration();
- imu.configAllSettings(config);
+ imu = new Pigeon2(canid, canbus);
SmartDashboard.putData(imu);
}
@@ -53,8 +53,11 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public void factoryDefault()
{
- imu.configFactoryDefault();
- imu.configEnableCompass(false); // Compass utilization causes readings to jump dramatically in some cases.
+ Pigeon2Configurator cfg = imu.getConfigurator();
+ Pigeon2Configuration config = new Pigeon2Configuration();
+
+ // Compass utilization causes readings to jump dramatically in some cases.
+ cfg.apply(config.Pigeon2Features.withEnableCompass(false));
}
/**
@@ -84,9 +87,12 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public Rotation3d getRawRotation3d()
{
- double[] wxyz = new double[4];
- imu.get6dQuaternion(wxyz);
- return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
+ // TODO: Switch to suppliers.
+ StatusSignal w = imu.getQuatW();
+ StatusSignal x = imu.getQuatX();
+ StatusSignal y = imu.getQuatY();
+ StatusSignal z = imu.getQuatZ();
+ return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue()));
}
/**
@@ -109,9 +115,12 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public Optional getAccel()
{
- short[] initial = new short[3];
- imu.getBiasedAccelerometer(initial);
- return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
+ // TODO: Switch to suppliers.
+ StatusSignal xAcc = imu.getAccelerationX();
+ StatusSignal yAcc = imu.getAccelerationX();
+ StatusSignal zAcc = imu.getAccelerationX();
+
+ return Optional.of(new Translation3d(xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()).times(9.81 / 16384.0));
}
/**
diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
index 4ab9a2d..0b4fdd5 100644
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -8,7 +8,6 @@ import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.List;
import swervelib.SwerveController;
import swervelib.SwerveModule;
@@ -214,12 +213,7 @@ public class SwerveMath
}
double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm();
- double maxAccel = 9.81 * horizontalDistance / robotCG.getZ();
- if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
- {
- SmartDashboard.putNumber("calcMaxAccel", maxAccel);
- }
- return maxAccel;
+ return 9.81 * horizontalDistance / robotCG.getZ();
}
/**
@@ -275,18 +269,10 @@ public class SwerveMath
{
// Get the robot's current field-relative velocity
Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity);
- if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
- {
- SmartDashboard.putNumber("currentVelocity", currentVelocity.getX());
- }
// Calculate the commanded change in velocity by subtracting current velocity
// from commanded velocity
Translation2d deltaV = commandedVelocity.minus(currentVelocity);
- if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
- {
- SmartDashboard.putNumber("deltaV", deltaV.getX());
- }
// Creates an acceleration vector with the direction of delta V and a magnitude
// of the maximum allowed acceleration in that direction
diff --git a/swervelib/motors/SparkFlexSwerve.java b/swervelib/motors/SparkFlexSwerve.java
new file mode 100644
index 0000000..a5aef96
--- /dev/null
+++ b/swervelib/motors/SparkFlexSwerve.java
@@ -0,0 +1,447 @@
+package swervelib.motors;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.CANSparkFlex;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
+import com.revrobotics.MotorFeedbackSensor;
+import com.revrobotics.REVLibError;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkAnalogSensor;
+import com.revrobotics.SparkPIDController;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.function.Supplier;
+import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.parser.PIDFConfig;
+import swervelib.telemetry.SwerveDriveTelemetry;
+
+/**
+ * An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}.
+ */
+public class SparkFlexSwerve extends SwerveMotor
+{
+
+ /**
+ * SparkMAX Instance.
+ */
+ public CANSparkFlex motor;
+ /**
+ * Integrated encoder.
+ */
+ public RelativeEncoder encoder;
+ /**
+ * Absolute encoder attached to the SparkMax (if exists)
+ */
+ public SwerveAbsoluteEncoder absoluteEncoder;
+ /**
+ * Closed-loop PID controller.
+ */
+ public SparkPIDController pid;
+ /**
+ * Factory default already occurred.
+ */
+ private boolean factoryDefaultOccurred = false;
+
+ /**
+ * Initialize the swerve motor.
+ *
+ * @param motor The SwerveMotor as a SparkFlex object.
+ * @param isDriveMotor Is the motor being initialized a drive motor?
+ */
+ public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor)
+ {
+ this.motor = motor;
+ this.isDriveMotor = isDriveMotor;
+ factoryDefaults();
+ clearStickyFaults();
+
+ encoder = motor.getEncoder();
+ pid = motor.getPIDController();
+ pid.setFeedbackDevice(
+ encoder); // Configure feedback of the PID controller as the integrated encoder.
+
+ // Spin off configurations in a different thread.
+ // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
+ }
+
+ /**
+ * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
+ *
+ * @param id CAN ID of the SparkMax.
+ * @param isDriveMotor Is the motor being initialized a drive motor?
+ */
+ public SparkFlexSwerve(int id, boolean isDriveMotor)
+ {
+ this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor);
+ }
+
+ /**
+ * Run the configuration until it succeeds or times out.
+ *
+ * @param config Lambda supplier returning the error state.
+ */
+ private void configureSparkFlex(Supplier config)
+ {
+ for (int i = 0; i < maximumRetries; i++)
+ {
+ if (config.get() == REVLibError.kOk)
+ {
+ return;
+ }
+ }
+ DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
+ }
+
+ /**
+ * Set the voltage compensation for the swerve module motor.
+ *
+ * @param nominalVoltage Nominal voltage for operation to output to.
+ */
+ @Override
+ public void setVoltageCompensation(double nominalVoltage)
+ {
+ configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage));
+ }
+
+ /**
+ * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
+ * voltage compensation. This is useful to protect the motor from current spikes.
+ *
+ * @param currentLimit Current limit in AMPS at free speed.
+ */
+ @Override
+ public void setCurrentLimit(int currentLimit)
+ {
+ configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit));
+ }
+
+ /**
+ * Set the maximum rate the open/closed loop output can change by.
+ *
+ * @param rampRate Time in seconds to go from 0 to full throttle.
+ */
+ @Override
+ public void setLoopRampRate(double rampRate)
+ {
+ configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate));
+ configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate));
+ }
+
+ /**
+ * Get the motor object from the module.
+ *
+ * @return Motor object.
+ */
+ @Override
+ public Object getMotor()
+ {
+ return motor;
+ }
+
+ /**
+ * Queries whether the absolute encoder is directly attached to the motor controller.
+ *
+ * @return connected absolute encoder state.
+ */
+ @Override
+ public boolean isAttachedAbsoluteEncoder()
+ {
+ return absoluteEncoder != null;
+ }
+
+ /**
+ * Configure the factory defaults.
+ */
+ @Override
+ public void factoryDefaults()
+ {
+ if (!factoryDefaultOccurred)
+ {
+ configureSparkFlex(motor::restoreFactoryDefaults);
+ factoryDefaultOccurred = true;
+ }
+ }
+
+ /**
+ * Clear the sticky faults on the motor controller.
+ */
+ @Override
+ public void clearStickyFaults()
+ {
+ configureSparkFlex(motor::clearFaults);
+ }
+
+ /**
+ * Set the absolute encoder to be a compatible absolute encoder.
+ *
+ * @param encoder The encoder to use.
+ * @return The {@link SwerveMotor} for easy instantiation.
+ */
+ @Override
+ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
+ {
+ if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
+ {
+ DriverStation.reportWarning(
+ "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" +
+ " absoluteEncoderOffset in the Swerve Module JSON!",
+ false);
+ absoluteEncoder = encoder;
+ configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
+ }
+ return this;
+ }
+
+ /**
+ * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
+ *
+ * @param positionConversionFactor The conversion factor to apply.
+ */
+ @Override
+ public void configureIntegratedEncoder(double positionConversionFactor)
+ {
+ if (absoluteEncoder == null)
+ {
+ configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor));
+ configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
+
+ // 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
+ {
+ configureSparkFlex(() -> {
+ if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+ {
+ return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
+ positionConversionFactor);
+ } else
+ {
+ return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
+ positionConversionFactor);
+ }
+ });
+ configureSparkFlex(() -> {
+ if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+ {
+ return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
+ positionConversionFactor / 60);
+ } else
+ {
+ return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
+ positionConversionFactor / 60);
+ }
+ });
+ }
+ }
+
+ /**
+ * Configure the PIDF values for the closed loop controller.
+ *
+ * @param config Configuration class holding the PIDF values.
+ */
+ @Override
+ public void configurePIDF(PIDFConfig config)
+ {
+// int pidSlot =
+// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ int pidSlot = 0;
+ configureSparkFlex(() -> pid.setP(config.p, pidSlot));
+ configureSparkFlex(() -> pid.setI(config.i, pidSlot));
+ configureSparkFlex(() -> pid.setD(config.d, pidSlot));
+ configureSparkFlex(() -> pid.setFF(config.f, pidSlot));
+ configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot));
+ configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
+ }
+
+ /**
+ * Configure the PID wrapping for the position closed loop controller.
+ *
+ * @param minInput Minimum PID input.
+ * @param maxInput Maximum PID input.
+ */
+ @Override
+ public void configurePIDWrapping(double minInput, double maxInput)
+ {
+ configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true));
+ configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput));
+ configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
+ }
+
+ /**
+ * Set the CAN status frames.
+ *
+ * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
+ * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
+ * @param CANStatus2 Motor Position
+ * @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)
+ {
+ configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
+ configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
+ configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
+ configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
+ configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
+ // TODO: Configure Status Frame 5 and 6 if necessary
+ // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
+ }
+
+ /**
+ * Set the idle mode.
+ *
+ * @param isBrakeMode Set the brake mode.
+ */
+ @Override
+ public void setMotorBrake(boolean isBrakeMode)
+ {
+ configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
+ }
+
+ /**
+ * Set the motor to be inverted.
+ *
+ * @param inverted State of inversion.
+ */
+ @Override
+ public void setInverted(boolean inverted)
+ {
+ motor.setInverted(inverted);
+ }
+
+ /**
+ * Save the configurations from flash to EEPROM.
+ */
+ @Override
+ public void burnFlash()
+ {
+ try
+ {
+ Thread.sleep(200);
+ } catch (Exception e)
+ {
+ }
+ configureSparkFlex(() -> motor.burnFlash());
+ }
+
+ /**
+ * Set the percentage output.
+ *
+ * @param percentOutput percent out for the motor controller.
+ */
+ @Override
+ public void set(double percentOutput)
+ {
+ motor.set(percentOutput);
+ }
+
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in MPS or Angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ */
+ @Override
+ public void setReference(double setpoint, double feedforward)
+ {
+ boolean possibleBurnOutIssue = true;
+// int pidSlot =
+// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ int pidSlot = 0;
+
+ if (isDriveMotor)
+ {
+ configureSparkFlex(() ->
+ pid.setReference(
+ setpoint,
+ ControlType.kVelocity,
+ pidSlot,
+ feedforward));
+ } else
+ {
+ configureSparkFlex(() ->
+ pid.setReference(
+ setpoint,
+ ControlType.kPosition,
+ pidSlot,
+ feedforward));
+ if(SwerveDriveTelemetry.isSimulation)
+ {
+ encoder.setPosition(setpoint);
+ }
+ }
+ }
+
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in meters per second or angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ * @param position Only used on the angle motor, the position of the motor in degrees.
+ */
+ @Override
+ public void setReference(double setpoint, double feedforward, double position)
+ {
+ setReference(setpoint, feedforward);
+ }
+
+ /**
+ * Get the velocity of the integrated encoder.
+ *
+ * @return velocity
+ */
+ @Override
+ public double getVelocity()
+ {
+ return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
+ }
+
+ /**
+ * Get the position of the integrated encoder.
+ *
+ * @return Position
+ */
+ @Override
+ public double getPosition()
+ {
+ return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition();
+ }
+
+ /**
+ * Set the integrated encoder position.
+ *
+ * @param position Integrated encoder position.
+ */
+ @Override
+ public void setPosition(double position)
+ {
+ if (absoluteEncoder == null)
+ {
+ configureSparkFlex(() -> encoder.setPosition(position));
+ }
+ }
+
+ /**
+ * REV Slots for PID configuration.
+ */
+ enum SparkMAX_slotIdx
+ {
+ /**
+ * Slot 1, used for position PID's.
+ */
+ Position,
+ /**
+ * Slot 2, used for velocity PID's.
+ */
+ Velocity,
+ /**
+ * Slot 3, used arbitrarily. (Documentation show simulations).
+ */
+ Simulation
+ }
+}
diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
index ba1add7..fc7d94e 100644
--- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java
+++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
@@ -2,15 +2,15 @@ package swervelib.motors;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMax.ControlType;
-import com.revrobotics.CANSparkMax.IdleMode;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxAlternateEncoder;
-import com.revrobotics.SparkMaxPIDController;
-import com.revrobotics.SparkMaxRelativeEncoder.Type;
+import com.revrobotics.SparkPIDController;
+import com.revrobotics.SparkRelativeEncoder.Type;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
@@ -38,7 +38,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
/**
* Closed-loop PID controller.
*/
- public SparkMaxPIDController pid;
+ public SparkPIDController pid;
/**
* Factory default already occurred.
*/
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index 7ec5c2b..e0aa13b 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -1,20 +1,21 @@
package swervelib.motors;
import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMax.ControlType;
-import com.revrobotics.CANSparkMax.IdleMode;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkMaxAnalogSensor;
-import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.SparkAnalogSensor;
+import com.revrobotics.SparkPIDController;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
+import swervelib.telemetry.SwerveDriveTelemetry;
/**
* An implementation of {@link CANSparkMax} as a {@link SwerveMotor}.
@@ -37,7 +38,7 @@ public class SparkMaxSwerve extends SwerveMotor
/**
* Closed-loop PID controller.
*/
- public SparkMaxPIDController pid;
+ public SparkPIDController pid;
/**
* Factory default already occurred.
*/
@@ -208,7 +209,7 @@ public class SparkMaxSwerve extends SwerveMotor
// 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);
+ configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200);
} else
{
configureSparkMax(() -> {
@@ -218,7 +219,7 @@ public class SparkMaxSwerve extends SwerveMotor
positionConversionFactor);
} else
{
- return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
+ return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
}
});
@@ -229,7 +230,7 @@ public class SparkMaxSwerve extends SwerveMotor
positionConversionFactor / 60);
} else
{
- return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
+ return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
}
});
@@ -247,12 +248,12 @@ public class SparkMaxSwerve extends SwerveMotor
// int pidSlot =
// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
- configureSparkMax(() -> pid.setP(config.p, pidSlot));
- configureSparkMax(() -> pid.setI(config.i, pidSlot));
- configureSparkMax(() -> pid.setD(config.d, pidSlot));
- configureSparkMax(() -> pid.setFF(config.f, pidSlot));
- configureSparkMax(() -> pid.setIZone(config.iz, pidSlot));
- configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
+ configureSparkMax(() -> pid.setP(config.p));
+ configureSparkMax(() -> pid.setI(config.i));
+ configureSparkMax(() -> pid.setD(config.d));
+ configureSparkMax(() -> pid.setFF(config.f));
+ configureSparkMax(() -> pid.setIZone(config.iz));
+ configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max));
}
/**
@@ -277,16 +278,19 @@ public class SparkMaxSwerve extends SwerveMotor
* @param CANStatus2 Motor Position
* @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
* @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
+ * @param CANStatus5 Duty Cycle Absolute Encoder Position, Duty Cycle Absolute Encoder Absolute Angle
+ * @param CANStatus6 Duty Cycle Absolute Encoder Velocity, Duty Cycle Absolute Encoder Frequency
*/
public void configureCANStatusFrames(
- int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
+ int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus5, int CANStatus6)
{
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
- // TODO: Configure Status Frame 5 and 6 if necessary
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, CANStatus5));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus6, CANStatus6));
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
@@ -368,6 +372,10 @@ public class SparkMaxSwerve extends SwerveMotor
ControlType.kPosition,
pidSlot,
feedforward));
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ encoder.setPosition(setpoint);
+ }
}
}
diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java
index 8afc6bf..c292cca 100644
--- a/swervelib/motors/TalonFXSwerve.java
+++ b/swervelib/motors/TalonFXSwerve.java
@@ -1,21 +1,18 @@
package swervelib.motors;
-import com.ctre.phoenix.motorcontrol.DemandType;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import edu.wpi.first.wpilibj.Timer;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
import swervelib.encoders.SwerveAbsoluteEncoder;
-import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
-import swervelib.simulation.ctre.PhysicsSim;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
- * {@link com.ctre.phoenix.motorcontrol.can.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics.
+ * {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics.
*/
public class TalonFXSwerve extends SwerveMotor
{
@@ -23,31 +20,31 @@ public class TalonFXSwerve extends SwerveMotor
/**
* Factory default already occurred.
*/
- private final boolean factoryDefaultOccurred = false;
+ private final boolean factoryDefaultOccurred = false;
/**
* Current TalonFX configuration.
*/
- private final TalonFXConfiguration configuration = new TalonFXConfiguration();
+ private TalonFXConfiguration configuration = new TalonFXConfiguration();
/**
* Whether the absolute encoder is integrated.
*/
- private final boolean absoluteEncoder = false;
+ private final boolean absoluteEncoder = false;
+ /**
+ * Motion magic angle voltage setter.
+ */
+ private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
+// /**
+// * Motion Magic exponential voltage setters.
+// */
+// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0);
+ /**
+ * Velocity voltage setter for controlling drive motor.
+ */
+ private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
/**
* TalonFX motor controller.
*/
- WPI_TalonFX motor;
- /**
- * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees.
- */
- private double positionConversionFactor = 1;
- /**
- * If the TalonFX configuration has changed.
- */
- private boolean configChanged = true;
- /**
- * Nominal voltage default to use with feedforward.
- */
- private double nominalVoltage = 12.0;
+ TalonFX motor;
/**
* Constructor for TalonFX swerve motor.
@@ -55,7 +52,7 @@ public class TalonFXSwerve extends SwerveMotor
* @param motor Motor to use.
* @param isDriveMotor Whether this motor is a drive motor.
*/
- public TalonFXSwerve(WPI_TalonFX motor, boolean isDriveMotor)
+ public TalonFXSwerve(TalonFX motor, boolean isDriveMotor)
{
this.isDriveMotor = isDriveMotor;
this.motor = motor;
@@ -63,10 +60,10 @@ public class TalonFXSwerve extends SwerveMotor
factoryDefaults();
clearStickyFaults();
- if (SwerveDriveTelemetry.isSimulation)
- {
- PhysicsSim.getInstance().addTalonFX(motor, .25, 6800);
- }
+// if (SwerveDriveTelemetry.isSimulation)
+// {
+//// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800);
+// }
}
/**
@@ -78,7 +75,7 @@ public class TalonFXSwerve extends SwerveMotor
*/
public TalonFXSwerve(int id, String canbus, boolean isDriveMotor)
{
- this(new WPI_TalonFX(id, canbus), isDriveMotor);
+ this(new TalonFX(id, canbus), isDriveMotor);
}
/**
@@ -89,7 +86,7 @@ public class TalonFXSwerve extends SwerveMotor
*/
public TalonFXSwerve(int id, boolean isDriveMotor)
{
- this(new WPI_TalonFX(id), isDriveMotor);
+ this(new TalonFX(id), isDriveMotor);
}
/**
@@ -100,10 +97,18 @@ public class TalonFXSwerve extends SwerveMotor
{
if (!factoryDefaultOccurred)
{
- motor.configFactoryDefault();
- motor.setSensorPhase(true);
- motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
- motor.configNeutralDeadband(0.001);
+ TalonFXConfigurator cfg = motor.getConfigurator();
+ configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ configuration.ClosedLoopGeneral.ContinuousWrap = true;
+ cfg.apply(configuration);
+
+ m_angleVoltageSetter.UpdateFreqHz = 0;
+// m_angleVoltageExpoSetter.UpdateFreqHz = 0;
+ m_velocityVoltageSetter.UpdateFreqHz = 0;
+// motor.configFactoryDefault();
+// motor.setSensorPhase(true);
+// motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
+// motor.configNeutralDeadband(0.001);
}
}
@@ -146,7 +151,22 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
- this.positionConversionFactor = positionConversionFactor;
+ TalonFXConfigurator cfg = motor.getConfigurator();
+ cfg.refresh(configuration);
+
+ positionConversionFactor = 1 / positionConversionFactor;
+
+ configuration.MotionMagic = configuration.MotionMagic
+ .withMotionMagicCruiseVelocity(100 / positionConversionFactor)
+ .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100)
+ .withMotionMagicExpo_kV(0.12 * positionConversionFactor)
+ .withMotionMagicExpo_kA(0.1);
+
+ configuration.Feedback = configuration.Feedback
+ .withSensorToMechanismRatio(positionConversionFactor)
+ .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor);
+
+ cfg.apply(configuration);
// Taken from democat's library.
// https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16
configureCANStatusFrames(250);
@@ -159,7 +179,7 @@ public class TalonFXSwerve extends SwerveMotor
*/
public void configureCANStatusFrames(int CANStatus1)
{
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
}
/**
@@ -182,17 +202,17 @@ public class TalonFXSwerve extends SwerveMotor
int CANStatus10, int CANStatus12, int CANStatus13, int CANStatus14,
int CANStatus21, int CANStatusCurrent)
{
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21);
- motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21);
+// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent);
// TODO: Configure Status Frame 2 thru 21 if necessary
// https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods
@@ -206,13 +226,14 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
- configuration.slot0.kP = config.p;
- configuration.slot0.kI = config.i;
- configuration.slot0.kD = config.d;
- configuration.slot0.kF = config.f;
- configuration.slot0.integralZone = config.iz;
- configuration.slot0.closedLoopPeakOutput = config.output.max;
- configChanged = true;
+ TalonFXConfigurator cfg = motor.getConfigurator();
+ cfg.refresh(configuration.Slot0);
+ cfg.apply(configuration.Slot0.withKP(config.p)
+ .withKI(config.i)
+ .withKD(config.d)
+ .withKS(config.f));
+// configuration.slot0.integralZone = config.iz;
+// configuration.slot0.closedLoopPeakOutput = config.output.max;
}
/**
@@ -224,7 +245,10 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
- // Do nothing
+ TalonFXConfigurator cfg = motor.getConfigurator();
+ cfg.refresh(configuration.ClosedLoopGeneral);
+ configuration.ClosedLoopGeneral.ContinuousWrap = true;
+ cfg.apply(configuration.ClosedLoopGeneral);
}
/**
@@ -235,7 +259,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setMotorBrake(boolean isBrakeMode)
{
- motor.setNeutralMode(isBrakeMode ? NeutralMode.Brake : NeutralMode.Coast);
+ motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast);
}
/**
@@ -246,7 +270,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setInverted(boolean inverted)
{
- Timer.delay(1);
+// Timer.delay(1);
motor.setInverted(inverted);
}
@@ -256,11 +280,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void burnFlash()
{
- if (configChanged)
- {
- motor.configAllSettings(configuration, 0);
- configChanged = false;
- }
+ // Do nothing
}
/**
@@ -274,20 +294,6 @@ public class TalonFXSwerve extends SwerveMotor
motor.set(percentOutput);
}
- /**
- * Convert the setpoint into native sensor units.
- *
- * @param setpoint Setpoint to mutate. In meters per second or degrees.
- * @param position Position in degrees, only used on angle motors.
- * @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick.
- */
- public double convertToNativeSensorUnits(double setpoint, double position)
- {
- setpoint =
- isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint);
- return setpoint / positionConversionFactor;
- }
-
/**
* Set the closed loop PID controller reference point.
*
@@ -310,27 +316,17 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward, double position)
{
- if (SwerveDriveTelemetry.isSimulation)
- {
- PhysicsSim.getInstance().run();
- }
-
- burnFlash();
+// if (SwerveDriveTelemetry.isSimulation)
+// {
+// PhysicsSim.getInstance().run();
+// }
if (isDriveMotor)
{
- motor.set(
- TalonFXControlMode.Velocity,
- convertToNativeSensorUnits(setpoint, position),
- DemandType.ArbitraryFeedForward,
- feedforward / nominalVoltage);
+ motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint));
} else
{
- motor.set(
- TalonFXControlMode.Position,
- convertToNativeSensorUnits(setpoint, position),
- DemandType.ArbitraryFeedForward,
- feedforward);
+ motor.setControl(m_angleVoltageSetter.withPosition(setpoint));
}
}
@@ -342,7 +338,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public double getVelocity()
{
- return (motor.getSelectedSensorVelocity() * 10) * positionConversionFactor;
+ return motor.getVelocity().getValue();
}
/**
@@ -353,7 +349,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public double getPosition()
{
- return motor.getSelectedSensorPosition() * positionConversionFactor;
+ return motor.getPosition().getValue();
}
/**
@@ -367,7 +363,8 @@ public class TalonFXSwerve extends SwerveMotor
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
position = position < 0 ? (position % 360) + 360 : position;
- motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 0);
+ TalonFXConfigurator cfg = motor.getConfigurator();
+ cfg.setPosition(position);
}
}
@@ -379,9 +376,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setVoltageCompensation(double nominalVoltage)
{
- configuration.voltageCompSaturation = nominalVoltage;
- configChanged = true;
- this.nominalVoltage = nominalVoltage;
+ // Do not implement
}
/**
@@ -393,9 +388,9 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
- configuration.supplyCurrLimit.currentLimit = currentLimit;
- configuration.supplyCurrLimit.enable = true;
- configChanged = true;
+ TalonFXConfigurator cfg = motor.getConfigurator();
+ cfg.refresh(configuration.CurrentLimits);
+ cfg.apply(configuration.CurrentLimits.withStatorCurrentLimit(currentLimit).withStatorCurrentLimitEnable(true));
}
/**
@@ -406,9 +401,9 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
- configuration.closedloopRamp = rampRate;
- configuration.openloopRamp = rampRate;
- configChanged = true;
+ TalonFXConfigurator cfg = motor.getConfigurator();
+ cfg.refresh(configuration.ClosedLoopRamps);
+ cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate));
}
/**
diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java
index 9a469fa..0290d82 100644
--- a/swervelib/motors/TalonSRXSwerve.java
+++ b/swervelib/motors/TalonSRXSwerve.java
@@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.Timer;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
-import swervelib.simulation.ctre.PhysicsSim;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
@@ -64,10 +63,6 @@ public class TalonSRXSwerve extends SwerveMotor
factoryDefaults();
clearStickyFaults();
- if (SwerveDriveTelemetry.isSimulation)
- {
- PhysicsSim.getInstance().addTalonSRX(motor, .25, 6800);
- }
}
/**
@@ -298,10 +293,6 @@ public class TalonSRXSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward, double position)
{
- if (SwerveDriveTelemetry.isSimulation)
- {
- PhysicsSim.getInstance().run();
- }
burnFlash();
diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java
index e55d693..f0df58a 100644
--- a/swervelib/parser/SwerveDriveConfiguration.java
+++ b/swervelib/parser/SwerveDriveConfiguration.java
@@ -81,4 +81,15 @@ public class SwerveDriveConfiguration
}
return modArr;
}
+
+ /**
+ * Assume the first module is the furthest. Usually front-left.
+ *
+ * @return Drive base radius from center of robot to the farthest wheel in meters.
+ */
+ public double getDriveBaseRadiusMeters()
+ {
+ Translation2d furthestModule = moduleLocationsMeters[0];
+ return Math.abs(Math.sqrt(Math.pow(furthestModule.getX(), 2) + Math.pow(furthestModule.getY(), 2)));
+ }
}
diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java
index 9f130d5..9b3a8aa 100644
--- a/swervelib/parser/json/DeviceJson.java
+++ b/swervelib/parser/json/DeviceJson.java
@@ -1,6 +1,6 @@
package swervelib.parser.json;
-import com.revrobotics.SparkMaxRelativeEncoder.Type;
+import com.revrobotics.SparkRelativeEncoder.Type;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.SPI;
@@ -20,6 +20,7 @@ import swervelib.imu.NavXSwerve;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.PigeonSwerve;
import swervelib.imu.SwerveIMU;
+import swervelib.motors.SparkFlexSwerve;
import swervelib.motors.SparkMaxBrushedMotorSwerve;
import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
@@ -175,6 +176,8 @@ public class DeviceJson
case "neo":
case "sparkmax":
return new SparkMaxSwerve(id, isDriveMotor);
+ case "sparkflex":
+ return new SparkFlexSwerve(id, isDriveMotor);
case "falcon":
case "talonfx":
return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor);
diff --git a/swervelib/simulation/ctre/PhysicsSim.java b/swervelib/simulation/ctre/PhysicsSim.java
deleted file mode 100644
index 1254e99..0000000
--- a/swervelib/simulation/ctre/PhysicsSim.java
+++ /dev/null
@@ -1,172 +0,0 @@
-package swervelib.simulation.ctre;
-
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import com.ctre.phoenix.motorcontrol.can.VictorSPX;
-import java.util.ArrayList;
-
-/**
- * Manages physics simulation for CTRE products.
- */
-public class PhysicsSim
-{
-
- private static PhysicsSim sim;
- private final ArrayList _simProfiles = new ArrayList();
-
- /**
- * Gets the robot simulator instance.
- *
- * @return {@link PhysicsSim} instance.
- */
- public static PhysicsSim getInstance()
- {
- if (sim == null)
- {
- sim = new PhysicsSim();
- }
- return sim;
- }
-
- /* 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.random() % 2 * Math.PI))
- + (max + min) / 2;
- }
-
- static double random(double max)
- {
- return random(0, max);
- }
-
- /**
- * Adds a TalonSRX controller to the simulator.
- *
- * @param talon The TalonSRX device
- * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
- * @param fullVel The maximum motor velocity, in ticks per 100ms
- */
- public void addTalonSRX(TalonSRX talon, final double accelToFullTime, final double fullVel)
- {
- addTalonSRX(talon, accelToFullTime, fullVel, false);
- }
-
- /**
- * Adds a TalonSRX controller to the simulator.
- *
- * @param talon The TalonSRX device
- * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
- * @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)
- {
- if (talon != null)
- {
- TalonSRXSimProfile simTalon =
- new TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase);
- _simProfiles.add(simTalon);
- }
- }
-
- /**
- * Adds a TalonFX controller to the simulator.
- *
- * @param falcon The TalonFX device
- * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
- * @param fullVel The maximum motor velocity, in ticks per 100ms
- */
- public void addTalonFX(TalonFX falcon, final double accelToFullTime, final double fullVel)
- {
- addTalonFX(falcon, accelToFullTime, fullVel, false);
- }
-
- /**
- * Adds a TalonFX controller to the simulator.
- *
- * @param falcon The TalonFX device
- * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
- * @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)
- {
- if (falcon != null)
- {
- TalonFXSimProfile simFalcon =
- new TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase);
- _simProfiles.add(simFalcon);
- }
- }
-
- /**
- * Adds a VictorSPX controller to the simulator.
- *
- * @param victor The VictorSPX device
- */
- public void addVictorSPX(VictorSPX victor)
- {
- if (victor != null)
- {
- VictorSPXSimProfile simVictor = new VictorSPXSimProfile(victor);
- _simProfiles.add(simVictor);
- }
- }
-
- /**
- * Runs the simulator: - enable the robot - simulate sensors
- */
- public void run()
- {
- // Simulate devices
- for (SimProfile simProfile : _simProfiles)
- {
- simProfile.run();
- }
- }
-
- /**
- * Holds information about a simulated device.
- */
- static class SimProfile
- {
-
- private long _lastTime;
- private boolean _running = false;
-
- /**
- * Runs the simulation profile. Implemented by device-specific profiles.
- */
- public void run()
- {
- }
-
- /**
- * Returns the time since last call, in milliseconds.
- */
- protected double getPeriod()
- {
- // set the start time if not yet running
- if (!_running)
- {
- _lastTime = System.nanoTime();
- _running = true;
- }
-
- long now = System.nanoTime();
- final double period = (now - _lastTime) / 1000000.;
- _lastTime = now;
-
- return period;
- }
- }
-}
diff --git a/swervelib/simulation/ctre/TalonFXSimProfile.java b/swervelib/simulation/ctre/TalonFXSimProfile.java
deleted file mode 100644
index 1c57d2e..0000000
--- a/swervelib/simulation/ctre/TalonFXSimProfile.java
+++ /dev/null
@@ -1,92 +0,0 @@
-package swervelib.simulation.ctre;
-
-import static swervelib.simulation.ctre.PhysicsSim.random;
-
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-import swervelib.simulation.ctre.PhysicsSim.SimProfile;
-
-/**
- * Holds information about a simulated TalonFX.
- */
-class TalonFXSimProfile extends SimProfile
-{
-
- private final TalonFX _falcon;
- private final double _accelToFullTime;
- private final double _fullVel;
- private final boolean _sensorPhase;
-
- /** The current position */
- // private double _pos = 0;
- /**
- * The current velocity
- */
- private double _vel = 0;
-
- /**
- * Creates a new simulation profile for a TalonFX device.
- *
- * @param falcon The TalonFX device
- * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
- * @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)
- {
- this._falcon = falcon;
- this._accelToFullTime = accelToFullTime;
- this._fullVel = fullVel;
- this._sensorPhase = sensorPhase;
- }
-
- /**
- * 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.
- */
- public void run()
- {
- final double period = getPeriod();
- final double accelAmount = _fullVel / _accelToFullTime * period / 1000;
-
- /// DEVICE SPEED SIMULATION
-
- double outPerc = _falcon.getSimCollection().getMotorOutputLeadVoltage() / 12;
- if (_sensorPhase)
- {
- outPerc *= -1;
- }
- // Calculate theoretical velocity with some randomness
- double theoreticalVel = outPerc * _fullVel * random(0.95, 1);
- // Simulate motor load
- if (theoreticalVel > _vel + accelAmount)
- {
- _vel += accelAmount;
- } else if (theoreticalVel < _vel - accelAmount)
- {
- _vel -= accelAmount;
- } else
- {
- _vel += 0.9 * (theoreticalVel - _vel);
- }
- // _pos += _vel * period / 100;
-
- /// SET SIM PHYSICS INPUTS
-
- _falcon.getSimCollection().addIntegratedSensorPosition((int) (_vel * period / 100));
- _falcon.getSimCollection().setIntegratedSensorVelocity((int) _vel);
-
- double supplyCurrent = Math.abs(outPerc) * 30 * random(0.95, 1.05);
- double statorCurrent = outPerc == 0 ? 0 : supplyCurrent / Math.abs(outPerc);
- _falcon.getSimCollection().setSupplyCurrent(supplyCurrent);
- _falcon.getSimCollection().setStatorCurrent(statorCurrent);
-
- _falcon.getSimCollection().setBusVoltage(12 - outPerc * outPerc * 3 / 4 * random(0.95, 1.05));
- }
-}
diff --git a/swervelib/simulation/ctre/TalonSRXSimProfile.java b/swervelib/simulation/ctre/TalonSRXSimProfile.java
deleted file mode 100644
index 7b55fc7..0000000
--- a/swervelib/simulation/ctre/TalonSRXSimProfile.java
+++ /dev/null
@@ -1,92 +0,0 @@
-package swervelib.simulation.ctre;
-
-import static swervelib.simulation.ctre.PhysicsSim.random;
-
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import swervelib.simulation.ctre.PhysicsSim.SimProfile;
-
-/**
- * Holds information about a simulated TalonSRX.
- */
-class TalonSRXSimProfile extends SimProfile
-{
-
- private final TalonSRX _talon;
- private final double _accelToFullTime;
- private final double _fullVel;
- private final boolean _sensorPhase;
-
- /** The current position */
- // private double _pos = 0;
- /**
- * The current velocity
- */
- private double _vel = 0;
-
- /**
- * Creates a new simulation profile for a TalonSRX device.
- *
- * @param talon The TalonSRX device
- * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
- * @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)
- {
- this._talon = talon;
- this._accelToFullTime = accelToFullTime;
- this._fullVel = fullVel;
- this._sensorPhase = sensorPhase;
- }
-
- /**
- * 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.
- */
- public void run()
- {
- final double period = getPeriod();
- final double accelAmount = _fullVel / _accelToFullTime * period / 1000;
-
- /// DEVICE SPEED SIMULATION
-
- double outPerc = _talon.getSimCollection().getMotorOutputLeadVoltage() / 12;
- if (_sensorPhase)
- {
- outPerc *= -1;
- }
- // Calculate theoretical velocity with some randomness
- double theoreticalVel = outPerc * _fullVel * random(0.95, 1);
- // Simulate motor load
- if (theoreticalVel > _vel + accelAmount)
- {
- _vel += accelAmount;
- } else if (theoreticalVel < _vel - accelAmount)
- {
- _vel -= accelAmount;
- } else
- {
- _vel += 0.9 * (theoreticalVel - _vel);
- }
- // _pos += _vel * period / 100;
-
- /// SET SIM PHYSICS INPUTS
-
- _talon.getSimCollection().addQuadraturePosition((int) (_vel * period / 100));
- _talon.getSimCollection().setQuadratureVelocity((int) _vel);
-
- double supplyCurrent = Math.abs(outPerc) * 30 * random(0.95, 1.05);
- double statorCurrent = outPerc == 0 ? 0 : supplyCurrent / Math.abs(outPerc);
- _talon.getSimCollection().setSupplyCurrent(supplyCurrent);
- _talon.getSimCollection().setStatorCurrent(statorCurrent);
-
- _talon.getSimCollection().setBusVoltage(12 - outPerc * outPerc * 3 / 4 * random(0.95, 1.05));
- }
-}
diff --git a/swervelib/simulation/ctre/VictorSPXSimProfile.java b/swervelib/simulation/ctre/VictorSPXSimProfile.java
deleted file mode 100644
index a4d1009..0000000
--- a/swervelib/simulation/ctre/VictorSPXSimProfile.java
+++ /dev/null
@@ -1,41 +0,0 @@
-package swervelib.simulation.ctre;
-
-import static swervelib.simulation.ctre.PhysicsSim.random;
-
-import com.ctre.phoenix.motorcontrol.can.VictorSPX;
-import swervelib.simulation.ctre.PhysicsSim.SimProfile;
-
-/**
- * Holds information about a simulated VictorSPX.
- */
-class VictorSPXSimProfile extends SimProfile
-{
-
- public final VictorSPX _victor;
-
- /**
- * Creates a new simulation profile for a VictorSPX device.
- *
- * @param victor The VictorSPX device
- */
- public VictorSPXSimProfile(final VictorSPX victor)
- {
- this._victor = victor;
- }
-
- /**
- * 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.
- */
- public void run()
- {
- // final double period = getPeriod();
-
- // Device voltage simulation
- double outPerc = _victor.getSimCollection().getMotorOutputLeadVoltage() / 12;
- _victor.getSimCollection().setBusVoltage(12 - outPerc * outPerc * 3 / 4 * random(0.95, 1.05));
- }
-}
diff --git a/swervelib/simulation/ctre/package-info.java b/swervelib/simulation/ctre/package-info.java
deleted file mode 100644
index d38353a..0000000
--- a/swervelib/simulation/ctre/package-info.java
+++ /dev/null
@@ -1,4 +0,0 @@
-/**
- * CTRE Physics Simulator.
- */
-package swervelib.simulation.ctre;
