updateValidityPeriod(long validityPeriod)
86 | {
87 | this.validityPeriod = validityPeriod * 1000L;
88 | update();
89 | return this;
90 | }
91 |
92 | /**
93 | * Get the most up to date cached value.
94 | *
95 | * @return {@link T} updated to the latest cached version.
96 | */
97 | public T getValue()
98 | {
99 | if (isStale() || RobotBase.isSimulation())
100 | {
101 | update();
102 | }
103 | return value;
104 | }
105 |
106 | }
107 |
--------------------------------------------------------------------------------
/swervelib/parser/SwerveControllerConfiguration.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser;
2 |
3 | import static swervelib.math.SwerveMath.calculateMaxAngularVelocity;
4 |
5 | /**
6 | * Swerve Controller configuration class which is used to configure {@link swervelib.SwerveController}.
7 | */
8 | public class SwerveControllerConfiguration
9 | {
10 |
11 | /**
12 | * PIDF for the heading of the robot.
13 | */
14 | public final PIDFConfig headingPIDF;
15 | /**
16 | * hypotenuse deadband for the robot angle control joystick.
17 | */
18 | public final double
19 | angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
20 | /**
21 | * Maximum chassis angular velocity in rad/s
22 | */
23 | public double maxAngularVelocity;
24 |
25 | /**
26 | * Construct the swerve controller configuration. Assumes robot is square to fetch maximum angular velocity.
27 | *
28 | * @param driveCfg {@link SwerveDriveConfiguration} to fetch the first module X and Y used to
29 | * calculate the maximum angular velocity.
30 | * @param headingPIDF Heading PIDF configuration.
31 | * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick.
32 | * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you have
33 | * feet per second use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}.
34 | */
35 | public SwerveControllerConfiguration(
36 | SwerveDriveConfiguration driveCfg,
37 | PIDFConfig headingPIDF,
38 | double angleJoyStickRadiusDeadband,
39 | double maxSpeedMPS)
40 | {
41 | this.maxAngularVelocity =
42 | calculateMaxAngularVelocity(
43 | maxSpeedMPS,
44 | Math.abs(driveCfg.moduleLocationsMeters[0].getX()),
45 | Math.abs(driveCfg.moduleLocationsMeters[0].getY()));
46 | this.headingPIDF = headingPIDF;
47 | this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband;
48 | }
49 |
50 | /**
51 | * Construct the swerve controller configuration. Assumes hypotenuse deadband of 0.5 (minimum radius for angle to be
52 | * set on angle joystick is .5 of the controller).
53 | *
54 | * @param driveCfg Drive configuration.
55 | * @param headingPIDF Heading PIDF configuration.
56 | * @param maxSpeedMPS Maximum speed in meters per second for angular velocity, remember if you have feet per second
57 | * use {@link edu.wpi.first.math.util.Units#feetToMeters(double)}.
58 | */
59 | public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF, double maxSpeedMPS)
60 | {
61 | this(driveCfg, headingPIDF, 0.5, maxSpeedMPS);
62 | }
63 | }
64 |
--------------------------------------------------------------------------------
/swervelib/encoders/TalonSRXEncoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import com.ctre.phoenix.motorcontrol.FeedbackDevice;
4 | import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
5 | import swervelib.motors.SwerveMotor;
6 | import swervelib.motors.TalonSRXSwerve;
7 |
8 | /**
9 | * Talon SRX attached absolute encoder.
10 | */
11 | public class TalonSRXEncoderSwerve extends SwerveAbsoluteEncoder
12 | {
13 |
14 | /**
15 | * Multiplying by this converts native Talon SRX units into degrees.
16 | */
17 | private final double degreesPerSensorUnit;
18 | /**
19 | * Reference to a Talon SRX for polling its attached absolute encoder.
20 | */
21 | private final WPI_TalonSRX talon;
22 |
23 | /**
24 | * Creates a {@link TalonSRXEncoderSwerve}.
25 | *
26 | * @param motor motor to poll the sensor from.
27 | * @param feedbackDevice the feedback device the sensor uses e.g. PWM or Analog.
28 | */
29 | public TalonSRXEncoderSwerve(SwerveMotor motor, FeedbackDevice feedbackDevice)
30 | {
31 | if (motor instanceof TalonSRXSwerve talonSRXSwerve)
32 | {
33 | talonSRXSwerve.setSelectedFeedbackDevice(feedbackDevice);
34 | this.talon = (WPI_TalonSRX) talonSRXSwerve.getMotor();
35 | // https://v5.docs.ctr-electronics.com/en/stable/ch14_MCSensor.html#sensor-resolution
36 | degreesPerSensorUnit = switch (feedbackDevice)
37 | {
38 | case Analog -> 360.0 / 1024.0;
39 | default -> 360.0 / 4096.0;
40 | };
41 | } else
42 | {
43 | throw new RuntimeException("Motor given to instantiate TalonSRXEncoder is not a WPI_TalonSRX");
44 | }
45 | }
46 |
47 | @Override
48 | public void close()
49 | {
50 | // TalonSRX encoder gets closed with the motor
51 | // I don't think an encoder getting closed should
52 | // close the entire motor so i will keep this empty
53 | // sparkFlex.close();
54 | }
55 |
56 | @Override
57 | public void factoryDefault()
58 | {
59 | // Handled in TalonSRXSwerve
60 | }
61 |
62 | @Override
63 | public void clearStickyFaults()
64 | {
65 | // Handled in TalonSRXSwerve
66 | }
67 |
68 | @Override
69 | public void configure(boolean inverted)
70 | {
71 | talon.setSensorPhase(inverted);
72 | }
73 |
74 | @Override
75 | public double getAbsolutePosition()
76 | {
77 | return (talon.getSelectedSensorPosition() * degreesPerSensorUnit) % 360;
78 | }
79 |
80 | @Override
81 | public Object getAbsoluteEncoder()
82 | {
83 | return talon;
84 | }
85 |
86 | @Override
87 | public boolean setAbsoluteEncoderOffset(double offset)
88 | {
89 | talon.setSelectedSensorPosition(talon.getSelectedSensorPosition() + offset / degreesPerSensorUnit);
90 | return true;
91 | }
92 |
93 | @Override
94 | public double getVelocity()
95 | {
96 | return talon.getSelectedSensorVelocity() * 10 * degreesPerSensorUnit;
97 | }
98 |
99 | }
100 |
--------------------------------------------------------------------------------
/swervelib/encoders/CanAndMagSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import com.reduxrobotics.sensors.canandmag.Canandmag;
4 | import com.reduxrobotics.sensors.canandmag.CanandmagSettings;
5 |
6 | /**
7 | * HELIUM {@link Canandmag} from ReduxRobotics absolute encoder, attached through the CAN bus.
8 | */
9 | public class CanAndMagSwerve extends SwerveAbsoluteEncoder
10 | {
11 |
12 | /**
13 | * The {@link Canandmag} representing the CANandMag on the CAN bus.
14 | */
15 | public Canandmag encoder;
16 | /**
17 | * The {@link Canandmag} settings object to use.
18 | */
19 | public CanandmagSettings settings;
20 |
21 | /**
22 | * Create the {@link Canandmag}
23 | *
24 | * @param canid The CAN ID whenever the CANandMag is operating on the CANBus.
25 | */
26 | public CanAndMagSwerve(int canid)
27 | {
28 | encoder = new Canandmag(canid);
29 | settings = new CanandmagSettings();
30 | }
31 |
32 | @Override
33 | public void close()
34 | {
35 | encoder.close();
36 | }
37 |
38 | /**
39 | * Reset the encoder to factory defaults.
40 | *
41 | * This will not clear the stored zero offset.
42 | */
43 | @Override
44 | public void factoryDefault()
45 | {
46 | encoder.resetFactoryDefaults(false);
47 | }
48 |
49 | /**
50 | * Clear sticky faults on the encoder.
51 | */
52 | @Override
53 | public void clearStickyFaults()
54 | {
55 | encoder.clearStickyFaults();
56 | }
57 |
58 | /**
59 | * Configure the CANandMag to read from [0, 360) per second.
60 | *
61 | * @param inverted Whether the encoder is inverted.
62 | */
63 | @Override
64 | public void configure(boolean inverted)
65 | {
66 | settings.setInvertDirection(inverted);
67 | encoder.setSettings(settings);
68 | }
69 |
70 | /**
71 | * Get the absolute position of the encoder.
72 | *
73 | * @return Absolute position in degrees from [0, 360).
74 | */
75 | @Override
76 | public double getAbsolutePosition()
77 | {
78 | return encoder.getAbsPosition() * 360;
79 | }
80 |
81 | /**
82 | * Get the instantiated absolute encoder Object.
83 | *
84 | * @return Absolute encoder object.
85 | */
86 | @Override
87 | public Object getAbsoluteEncoder()
88 | {
89 | return encoder;
90 | }
91 |
92 | /**
93 | * Cannot set the offset of the CANandMag.
94 | *
95 | * @param offset the offset the Absolute Encoder uses as the zero point.
96 | * @return true if setting the zero point succeeded, false otherwise
97 | */
98 | @Override
99 | public boolean setAbsoluteEncoderOffset(double offset)
100 | {
101 | settings.setZeroOffset(offset);
102 | return encoder.setSettings(settings);
103 | }
104 |
105 | /**
106 | * Get the velocity in degrees/sec.
107 | *
108 | * @return velocity in degrees/sec.
109 | */
110 | @Override
111 | public double getVelocity()
112 | {
113 | return encoder.getVelocity() * 360;
114 | }
115 | }
116 |
--------------------------------------------------------------------------------
/swervelib/encoders/ThriftyNovaEncoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import swervelib.motors.SwerveMotor;
4 | import swervelib.motors.ThriftyNovaSwerve;
5 |
6 | /**
7 | * Thrifty Nova absolute encoder, attached through the data port.
8 | */
9 | public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder
10 | {
11 |
12 | /**
13 | * The absolute encoder is directly interfaced through the Thrifty Nova motor.
14 | */
15 | protected ThriftyNovaSwerve motor;
16 | /**
17 | * Inversion state of the attached encoder.
18 | */
19 | protected boolean inverted = false;
20 | /**
21 | * Offset of the absolute encoder.
22 | */
23 | protected double offset = 0.0;
24 |
25 | /**
26 | * Create the {@link ThriftyNovaEncoderSwerve} object as an absolute encoder from the {@link ThriftyNovaSwerve}
27 | * motor.
28 | *
29 | * @param motor {@link SwerveMotor} through which to interface with the attached encoder .
30 | */
31 | public ThriftyNovaEncoderSwerve(SwerveMotor motor)
32 | {
33 | this.motor = (ThriftyNovaSwerve) motor;
34 | motor.setAbsoluteEncoder(null);
35 | }
36 |
37 | @Override
38 | public void close()
39 | {
40 | // ThriftyNova encoder gets closed with the motor
41 | // I don't think an encoder getting closed should
42 | // close the entire motor so i will keep this empty
43 | // sparkFlex.close();
44 | }
45 |
46 | /**
47 | * Set factory default.
48 | */
49 | @Override
50 | public void factoryDefault()
51 | {
52 | }
53 |
54 | /**
55 | * Clear sticky faults.
56 | */
57 | @Override
58 | public void clearStickyFaults()
59 | {
60 | }
61 |
62 | /**
63 | * Configure the absolute encoder.
64 | *
65 | * @param inverted Whether the encoder is inverted.
66 | */
67 | @Override
68 | public void configure(boolean inverted)
69 | {
70 | this.inverted = inverted;
71 | }
72 |
73 | /**
74 | * Get the absolute position of the encoder.
75 | *
76 | * @return Absolute position in degrees from [0, 360).
77 | */
78 | @Override
79 | public double getAbsolutePosition()
80 | {
81 | return (motor.getPosition() + offset) * (inverted ? -1.0 : 1.0);
82 | }
83 |
84 | /**
85 | * Get the instantiated absolute encoder Object.
86 | */
87 | @Override
88 | public Object getAbsoluteEncoder()
89 | {
90 | return null;
91 | }
92 |
93 | /**
94 | * Set the absolute encoder offset.
95 | *
96 | * @param offset offset in degrees from [0, 360).
97 | * @return true if successful.
98 | */
99 | @Override
100 | public boolean setAbsoluteEncoderOffset(double offset)
101 | {
102 | this.offset = offset;
103 | return true;
104 | }
105 |
106 | /**
107 | * Get the absolute encoder velocity. WARNING: Angular velocity is generally not measurable at high speeds.
108 | *
109 | * @return Velocity in degrees per second.
110 | */
111 | @Override
112 | public double getVelocity()
113 | {
114 | return motor.getVelocity();
115 | }
116 | }
117 |
--------------------------------------------------------------------------------
/swervelib/encoders/PWMDutyCycleEncoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import edu.wpi.first.wpilibj.Alert;
4 | import edu.wpi.first.wpilibj.Alert.AlertType;
5 | import edu.wpi.first.wpilibj.DutyCycleEncoder;
6 |
7 | /**
8 | * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
9 | * Encoder." attached via a PWM lane.
10 | *
11 | * Credits to
12 | *
13 | * p2reneker25 for building this.
14 | */
15 | public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
16 | {
17 |
18 | /**
19 | * Duty Cycle Encoder.
20 | */
21 | private final DutyCycleEncoder encoder;
22 | /**
23 | * Inversion state.
24 | */
25 | private boolean isInverted;
26 | /**
27 | * An {@link Alert} for if the encoder cannot report accurate velocities.
28 | */
29 | private Alert inaccurateVelocities;
30 | /**
31 | * The Offset in degrees of the PWM absolute encoder.
32 | */
33 | private double offset;
34 |
35 | /**
36 | * Constructor for the PWM duty cycle encoder.
37 | *
38 | * @param pin PWM lane for the encoder.
39 | */
40 | public PWMDutyCycleEncoderSwerve(int pin)
41 | {
42 | encoder = new DutyCycleEncoder(pin);
43 | inaccurateVelocities = new Alert(
44 | "Encoders",
45 | "The PWM Duty Cycle encoder may not report accurate velocities!",
46 | AlertType.kWarning);
47 |
48 | }
49 |
50 | /**
51 | * Configure the inversion state of the encoder.
52 | *
53 | * @param inverted Whether the encoder is inverted.
54 | */
55 | @Override
56 | public void configure(boolean inverted)
57 | {
58 | isInverted = inverted;
59 | }
60 |
61 | /**
62 | * Get the absolute position of the encoder.
63 | *
64 | * @return Absolute position in degrees from [0, 360).
65 | */
66 | @Override
67 | public double getAbsolutePosition()
68 | {
69 | return (isInverted ? -1.0 : 1.0) * ((encoder.get() * 360) - offset);
70 | }
71 |
72 | /**
73 | * Get the encoder object.
74 | *
75 | * @return {@link DutyCycleEncoder} from the class.
76 | */
77 | @Override
78 | public Object getAbsoluteEncoder()
79 | {
80 | return encoder;
81 | }
82 |
83 | /**
84 | * Get the velocity in degrees/sec.
85 | *
86 | * @return velocity in degrees/sec.
87 | */
88 | @Override
89 | public double getVelocity()
90 | {
91 | inaccurateVelocities.set(true);
92 | return encoder.get();
93 | }
94 |
95 | /**
96 | * Reset the encoder to factory defaults.
97 | */
98 | @Override
99 | public void factoryDefault()
100 | {
101 | // Do nothing
102 | }
103 |
104 | /**
105 | * Clear sticky faults on the encoder.
106 | */
107 | @Override
108 | public void clearStickyFaults()
109 | {
110 | // Do nothing
111 | }
112 |
113 |
114 | @Override
115 | public boolean setAbsoluteEncoderOffset(double offset)
116 | {
117 | this.offset = offset;
118 |
119 | return true;
120 | }
121 | }
--------------------------------------------------------------------------------
/swervelib/encoders/DIODutyCycleEncoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import edu.wpi.first.wpilibj.Alert;
4 | import edu.wpi.first.wpilibj.Alert.AlertType;
5 | import edu.wpi.first.wpilibj.DutyCycleEncoder;
6 | import edu.wpi.first.wpilibj.Timer;
7 |
8 | /**
9 | * DutyCycle encoders such as "US Digital MA3 with DIO Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
10 | * Encoder." attached via a DIO lane.
11 | *
12 | * Credits to
13 | *
14 | * p2reneker25 for building this.
15 | */
16 | public class DIODutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
17 | {
18 |
19 | /**
20 | * Duty Cycle Encoder.
21 | */
22 | private final DutyCycleEncoder encoder;
23 | /**
24 | * Inversion state.
25 | */
26 | private boolean isInverted;
27 | /**
28 | * An {@link Alert} for if the encoder cannot report accurate velocities.
29 | */
30 | private Alert inaccurateVelocities;
31 | /**
32 | * The Offset in degrees of the DIO absolute encoder.
33 | */
34 | private double offset;
35 |
36 | /**
37 | * Constructor for the DIO duty cycle encoder.
38 | *
39 | * @param pin DIO lane for the encoder.
40 | */
41 | public DIODutyCycleEncoderSwerve(int pin)
42 | {
43 | encoder = new DutyCycleEncoder(pin);
44 | Timer.delay(2);
45 | inaccurateVelocities = new Alert(
46 | "Encoders",
47 | "The DIO Duty Cycle encoder may not report accurate velocities!",
48 | AlertType.kWarning);
49 |
50 | }
51 |
52 | @Override
53 | public void close()
54 | {
55 | encoder.close();
56 | }
57 |
58 | /**
59 | * Configure the inversion state of the encoder.
60 | *
61 | * @param inverted Whether the encoder is inverted.
62 | */
63 | @Override
64 | public void configure(boolean inverted)
65 | {
66 | isInverted = inverted;
67 | }
68 |
69 | /**
70 | * Get the absolute position of the encoder.
71 | *
72 | * @return Absolute position in degrees from [0, 360).
73 | */
74 | @Override
75 | public double getAbsolutePosition()
76 | {
77 | return (isInverted ? -1.0 : 1.0) * ((encoder.get() * 360) - offset);
78 | }
79 |
80 | /**
81 | * Get the encoder object.
82 | *
83 | * @return {@link DutyCycleEncoder} from the class.
84 | */
85 | @Override
86 | public Object getAbsoluteEncoder()
87 | {
88 | return encoder;
89 | }
90 |
91 | /**
92 | * Get the velocity in degrees/sec.
93 | *
94 | * @return velocity in degrees/sec.
95 | */
96 | @Override
97 | public double getVelocity()
98 | {
99 | inaccurateVelocities.set(true);
100 | return encoder.get();
101 | }
102 |
103 | /**
104 | * Reset the encoder to factory defaults.
105 | */
106 | @Override
107 | public void factoryDefault()
108 | {
109 | // Do nothing
110 | }
111 |
112 | /**
113 | * Clear sticky faults on the encoder.
114 | */
115 | @Override
116 | public void clearStickyFaults()
117 | {
118 | // Do nothing
119 | }
120 |
121 |
122 | @Override
123 | public boolean setAbsoluteEncoderOffset(double offset)
124 | {
125 | this.offset = offset;
126 |
127 | return true;
128 | }
129 | }
--------------------------------------------------------------------------------
/swervelib/simulation/SwerveIMUSimulation.java:
--------------------------------------------------------------------------------
1 | package swervelib.simulation;
2 |
3 | import edu.wpi.first.math.geometry.Pose2d;
4 | import edu.wpi.first.math.geometry.Rotation2d;
5 | import edu.wpi.first.math.geometry.Rotation3d;
6 | import edu.wpi.first.math.geometry.Translation3d;
7 | import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
8 | import edu.wpi.first.math.kinematics.SwerveModuleState;
9 | import edu.wpi.first.wpilibj.smartdashboard.Field2d;
10 | import java.util.Optional;
11 | import org.ironmaple.simulation.drivesims.GyroSimulation;
12 |
13 | /**
14 | * Simulation for {@link swervelib.SwerveDrive} IMU.
15 | */
16 | public class SwerveIMUSimulation
17 | {
18 |
19 | private final GyroSimulation gyroSimulation;
20 |
21 | /**
22 | * Create the swerve drive IMU simulation.
23 | *
24 | * @param gyroSimulation Gyro simulation from MapleSim.
25 | */
26 | public SwerveIMUSimulation(GyroSimulation gyroSimulation)
27 | {
28 | this.gyroSimulation = gyroSimulation;
29 | }
30 |
31 | /**
32 | * Get the estimated angle of the robot.
33 | *
34 | * @return {@link Rotation2d} estimation of the robot.
35 | */
36 | public Rotation2d getYaw()
37 | {
38 | return gyroSimulation.getGyroReading();
39 | }
40 |
41 | /**
42 | * Pitch is not simulated currently, always returns 0.
43 | *
44 | * @return Pitch of the robot as {@link Rotation2d}.
45 | */
46 | public Rotation2d getPitch()
47 | {
48 | return new Rotation2d();
49 | }
50 |
51 | /**
52 | * Roll is not simulated currently, always returns 0.
53 | *
54 | * @return Roll of the robot as {@link Rotation2d}.
55 | */
56 | public Rotation2d getRoll()
57 | {
58 | return new Rotation2d();
59 | }
60 |
61 | /**
62 | * Gets the estimated gyro {@link Rotation3d} of the robot.
63 | *
64 | * @return The heading as a {@link Rotation3d} angle
65 | */
66 | public Rotation3d getGyroRotation3d()
67 | {
68 | return new Rotation3d(0, 0, getYaw().getRadians());
69 | }
70 |
71 | /**
72 | * Fetch the acceleration [x, y, z] from the IMU in m/s/s. If acceleration isn't supported returns empty.
73 | *
74 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
75 | */
76 | public Optional getAccel()
77 | {
78 | return Optional.empty();
79 | }
80 |
81 | /**
82 | * Update the odometry of the simulated {@link swervelib.SwerveDrive} and post the {@link swervelib.SwerveModule}
83 | * states to the {@link Field2d}.
84 | *
85 | * @param kinematics {@link SwerveDriveKinematics} of the swerve drive.
86 | * @param states {@link SwerveModuleState} array of the module states.
87 | * @param modulePoses {@link Pose2d} representing the swerve modules.
88 | * @param field {@link Field2d} to update.
89 | */
90 | public void updateOdometry(
91 | SwerveDriveKinematics kinematics,
92 | SwerveModuleState[] states,
93 | Pose2d[] modulePoses,
94 | Field2d field)
95 | {
96 | field.getObject("XModules").setPoses(modulePoses);
97 | }
98 |
99 | /**
100 | * Set the heading of the robot.
101 | *
102 | * @param angle Angle of the robot in radians.
103 | */
104 | public void setAngle(double angle)
105 | {
106 | this.gyroSimulation.setRotation(Rotation2d.fromRadians(angle));
107 | }
108 | }
109 |
--------------------------------------------------------------------------------
/swervelib/parser/json/PhysicalPropertiesJson.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser.json;
2 |
3 | import static edu.wpi.first.units.Units.Kilogram;
4 | import static edu.wpi.first.units.Units.Pounds;
5 |
6 | import edu.wpi.first.wpilibj.Alert;
7 | import edu.wpi.first.wpilibj.Alert.AlertType;
8 | import swervelib.parser.SwerveModulePhysicalCharacteristics;
9 | import swervelib.parser.json.modules.ConversionFactorsJson;
10 |
11 | /**
12 | * {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the SwerveModule.
13 | */
14 | public class PhysicalPropertiesJson
15 | {
16 |
17 | /**
18 | * DEPRECATED! Use {@link PhysicalPropertiesJson#conversionFactors} instead.
19 | */
20 | @Deprecated(since = "2025", forRemoval = true)
21 | public MotorConfigDouble conversionFactor = new MotorConfigDouble();
22 | /**
23 | * Minimum voltage to spin the module or wheel.
24 | */
25 | public MotorConfigDouble friction = new MotorConfigDouble(0.3, 0.2);
26 | /**
27 | * Steer rotational inertia in KilogramMetersSquare.
28 | */
29 | public double steerRotationalInertia = 0.03;
30 | /**
31 | * Robot mass in lb (pounds)
32 | */
33 | public double robotMass = 110.2311;
34 | /**
35 | * Conversion Factors composition. Auto-calculates the conversion factors.
36 | */
37 | public ConversionFactorsJson conversionFactors = new ConversionFactorsJson();
38 | /**
39 | * The current limit in AMPs to apply to the motors.
40 | */
41 | public MotorConfigInt currentLimit = new MotorConfigInt(40, 20);
42 | /**
43 | * The minimum number of seconds to take for the motor to go from 0 to full throttle.
44 | */
45 | public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25);
46 | /**
47 | * The grip tape coefficient of friction on carpet. Used to calculate the practical maximum acceleration.
48 | */
49 | public double wheelGripCoefficientOfFriction = 1.19;
50 | /**
51 | * The voltage to use for the smart motor voltage compensation, default is 12.
52 | */
53 | public double optimalVoltage = 12;
54 |
55 | /**
56 | * Create the physical characteristics based off the parsed data.
57 | *
58 | * @return {@link SwerveModulePhysicalCharacteristics} based on parsed data.
59 | */
60 | public SwerveModulePhysicalCharacteristics createPhysicalProperties()
61 | {
62 | // Setup deprecation notice.
63 | if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() &&
64 | conversionFactors.isAngleEmpty())
65 | {
66 | throw new RuntimeException(
67 | "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
68 | "} \nis deprecated, please use\n" +
69 | "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
70 | conversionFactor.angle + "} }");
71 | }
72 |
73 | return new SwerveModulePhysicalCharacteristics(
74 | conversionFactors,
75 | wheelGripCoefficientOfFriction,
76 | optimalVoltage,
77 | currentLimit.drive,
78 | currentLimit.angle,
79 | rampRate.drive,
80 | rampRate.angle,
81 | friction.drive,
82 | friction.angle,
83 | steerRotationalInertia,
84 | Pounds.of(robotMass).in(Kilogram));
85 | }
86 | }
87 |
88 |
--------------------------------------------------------------------------------
/swervelib/imu/ADXRS450Swerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 |
5 | import edu.wpi.first.math.geometry.Rotation3d;
6 | import edu.wpi.first.math.geometry.Translation3d;
7 | import edu.wpi.first.units.measure.MutAngularVelocity;
8 | import edu.wpi.first.wpilibj.ADXRS450_Gyro;
9 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
10 | import java.util.Optional;
11 |
12 | /**
13 | * IMU Swerve class for the {@link ADXRS450_Gyro} device.
14 | */
15 | public class ADXRS450Swerve extends SwerveIMU
16 | {
17 |
18 | /**
19 | * {@link ADXRS450_Gyro} device to read the current headings from.
20 | */
21 | private final ADXRS450_Gyro imu;
22 | /**
23 | * Mutable {@link MutAngularVelocity} for readings.
24 | */
25 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
26 | /**
27 | * Offset for the ADXRS450.
28 | */
29 | private Rotation3d offset = new Rotation3d();
30 | /**
31 | * Inversion for the gyro
32 | */
33 | private boolean invertedIMU = false;
34 |
35 | /**
36 | * Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the SmartDashboard.
37 | */
38 | public ADXRS450Swerve()
39 | {
40 | imu = new ADXRS450_Gyro();
41 | factoryDefault();
42 | SmartDashboard.putData(imu);
43 | }
44 |
45 | @Override
46 | public void close() {
47 | imu.close();
48 | }
49 |
50 | /**
51 | * Reset IMU to factory default.
52 | */
53 | @Override
54 | public void factoryDefault()
55 | {
56 | imu.calibrate();
57 | offset = new Rotation3d(0, 0, 0);//Math.toRadians(-imu.getAngle()));
58 | }
59 |
60 | /**
61 | * Clear sticky faults on IMU.
62 | */
63 | @Override
64 | public void clearStickyFaults()
65 | {
66 | // Do nothing.
67 | }
68 |
69 | /**
70 | * Set the gyro offset.
71 | *
72 | * @param offset gyro offset as a {@link Rotation3d}.
73 | */
74 | public void setOffset(Rotation3d offset)
75 | {
76 | this.offset = offset;
77 | }
78 |
79 | /**
80 | * Set the gyro to invert its default direction
81 | *
82 | * @param invertIMU invert gyro direction
83 | */
84 | public void setInverted(boolean invertIMU)
85 | {
86 | invertedIMU = invertIMU;
87 | }
88 |
89 | /**
90 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
91 | *
92 | * @return {@link Rotation3d} from the IMU.
93 | */
94 | public Rotation3d getRawRotation3d()
95 | {
96 | Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle()));
97 | return invertedIMU ? reading.unaryMinus() : reading;
98 | }
99 |
100 | /**
101 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
102 | *
103 | * @return {@link Rotation3d} from the IMU.
104 | */
105 | @Override
106 | public Rotation3d getRotation3d()
107 | {
108 | return getRawRotation3d().rotateBy(offset.unaryMinus());
109 | }
110 |
111 | /**
112 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
113 | * empty.
114 | *
115 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
116 | */
117 | @Override
118 | public Optional getAccel()
119 | {
120 | return Optional.empty();
121 | }
122 |
123 | @Override
124 | public MutAngularVelocity getYawAngularVelocity()
125 | {
126 | return yawVel.mut_setMagnitude(imu.getRate());
127 | }
128 |
129 | /**
130 | * Get the instantiated IMU object.
131 | *
132 | * @return IMU object.
133 | */
134 | @Override
135 | public Object getIMU()
136 | {
137 | return imu;
138 | }
139 | }
140 |
--------------------------------------------------------------------------------
/swervelib/simulation/SwerveModuleSimulation.java:
--------------------------------------------------------------------------------
1 | package swervelib.simulation;
2 |
3 | import static edu.wpi.first.units.Units.Amps;
4 |
5 | import edu.wpi.first.math.geometry.Rotation2d;
6 | import edu.wpi.first.math.kinematics.SwerveModulePosition;
7 | import edu.wpi.first.math.kinematics.SwerveModuleState;
8 | import org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation;
9 | import swervelib.SwerveDrive;
10 | import swervelib.parser.SwerveModulePhysicalCharacteristics;
11 |
12 | /**
13 | * Class that wraps around {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation}
14 | */
15 | public class SwerveModuleSimulation
16 | {
17 |
18 | /**
19 | * MapleSim module.
20 | */
21 | public SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null;
22 |
23 | /**
24 | * Configure the maple sim module
25 | *
26 | * @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for
27 | * simulation
28 | * @param physicalCharacteristics Physical characteristics of the swerve drive from the JSON or built.
29 | */
30 | public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation simModule,
31 | SwerveModulePhysicalCharacteristics physicalCharacteristics)
32 | {
33 | this.mapleSimModule = new SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation(simModule);
34 | this.mapleSimModule.withCurrentLimits(
35 | Amps.of(physicalCharacteristics.driveMotorCurrentLimit),
36 | Amps.of(physicalCharacteristics.angleMotorCurrentLimit));
37 | }
38 |
39 | /**
40 | * Update the position and state of the module. Called from {@link swervelib.SwerveModule#setDesiredState} function
41 | * when simulated.
42 | *
43 | * @param desiredState State the swerve module is set to.
44 | */
45 | public void updateStateAndPosition(SwerveModuleState desiredState)
46 | {
47 | mapleSimModule.runModuleState(desiredState);
48 | }
49 |
50 | /**
51 | * Runs a drive motor characterization on the sim module. This is called from
52 | * {@link swervelib.SwerveDriveTest#runDriveMotorsCharacterizationOnSimModules(SwerveDrive, double, boolean)} to run
53 | * sysId during simulation
54 | *
55 | * @param desiredFacing the desired facing of the module
56 | * @param volts the voltage to run
57 | */
58 | public void runDriveMotorCharacterization(Rotation2d desiredFacing, double volts)
59 | {
60 | mapleSimModule.runDriveMotorCharacterization(desiredFacing, volts);
61 | }
62 |
63 | /**
64 | * Runs a drive motor characterization on the sim module. This method is called from
65 | * {@link swervelib.SwerveDriveTest#runAngleMotorsCharacterizationOnSimModules(SwerveDrive, double)} to run sysId
66 | * during simulation
67 | *
68 | * @param volts the voltage to run
69 | */
70 | public void runAngleMotorCharacterization(double volts)
71 | {
72 | mapleSimModule.runSteerMotorCharacterization(volts);
73 | }
74 |
75 | /**
76 | * Get the simulated swerve module position.
77 | *
78 | * @return {@link SwerveModulePosition} of the simulated module.
79 | */
80 | public SwerveModulePosition getPosition()
81 | {
82 | return mapleSimModule.getModulePosition();
83 | }
84 |
85 | /**
86 | * Get the {@link SwerveModuleState} of the simulated module.
87 | *
88 | * @return {@link SwerveModuleState} of the simulated module.
89 | */
90 | public SwerveModuleState getState()
91 | {
92 | if (mapleSimModule == null)
93 | {
94 | return new SwerveModuleState();
95 | }
96 | SwerveModuleState state = mapleSimModule.getMeasuredState();
97 | state.angle = state.angle.minus(Rotation2d.kZero);
98 | return state;
99 | }
100 | }
101 |
--------------------------------------------------------------------------------
/swervelib/imu/ADIS16470Swerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 |
5 | import edu.wpi.first.math.geometry.Rotation3d;
6 | import edu.wpi.first.math.geometry.Translation3d;
7 | import edu.wpi.first.units.measure.MutAngularVelocity;
8 | import edu.wpi.first.wpilibj.ADIS16470_IMU;
9 | import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
10 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
11 | import java.util.Optional;
12 |
13 | /**
14 | * IMU Swerve class for the {@link ADIS16470_IMU} device.
15 | */
16 | public class ADIS16470Swerve extends SwerveIMU
17 | {
18 |
19 | /**
20 | * {@link ADIS16470_IMU} device to read the current headings from.
21 | */
22 | private final ADIS16470_IMU imu;
23 | /**
24 | * Mutable {@link MutAngularVelocity} for readings.
25 | */
26 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
27 | /**
28 | * Offset for the ADIS16470.
29 | */
30 | private Rotation3d offset = new Rotation3d();
31 | /**
32 | * Inversion for the gyro
33 | */
34 | private boolean invertedIMU = false;
35 |
36 | /**
37 | * Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard.
38 | */
39 | public ADIS16470Swerve()
40 | {
41 | imu = new ADIS16470_IMU();
42 | offset = new Rotation3d();
43 | factoryDefault();
44 | SmartDashboard.putData(imu);
45 | }
46 |
47 | @Override
48 | public void close() {
49 | imu.close();
50 | }
51 |
52 | /**
53 | * Reset IMU to factory default.
54 | */
55 | @Override
56 | public void factoryDefault()
57 | {
58 | offset = new Rotation3d(0, 0, 0);
59 | imu.calibrate();
60 | }
61 |
62 | /**
63 | * Clear sticky faults on IMU.
64 | */
65 | @Override
66 | public void clearStickyFaults()
67 | {
68 | // Do nothing.
69 | }
70 |
71 | /**
72 | * Set the gyro offset.
73 | *
74 | * @param offset gyro offset as a {@link Rotation3d}.
75 | */
76 | public void setOffset(Rotation3d offset)
77 | {
78 | this.offset = offset;
79 | }
80 |
81 | /**
82 | * Set the gyro to invert its default direction
83 | *
84 | * @param invertIMU invert gyro direction
85 | */
86 | public void setInverted(boolean invertIMU)
87 | {
88 | invertedIMU = invertIMU;
89 | }
90 |
91 | /**
92 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
93 | *
94 | * @return {@link Rotation3d} from the IMU.
95 | */
96 | public Rotation3d getRawRotation3d()
97 | {
98 | Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw)));
99 | return invertedIMU ? reading.unaryMinus() : reading;
100 | }
101 |
102 | /**
103 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
104 | *
105 | * @return {@link Rotation3d} from the IMU.
106 | */
107 | @Override
108 | public Rotation3d getRotation3d()
109 | {
110 | return getRawRotation3d().rotateBy(offset.unaryMinus());
111 | }
112 |
113 | /**
114 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
115 | * empty.
116 | *
117 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
118 | */
119 | @Override
120 | public Optional getAccel()
121 | {
122 | return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
123 | }
124 |
125 | @Override
126 | public MutAngularVelocity getYawAngularVelocity()
127 | {
128 | return yawVel.mut_setMagnitude(imu.getRate());
129 | }
130 |
131 | /**
132 | * Get the instantiated IMU object.
133 | *
134 | * @return IMU object.
135 | */
136 | @Override
137 | public Object getIMU()
138 | {
139 | return imu;
140 | }
141 | }
142 |
--------------------------------------------------------------------------------
/swervelib/imu/ADIS16448Swerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 |
5 | import edu.wpi.first.math.geometry.Rotation3d;
6 | import edu.wpi.first.math.geometry.Translation3d;
7 | import edu.wpi.first.units.measure.MutAngularVelocity;
8 | import edu.wpi.first.wpilibj.ADIS16448_IMU;
9 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
10 | import java.util.Optional;
11 |
12 | /**
13 | * IMU Swerve class for the {@link ADIS16448_IMU} device.
14 | */
15 | public class ADIS16448Swerve extends SwerveIMU
16 | {
17 |
18 | /**
19 | * {@link ADIS16448_IMU} device to read the current headings from.
20 | */
21 | private final ADIS16448_IMU imu;
22 | /**
23 | * Mutable {@link MutAngularVelocity} for readings.
24 | */
25 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
26 | /**
27 | * Offset for the ADIS16448.
28 | */
29 | private Rotation3d offset = new Rotation3d();
30 | /**
31 | * Inversion for the gyro
32 | */
33 | private boolean invertedIMU = false;
34 |
35 | /**
36 | * Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard.
37 | */
38 | public ADIS16448Swerve()
39 | {
40 | imu = new ADIS16448_IMU();
41 | factoryDefault();
42 | SmartDashboard.putData(imu);
43 | }
44 |
45 | @Override
46 | public void close() {
47 | imu.close();
48 | }
49 |
50 | /**
51 | * Reset IMU to factory default.
52 | */
53 | @Override
54 | public void factoryDefault()
55 | {
56 | offset = new Rotation3d(0, 0, 0);
57 | imu.calibrate();
58 | }
59 |
60 | /**
61 | * Clear sticky faults on IMU.
62 | */
63 | @Override
64 | public void clearStickyFaults()
65 | {
66 | // Do nothing.
67 | }
68 |
69 | /**
70 | * Set the gyro offset.
71 | *
72 | * @param offset gyro offset as a {@link Rotation3d}.
73 | */
74 | public void setOffset(Rotation3d offset)
75 | {
76 | this.offset = offset;
77 | }
78 |
79 | /**
80 | * Set the gyro to invert its default direction
81 | *
82 | * @param invertIMU invert gyro direction
83 | */
84 | public void setInverted(boolean invertIMU)
85 | {
86 | invertedIMU = invertIMU;
87 | }
88 |
89 | /**
90 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
91 | *
92 | * @return {@link Rotation3d} from the IMU.
93 | */
94 | public Rotation3d getRawRotation3d()
95 | {
96 | Rotation3d reading = new Rotation3d(Math.toRadians(-imu.getGyroAngleX()),
97 | Math.toRadians(-imu.getGyroAngleY()),
98 | Math.toRadians(-imu.getGyroAngleZ()));
99 | return invertedIMU ? reading.unaryMinus() : reading;
100 | }
101 |
102 | /**
103 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
104 | *
105 | * @return {@link Rotation3d} from the IMU.
106 | */
107 | @Override
108 | public Rotation3d getRotation3d()
109 | {
110 | return getRawRotation3d().rotateBy(offset.unaryMinus());
111 | }
112 |
113 | /**
114 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
115 | * empty.
116 | *
117 | * @return {@link Translation3d} of the acceleration.
118 | */
119 | @Override
120 | public Optional getAccel()
121 | {
122 | return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
123 | }
124 |
125 | @Override
126 | public MutAngularVelocity getYawAngularVelocity()
127 | {
128 |
129 | return yawVel.mut_setMagnitude(imu.getRate());
130 | }
131 |
132 | /**
133 | * Get the instantiated IMU object.
134 | *
135 | * @return IMU object.
136 | */
137 | @Override
138 | public Object getIMU()
139 | {
140 | return imu;
141 | }
142 | }
143 |
--------------------------------------------------------------------------------
/swervelib/imu/CanandgyroSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.RotationsPerSecond;
4 |
5 | import com.reduxrobotics.sensors.canandgyro.Canandgyro;
6 | import edu.wpi.first.math.geometry.Rotation3d;
7 | import edu.wpi.first.math.geometry.Translation3d;
8 | import edu.wpi.first.units.measure.MutAngularVelocity;
9 | import java.util.Optional;
10 |
11 | /**
12 | * SwerveIMU interface for the Boron {@link Canandgyro} by Redux Robotics
13 | */
14 | public class CanandgyroSwerve extends SwerveIMU
15 | {
16 |
17 | /**
18 | * Wait time for status frames to show up.
19 | */
20 | public static double STATUS_TIMEOUT_SECONDS = 0.04;
21 | /**
22 | * Boron {@link Canandgyro} by Redux Robotics.
23 | */
24 | private final Canandgyro imu;
25 | /**
26 | * Mutable {@link MutAngularVelocity} for readings.
27 | */
28 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, RotationsPerSecond);
29 | /**
30 | * Offset for the Boron {@link Canandgyro}.
31 | */
32 | private Rotation3d offset = new Rotation3d();
33 | /**
34 | * Inversion for the gyro
35 | */
36 | private boolean invertedIMU = false;
37 |
38 | /**
39 | * Generate the SwerveIMU for {@link Canandgyro}.
40 | *
41 | * @param canid CAN ID for the Boron {@link Canandgyro}
42 | */
43 | public CanandgyroSwerve(int canid)
44 | {
45 | imu = new Canandgyro(canid);
46 | }
47 |
48 | @Override
49 | public void close()
50 | {
51 | imu.close();
52 | }
53 |
54 | /**
55 | * Reset {@link Canandgyro} to factory default.
56 | */
57 | @Override
58 | public void factoryDefault()
59 | {
60 | imu.resetFactoryDefaults(STATUS_TIMEOUT_SECONDS);
61 | }
62 |
63 | /**
64 | * Clear sticky faults on {@link Canandgyro}.
65 | */
66 | @Override
67 | public void clearStickyFaults()
68 | {
69 | imu.clearStickyFaults();
70 | }
71 |
72 | /**
73 | * Set the gyro offset.
74 | *
75 | * @param offset gyro offset as a {@link Rotation3d}.
76 | */
77 | public void setOffset(Rotation3d offset)
78 | {
79 | this.offset = offset;
80 | }
81 |
82 | /**
83 | * Set the gyro to invert its default direction
84 | *
85 | * @param invertIMU invert gyro direction
86 | */
87 | public void setInverted(boolean invertIMU)
88 | {
89 | invertedIMU = invertIMU;
90 | }
91 |
92 | /**
93 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
94 | *
95 | * @return {@link Rotation3d} from the IMU.
96 | */
97 | @Override
98 | public Rotation3d getRawRotation3d()
99 | {
100 | Rotation3d reading = imu.getRotation3d();
101 | return invertedIMU ? reading.unaryMinus() : reading;
102 | }
103 |
104 | /**
105 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
106 | *
107 | * @return {@link Rotation3d} from the IMU.
108 | */
109 | @Override
110 | public Rotation3d getRotation3d()
111 | {
112 | return getRawRotation3d().rotateBy(offset.unaryMinus());
113 | }
114 |
115 | /**
116 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
117 | * empty.
118 | *
119 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
120 | */
121 | @Override
122 | public Optional getAccel()
123 | {
124 |
125 | return Optional.of(new Translation3d(imu.getAccelerationFrame().getValue()).times(9.81 / 16384.0));
126 | }
127 |
128 | @Override
129 | public MutAngularVelocity getYawAngularVelocity()
130 | {
131 | return yawVel.mut_setMagnitude(imu.getAngularVelocityYaw());
132 | }
133 |
134 | /**
135 | * Get the instantiated {@link Canandgyro} IMU object.
136 | *
137 | * @return IMU object.
138 | */
139 | @Override
140 | public Object getIMU()
141 | {
142 | return imu;
143 | }
144 | }
145 |
--------------------------------------------------------------------------------
/swervelib/imu/AnalogGyroSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 |
5 | import edu.wpi.first.math.geometry.Rotation3d;
6 | import edu.wpi.first.math.geometry.Translation3d;
7 | import edu.wpi.first.units.measure.MutAngularVelocity;
8 | import edu.wpi.first.wpilibj.AnalogGyro;
9 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
10 | import java.util.Optional;
11 |
12 | /**
13 | * Creates a IMU for {@link edu.wpi.first.wpilibj.AnalogGyro} devices, only uses yaw.
14 | */
15 | public class AnalogGyroSwerve extends SwerveIMU
16 | {
17 |
18 | /**
19 | * Gyroscope object.
20 | */
21 | private final AnalogGyro imu;
22 | /**
23 | * Mutable {@link MutAngularVelocity} for readings.
24 | */
25 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
26 | /**
27 | * Offset for the analog gyro.
28 | */
29 | private Rotation3d offset = new Rotation3d();
30 | /**
31 | * Inversion for the gyro
32 | */
33 | private boolean invertedIMU = false;
34 |
35 | /**
36 | * Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1.
37 | *
38 | * @param channel Analog port 0 or 1.
39 | */
40 | public AnalogGyroSwerve(int channel)
41 | {
42 | if (!(channel == 0 || channel == 1))
43 | {
44 | throw new RuntimeException(
45 | "Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n");
46 | }
47 | imu = new AnalogGyro(channel);
48 | factoryDefault();
49 | SmartDashboard.putData(imu);
50 | }
51 |
52 | @Override
53 | public void close() {
54 | imu.close();
55 | }
56 |
57 | /**
58 | * Reset IMU to factory default.
59 | */
60 | @Override
61 | public void factoryDefault()
62 | {
63 | imu.calibrate();
64 | offset = new Rotation3d(0, 0, 0);
65 | }
66 |
67 | /**
68 | * Clear sticky faults on IMU.
69 | */
70 | @Override
71 | public void clearStickyFaults()
72 | {
73 | // Do nothing.
74 | }
75 |
76 | /**
77 | * Set the gyro offset.
78 | *
79 | * @param offset gyro offset as a {@link Rotation3d}.
80 | */
81 | public void setOffset(Rotation3d offset)
82 | {
83 | this.offset = offset;
84 | }
85 |
86 | /**
87 | * Set the gyro to invert its default direction
88 | *
89 | * @param invertIMU invert gyro direction
90 | */
91 | public void setInverted(boolean invertIMU)
92 | {
93 | invertedIMU = invertIMU;
94 | }
95 |
96 | /**
97 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
98 | *
99 | * @return {@link Rotation3d} from the IMU.
100 | */
101 | public Rotation3d getRawRotation3d()
102 | {
103 | Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle()));
104 | return invertedIMU ? reading.unaryMinus() : reading;
105 | }
106 |
107 | /**
108 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
109 | *
110 | * @return {@link Rotation3d} from the IMU.
111 | */
112 | @Override
113 | public Rotation3d getRotation3d()
114 | {
115 | return getRawRotation3d().rotateBy(offset.unaryMinus());
116 | }
117 |
118 | /**
119 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
120 | * empty.
121 | *
122 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
123 | */
124 | @Override
125 | public Optional getAccel()
126 | {
127 | return Optional.empty();
128 | }
129 |
130 | @Override
131 | public MutAngularVelocity getYawAngularVelocity()
132 | {
133 | return yawVel.mut_setMagnitude(imu.getRate());
134 | }
135 |
136 | /**
137 | * Get the instantiated IMU object.
138 | *
139 | * @return IMU object.
140 | */
141 | @Override
142 | public Object getIMU()
143 | {
144 | return imu;
145 | }
146 | }
147 |
--------------------------------------------------------------------------------
/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import edu.wpi.first.wpilibj.Alert;
4 | import edu.wpi.first.wpilibj.Alert.AlertType;
5 | import edu.wpi.first.wpilibj.AnalogInput;
6 | import edu.wpi.first.wpilibj.RobotController;
7 |
8 | /**
9 | * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders.
10 | */
11 | public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
12 | {
13 | // Entire class inspired by 5010
14 | // Source: https://github.com/FRC5010/FRCLibrary/blob/main/FRC5010Example2023/src/main/java/frc/robot/FRC5010/sensors/AnalogInput5010.java
15 | /**
16 | * Encoder as Analog Input.
17 | */
18 | public AnalogInput encoder;
19 | /**
20 | * Inversion state of the encoder.
21 | */
22 | private boolean inverted = false;
23 | /**
24 | * An {@link Alert} for if the absolute encoder offset cannot be set.
25 | */
26 | private Alert cannotSetOffset;
27 | /**
28 | * An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities.
29 | */
30 | private Alert inaccurateVelocities;
31 |
32 | /**
33 | * Construct the Thrifty Encoder as a Swerve Absolute Encoder.
34 | *
35 | * @param encoder Encoder to construct.
36 | */
37 | public AnalogAbsoluteEncoderSwerve(AnalogInput encoder)
38 | {
39 | this.encoder = encoder;
40 | cannotSetOffset = new Alert(
41 | "Encoders",
42 | "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(),
43 | AlertType.kWarning);
44 | inaccurateVelocities = new Alert(
45 | "Encoders",
46 | "The Analog Absolute encoder may not report accurate velocities!",
47 | AlertType.kWarning);
48 | }
49 |
50 | @Override
51 | public void close()
52 | {
53 | encoder.close();
54 | }
55 |
56 | /**
57 | * Construct the Encoder given the analog input channel.
58 | *
59 | * @param channel Analog Input channel of which the encoder resides.
60 | */
61 | public AnalogAbsoluteEncoderSwerve(int channel)
62 | {
63 | this(new AnalogInput(channel));
64 | }
65 |
66 | /**
67 | * Reset the encoder to factory defaults.
68 | */
69 | @Override
70 | public void factoryDefault()
71 | {
72 | // Do nothing
73 | }
74 |
75 | /**
76 | * Clear sticky faults on the encoder.
77 | */
78 | @Override
79 | public void clearStickyFaults()
80 | {
81 | // Do nothing
82 | }
83 |
84 | /**
85 | * Configure the absolute encoder to read from [0, 360) per second.
86 | *
87 | * @param inverted Whether the encoder is inverted.
88 | */
89 | @Override
90 | public void configure(boolean inverted)
91 | {
92 | this.inverted = inverted;
93 | }
94 |
95 | /**
96 | * Get the absolute position of the encoder.
97 | *
98 | * @return Absolute position in degrees from [0, 360).
99 | */
100 | @Override
101 | public double getAbsolutePosition()
102 | {
103 | return (inverted ? -1.0 : 1.0) * (encoder.getAverageVoltage() / RobotController.getVoltage5V()) * 360;
104 | }
105 |
106 | /**
107 | * Get the instantiated absolute encoder Object.
108 | *
109 | * @return Absolute encoder object.
110 | */
111 | @Override
112 | public Object getAbsoluteEncoder()
113 | {
114 | return encoder;
115 | }
116 |
117 | /**
118 | * Cannot Set the offset of an Analog Absolute Encoder.
119 | *
120 | * @param offset the offset the Absolute Encoder uses as the zero point.
121 | * @return Will always be false as setting the offset is unsupported of an Analog absolute encoder.
122 | */
123 | @Override
124 | public boolean setAbsoluteEncoderOffset(double offset)
125 | {
126 | //Do Nothing
127 | cannotSetOffset.set(true);
128 | return false;
129 | }
130 |
131 | /**
132 | * Get the velocity in degrees/sec.
133 | *
134 | * @return velocity in degrees/sec.
135 | */
136 | @Override
137 | public double getVelocity()
138 | {
139 | inaccurateVelocities.set(true);
140 | return encoder.getValue() * 360;
141 | }
142 | }
143 |
--------------------------------------------------------------------------------
/swervelib/imu/PigeonSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 |
5 | import com.ctre.phoenix.sensors.WPI_PigeonIMU;
6 | import edu.wpi.first.math.geometry.Quaternion;
7 | import edu.wpi.first.math.geometry.Rotation3d;
8 | import edu.wpi.first.math.geometry.Translation3d;
9 | import edu.wpi.first.units.measure.MutAngularVelocity;
10 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
11 | import java.util.Optional;
12 |
13 | /**
14 | * SwerveIMU interface for the {@link WPI_PigeonIMU}.
15 | */
16 | public class PigeonSwerve extends SwerveIMU
17 | {
18 |
19 | /**
20 | * {@link WPI_PigeonIMU} IMU device.
21 | */
22 | private final WPI_PigeonIMU imu;
23 | /**
24 | * Mutable {@link MutAngularVelocity} for readings.
25 | */
26 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
27 | /**
28 | * Offset for the {@link WPI_PigeonIMU}.
29 | */
30 | private Rotation3d offset = new Rotation3d();
31 | /**
32 | * Inversion for the gyro
33 | */
34 | private boolean invertedIMU = false;
35 |
36 | /**
37 | * Generate the SwerveIMU for {@link WPI_PigeonIMU}.
38 | *
39 | * @param canid CAN ID for the {@link WPI_PigeonIMU}, does not support CANBus.
40 | */
41 | public PigeonSwerve(int canid)
42 | {
43 | imu = new WPI_PigeonIMU(canid);
44 | offset = new Rotation3d();
45 | SmartDashboard.putData(imu);
46 | }
47 |
48 | @Override
49 | public void close() {
50 | imu.close();
51 | }
52 |
53 |
54 | /**
55 | * Reset IMU to factory default.
56 | */
57 | @Override
58 | public void factoryDefault()
59 | {
60 | imu.configFactoryDefault();
61 | }
62 |
63 | /**
64 | * Clear sticky faults on IMU.
65 | */
66 | @Override
67 | public void clearStickyFaults()
68 | {
69 | imu.clearStickyFaults();
70 | }
71 |
72 | /**
73 | * Set the gyro offset.
74 | *
75 | * @param offset gyro offset as a {@link Rotation3d}.
76 | */
77 | public void setOffset(Rotation3d offset)
78 | {
79 | this.offset = offset;
80 | }
81 |
82 | /**
83 | * Set the gyro to invert its default direction
84 | *
85 | * @param invertIMU invert gyro direction
86 | */
87 | public void setInverted(boolean invertIMU)
88 | {
89 | invertedIMU = invertIMU;
90 | }
91 |
92 | /**
93 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
94 | *
95 | * @return {@link Rotation3d} from the IMU.
96 | */
97 | @Override
98 | public Rotation3d getRawRotation3d()
99 | {
100 | double[] wxyz = new double[4];
101 | imu.get6dQuaternion(wxyz);
102 | Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
103 | return invertedIMU ? reading.unaryMinus() : reading;
104 | }
105 |
106 | /**
107 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
108 | *
109 | * @return {@link Rotation3d} from the IMU.
110 | */
111 | @Override
112 | public Rotation3d getRotation3d()
113 | {
114 | return getRawRotation3d().rotateBy(offset.unaryMinus());
115 | }
116 |
117 | /**
118 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
119 | * empty.
120 | *
121 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
122 | */
123 | @Override
124 | public Optional getAccel()
125 | {
126 | short[] initial = new short[3];
127 | imu.getBiasedAccelerometer(initial);
128 | return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
129 | }
130 |
131 | @Override
132 | public MutAngularVelocity getYawAngularVelocity()
133 | {
134 | return yawVel.mut_setMagnitude(imu.getRate());
135 | }
136 |
137 | /**
138 | * Get the instantiated {@link WPI_PigeonIMU} IMU object.
139 | *
140 | * @return IMU object.
141 | */
142 | @Override
143 | public Object getIMU()
144 | {
145 | return imu;
146 | }
147 | }
148 |
--------------------------------------------------------------------------------
/swervelib/imu/PigeonViaTalonSRXSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 |
5 | import com.ctre.phoenix.motorcontrol.can.TalonSRX;
6 | import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
7 | import com.ctre.phoenix.sensors.WPI_PigeonIMU;
8 | import edu.wpi.first.math.geometry.Quaternion;
9 | import edu.wpi.first.math.geometry.Rotation3d;
10 | import edu.wpi.first.math.geometry.Translation3d;
11 | import edu.wpi.first.units.measure.AngularVelocity;
12 | import edu.wpi.first.units.measure.MutAngularVelocity;
13 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
14 | import java.util.Optional;
15 |
16 | /**
17 | * SwerveIMU interface for the {@link WPI_PigeonIMU}.
18 | */
19 | public class PigeonViaTalonSRXSwerve extends SwerveIMU
20 | {
21 |
22 |
23 | /**
24 | * {@link TalonSRX} TalonSRX the IMU is attached to.
25 | */
26 | private final WPI_TalonSRX talon;
27 |
28 | /**
29 | * {@link WPI_PigeonIMU} IMU device.
30 | */
31 | private final WPI_PigeonIMU imu;
32 | /**
33 | * Mutable {@link AngularVelocity} for readings.
34 | */
35 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
36 | /**
37 | * Offset for the {@link WPI_PigeonIMU}.
38 | */
39 | private Rotation3d offset = new Rotation3d();
40 | /**
41 | * Inversion for the gyro
42 | */
43 | private boolean invertedIMU = false;
44 |
45 | /**
46 | * Generate the SwerveIMU for {@link WPI_PigeonIMU} attached to a {@link TalonSRX}.
47 | *
48 | * @param canid CAN ID for the {@link TalonSRX} the {@link WPI_PigeonIMU} is attached to, does not support CANBus.
49 | */
50 | public PigeonViaTalonSRXSwerve(int canid)
51 | {
52 | talon = new WPI_TalonSRX(canid);
53 | imu = new WPI_PigeonIMU(talon);
54 | offset = new Rotation3d();
55 | SmartDashboard.putData(imu);
56 | }
57 |
58 | @Override
59 | public void close() {
60 | imu.close();
61 | talon.close();
62 | }
63 |
64 | /**
65 | * Reset IMU to factory default.
66 | */
67 | @Override
68 | public void factoryDefault()
69 | {
70 | imu.configFactoryDefault();
71 | }
72 |
73 | /**
74 | * Clear sticky faults on IMU.
75 | */
76 | @Override
77 | public void clearStickyFaults()
78 | {
79 | imu.clearStickyFaults();
80 | }
81 |
82 | /**
83 | * Set the gyro offset.
84 | *
85 | * @param offset gyro offset as a {@link Rotation3d}.
86 | */
87 | public void setOffset(Rotation3d offset)
88 | {
89 | this.offset = offset;
90 | }
91 |
92 | /**
93 | * Set the gyro to invert its default direction
94 | *
95 | * @param invertIMU invert gyro direction
96 | */
97 | public void setInverted(boolean invertIMU)
98 | {
99 | invertedIMU = invertIMU;
100 | }
101 |
102 | /**
103 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
104 | *
105 | * @return {@link Rotation3d} from the IMU.
106 | */
107 | @Override
108 | public Rotation3d getRawRotation3d()
109 | {
110 | double[] wxyz = new double[4];
111 | imu.get6dQuaternion(wxyz);
112 | Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
113 | return invertedIMU ? reading.unaryMinus() : reading;
114 | }
115 |
116 | /**
117 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
118 | *
119 | * @return {@link Rotation3d} from the IMU.
120 | */
121 | @Override
122 | public Rotation3d getRotation3d()
123 | {
124 | return getRawRotation3d().rotateBy(offset.unaryMinus());
125 | }
126 |
127 | /**
128 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
129 | * empty.
130 | *
131 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
132 | */
133 | @Override
134 | public Optional getAccel()
135 | {
136 | short[] initial = new short[3];
137 | imu.getBiasedAccelerometer(initial);
138 | return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
139 | }
140 |
141 | @Override
142 | public MutAngularVelocity getYawAngularVelocity()
143 | {
144 | return yawVel.mut_setMagnitude(imu.getRate());
145 | }
146 |
147 | /**
148 | * Get the instantiated {@link WPI_PigeonIMU} IMU object.
149 | *
150 | * @return IMU object.
151 | */
152 | @Override
153 | public Object getIMU()
154 | {
155 | return imu;
156 | }
157 |
158 | }
159 |
--------------------------------------------------------------------------------
/swervelib/imu/NavXSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 |
5 | import com.studica.frc.AHRS;
6 | import com.studica.frc.AHRS.NavXComType;
7 | import edu.wpi.first.math.geometry.Rotation3d;
8 | import edu.wpi.first.math.geometry.Translation3d;
9 | import edu.wpi.first.units.measure.MutAngularVelocity;
10 | import edu.wpi.first.wpilibj.Alert;
11 | import edu.wpi.first.wpilibj.Alert.AlertType;
12 | import java.util.Optional;
13 |
14 | /**
15 | * Communicates with the NavX({@link AHRS}) as the IMU.
16 | */
17 | public class NavXSwerve extends SwerveIMU
18 | {
19 |
20 | /**
21 | * Mutable {@link MutAngularVelocity} for readings.
22 | */
23 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
24 | /**
25 | * NavX IMU.
26 | */
27 | private AHRS imu;
28 | /**
29 | * Offset for the NavX.
30 | */
31 | private Rotation3d offset = new Rotation3d();
32 | /**
33 | * An {@link Alert} for if there is an error instantiating the NavX.
34 | */
35 | private Alert navXError;
36 | /**
37 | * Inversion state of the {@link AHRS}.
38 | */
39 | private boolean inverted = false;
40 |
41 | /**
42 | * Constructor for the NavX({@link AHRS}) swerve.
43 | *
44 | * @param port Serial Port to connect to.
45 | */
46 | public NavXSwerve(NavXComType port)
47 | {
48 | navXError = new Alert("IMU", "Error instantiating NavX.", AlertType.kError);
49 | try
50 | {
51 | /* Communicate w/navX-MXP via the MXP SPI Bus. */
52 | /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */
53 | /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
54 | imu = new AHRS(port);
55 | factoryDefault();
56 | } catch (RuntimeException ex)
57 | {
58 | navXError.setText("Error instantiating NavX: " + ex.getMessage());
59 | navXError.set(true);
60 | }
61 | }
62 |
63 | @Override
64 | public void close()
65 | {
66 | imu.close();
67 | }
68 |
69 | /**
70 | * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be
71 | * too slow.
72 | */
73 | @Override
74 | public void factoryDefault()
75 | {
76 | // gyro.reset(); // Reported to be slow
77 | offset = imu.getRotation3d();
78 | }
79 |
80 | /**
81 | * Clear sticky faults on IMU.
82 | */
83 | @Override
84 | public void clearStickyFaults()
85 | {
86 | }
87 |
88 | /**
89 | * Set the gyro offset.
90 | *
91 | * @param offset gyro offset as a {@link Rotation3d}.
92 | */
93 | public void setOffset(Rotation3d offset)
94 | {
95 | this.offset = offset;
96 | }
97 |
98 | /**
99 | * Set the gyro to invert its default direction
100 | *
101 | * @param invertIMU invert gyro direction
102 | */
103 | public void setInverted(boolean invertIMU)
104 | {
105 | inverted = invertIMU;
106 | // setOffset(getRawRotation3d());
107 | }
108 |
109 | /**
110 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
111 | *
112 | * @return {@link Rotation3d} from the IMU.
113 | */
114 | @Override
115 | public Rotation3d getRawRotation3d()
116 | {
117 | return inverted ? imu.getRotation3d().unaryMinus() : imu.getRotation3d();
118 | }
119 |
120 | /**
121 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
122 | *
123 | * @return {@link Rotation3d} from the IMU.
124 | */
125 | @Override
126 | public Rotation3d getRotation3d()
127 | {
128 | return getRawRotation3d().rotateBy(offset.unaryMinus());
129 | }
130 |
131 | /**
132 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
133 | * empty.
134 | *
135 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
136 | */
137 | @Override
138 | public Optional getAccel()
139 | {
140 | return Optional.of(
141 | new Translation3d(
142 | imu.getWorldLinearAccelX(),
143 | imu.getWorldLinearAccelY(),
144 | imu.getWorldLinearAccelZ())
145 | .times(9.81));
146 | }
147 |
148 | @Override
149 | public MutAngularVelocity getYawAngularVelocity()
150 | {
151 | return yawVel.mut_setMagnitude(imu.getRate());
152 | }
153 |
154 | /**
155 | * Get the instantiated NavX({@link AHRS}) IMU object.
156 | *
157 | * @return IMU object.
158 | */
159 | @Override
160 | public Object getIMU()
161 | {
162 | return imu;
163 | }
164 | }
165 |
--------------------------------------------------------------------------------
/swervelib/encoders/SparkFlexEncoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import com.revrobotics.AbsoluteEncoder;
4 | import com.revrobotics.spark.SparkAbsoluteEncoder;
5 | import com.revrobotics.spark.SparkFlex;
6 | import com.revrobotics.spark.config.SparkFlexConfig;
7 | import edu.wpi.first.wpilibj.Alert;
8 | import edu.wpi.first.wpilibj.Alert.AlertType;
9 | import swervelib.motors.SparkFlexSwerve;
10 | import swervelib.motors.SwerveMotor;
11 |
12 | /**
13 | * SparkFlex absolute encoder, attached through the data port.
14 | */
15 | public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder
16 | {
17 |
18 | /**
19 | * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkFlex.
20 | */
21 | public SparkAbsoluteEncoder encoder;
22 | /**
23 | * An {@link Alert} for if there is a failure configuring the encoder.
24 | */
25 | private Alert failureConfiguring;
26 | /**
27 | * {@link SparkFlexSwerve} instance.
28 | */
29 | private SwerveMotor sparkFlex;
30 |
31 | /**
32 | * Create the {@link SparkFlexEncoderSwerve} object as a duty cycle from the {@link SparkFlex} motor.
33 | *
34 | * @param motor Motor to create the encoder from.
35 | * @param conversionFactor The conversion factor to set if the output is not from 0 to 360.
36 | */
37 | public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor)
38 | {
39 | failureConfiguring = new Alert(
40 | "Encoders",
41 | "Failure configuring SparkFlex Absolute Encoder",
42 | AlertType.kWarning);
43 | if (motor.getMotor() instanceof SparkFlex)
44 | {
45 | sparkFlex = motor;
46 | encoder = ((SparkFlex) motor.getMotor()).getAbsoluteEncoder();
47 | setConversionFactor(conversionFactor);
48 | } else
49 | {
50 | throw new RuntimeException("Motor given to instantiate SparkFlexEncoder is not a CANSparkFlex");
51 | }
52 | }
53 |
54 | @Override
55 | public void close()
56 | {
57 | // SPARK Flex encoder gets closed with the motor
58 | // I don't think an encoder getting closed should
59 | // close the entire motor so i will keep this empty
60 | // sparkFlex.close();
61 | }
62 |
63 | /**
64 | * Reset the encoder to factory defaults.
65 | */
66 | @Override
67 | public void factoryDefault()
68 | {
69 | // Do nothing
70 | }
71 |
72 | /**
73 | * Clear sticky faults on the encoder.
74 | */
75 | @Override
76 | public void clearStickyFaults()
77 | {
78 | // Do nothing
79 | }
80 |
81 | /**
82 | * Configure the absolute encoder to read from [0, 360) per second.
83 | *
84 | * @param inverted Whether the encoder is inverted.
85 | */
86 | @Override
87 | public void configure(boolean inverted)
88 | {
89 | if (sparkFlex instanceof SparkFlexSwerve)
90 | {
91 | SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
92 | cfg.absoluteEncoder.inverted(inverted);
93 | ((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
94 | }
95 | }
96 |
97 | /**
98 | * Set the conversion factor of the {@link SparkFlexEncoderSwerve}.
99 | *
100 | * @param conversionFactor Position conversion factor from ticks to unit.
101 | */
102 | public void setConversionFactor(double conversionFactor)
103 | {
104 | SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
105 | cfg.signals
106 | .absoluteEncoderPositionAlwaysOn(true)
107 | .absoluteEncoderPositionPeriodMs(20);
108 | cfg.absoluteEncoder
109 | .positionConversionFactor(conversionFactor)
110 | .velocityConversionFactor(conversionFactor / 60);
111 | ((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
112 | }
113 |
114 | /**
115 | * Get the absolute position of the encoder.
116 | *
117 | * @return Absolute position in degrees from [0, 360).
118 | */
119 | @Override
120 | public double getAbsolutePosition()
121 | {
122 | return encoder.getPosition();
123 | }
124 |
125 | /**
126 | * Get the instantiated absolute encoder Object.
127 | *
128 | * @return Absolute encoder object.
129 | */
130 | @Override
131 | public Object getAbsoluteEncoder()
132 | {
133 | return encoder;
134 | }
135 |
136 | /**
137 | * Sets the Absolute Encoder Offset inside of the SparkFlex's Memory.
138 | *
139 | * @param offset the offset the Absolute Encoder uses as the zero point.
140 | * @return if setting Absolute Encoder Offset was successful or not.
141 | */
142 | @Override
143 | public boolean setAbsoluteEncoderOffset(double offset)
144 | {
145 | if (sparkFlex instanceof SparkFlexSwerve)
146 | {
147 | SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
148 | cfg.absoluteEncoder.zeroOffset(offset);
149 | ((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
150 | return true;
151 | }
152 | return false;
153 | }
154 |
155 | /**
156 | * Get the velocity in degrees/sec.
157 | *
158 | * @return velocity in degrees/sec.
159 | */
160 | @Override
161 | public double getVelocity()
162 | {
163 | return encoder.getVelocity();
164 | }
165 | }
166 |
--------------------------------------------------------------------------------
/swervelib/parser/SwerveDriveConfiguration.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser;
2 |
3 | import edu.wpi.first.math.geometry.Translation2d;
4 | import edu.wpi.first.math.system.plant.DCMotor;
5 | import java.util.function.Supplier;
6 | import org.ironmaple.simulation.drivesims.COTS;
7 | import org.ironmaple.simulation.drivesims.GyroSimulation;
8 | import swervelib.SwerveModule;
9 | import swervelib.imu.NavXSwerve;
10 | import swervelib.imu.Pigeon2Swerve;
11 | import swervelib.imu.SwerveIMU;
12 | import swervelib.math.SwerveMath;
13 |
14 | /**
15 | * Swerve drive configurations used during SwerveDrive construction.
16 | */
17 | public class SwerveDriveConfiguration
18 | {
19 |
20 | /**
21 | * Number of modules on the robot.
22 | */
23 | public final int moduleCount;
24 | /**
25 | * Swerve Module locations.
26 | */
27 | public Translation2d[] moduleLocationsMeters;
28 | /**
29 | * Swerve IMU
30 | */
31 | public SwerveIMU imu;
32 | /**
33 | * Swerve Modules.
34 | */
35 | public SwerveModule[] modules;
36 | /**
37 | * Physical characteristics of the swerve drive from physicalproperties.json.
38 | */
39 | public SwerveModulePhysicalCharacteristics physicalCharacteristics;
40 |
41 | /**
42 | * Create swerve drive configuration.
43 | *
44 | * @param moduleConfigs Module configuration.
45 | * @param swerveIMU Swerve IMU.
46 | * @param invertedIMU Invert the IMU.
47 | * @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} to store in association with self.
48 | */
49 | public SwerveDriveConfiguration(
50 | SwerveModuleConfiguration[] moduleConfigs,
51 | SwerveIMU swerveIMU,
52 | boolean invertedIMU,
53 | SwerveModulePhysicalCharacteristics physicalCharacteristics)
54 | {
55 | this.moduleCount = moduleConfigs.length;
56 | this.imu = swerveIMU;
57 | swerveIMU.setInverted(invertedIMU);
58 | this.modules = createModules(moduleConfigs);
59 | this.moduleLocationsMeters = new Translation2d[moduleConfigs.length];
60 | for (SwerveModule module : modules)
61 | {
62 | this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation;
63 | }
64 | this.physicalCharacteristics = physicalCharacteristics;
65 | }
66 |
67 | /**
68 | * Create modules based off of the SwerveModuleConfiguration.
69 | *
70 | * @param swerves Swerve constants.
71 | * @return Swerve Modules.
72 | */
73 | public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves)
74 | {
75 | SwerveModule[] modArr = new SwerveModule[swerves.length];
76 | for (int i = 0; i < swerves.length; i++)
77 | {
78 | modArr[i] = new SwerveModule(i, swerves[i]);
79 | }
80 | return modArr;
81 | }
82 |
83 | /**
84 | * Calculate the Drive Base Radius
85 | *
86 | * @return Drive base radius from center of robot to the farthest wheel in meters.
87 | */
88 | public double getDriveBaseRadiusMeters()
89 | {
90 | Translation2d centerOfModules = moduleLocationsMeters[0];
91 |
92 | //Calculate the Center by adding all module offsets together.
93 | for (int i = 1; i < moduleLocationsMeters.length; i++)
94 | {
95 | centerOfModules = centerOfModules.plus(moduleLocationsMeters[i]);
96 | }
97 |
98 | //Return Largest Radius
99 | return centerOfModules.getDistance(moduleLocationsMeters[0]);
100 | }
101 |
102 | /**
103 | * Get the trackwidth of the swerve modules.
104 | *
105 | * @return Effective trackwdtih in Meters
106 | */
107 | public double getTrackwidth()
108 | {
109 | SwerveModuleConfiguration fr = SwerveMath.getSwerveModule(modules, true, false);
110 | SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
111 | return fr.moduleLocation.getDistance(fl.moduleLocation);
112 | }
113 |
114 | /**
115 | * Get the tracklength of the swerve modules.
116 | *
117 | * @return Effective tracklength in Meters
118 | */
119 | public double getTracklength()
120 | {
121 | SwerveModuleConfiguration br = SwerveMath.getSwerveModule(modules, false, false);
122 | SwerveModuleConfiguration bl = SwerveMath.getSwerveModule(modules, false, true);
123 | return br.moduleLocation.getDistance(bl.moduleLocation);
124 | }
125 |
126 | /**
127 | * Get the {@link DCMotor} corresponding to the first module's configuration.
128 | *
129 | * @return {@link DCMotor} of the drive motor.
130 | */
131 | public DCMotor getDriveMotorSim()
132 | {
133 | SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
134 | return fl.driveMotor.getSimMotor();
135 | }
136 |
137 | /**
138 | * Get the {@link DCMotor} corresponding to the first module configuration.
139 | *
140 | * @return {@link DCMotor} of the angle motor.
141 | */
142 | public DCMotor getAngleMotorSim()
143 | {
144 | SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
145 | return fl.angleMotor.getSimMotor();
146 | }
147 |
148 | /**
149 | * Get the gyro simulation for the robot.
150 | *
151 | * @return {@link GyroSimulation} gyro simulation.
152 | */
153 | public Supplier getGyroSim()
154 | {
155 | if (imu instanceof Pigeon2Swerve)
156 | {
157 | return COTS.ofPigeon2();
158 | } else if (imu instanceof NavXSwerve)
159 | {
160 | return COTS.ofNav2X();
161 | }
162 | return COTS.ofGenericGyro();
163 | }
164 |
165 | }
166 |
--------------------------------------------------------------------------------
/swervelib/imu/Pigeon2Swerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 |
5 | import com.ctre.phoenix6.StatusSignal;
6 | import com.ctre.phoenix6.configs.Pigeon2Configuration;
7 | import com.ctre.phoenix6.configs.Pigeon2Configurator;
8 | import com.ctre.phoenix6.hardware.Pigeon2;
9 | import edu.wpi.first.math.geometry.Rotation3d;
10 | import edu.wpi.first.math.geometry.Translation3d;
11 | import edu.wpi.first.units.measure.LinearAcceleration;
12 | import edu.wpi.first.units.measure.MutAngularVelocity;
13 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
14 | import java.util.Optional;
15 | import java.util.function.Supplier;
16 |
17 | /**
18 | * SwerveIMU interface for the {@link Pigeon2}
19 | */
20 | public class Pigeon2Swerve extends SwerveIMU
21 | {
22 |
23 | /**
24 | * Wait time for status frames to show up.
25 | */
26 | public static double STATUS_TIMEOUT_SECONDS = 0.04;
27 | /**
28 | * {@link Pigeon2} IMU device.
29 | */
30 | private final Pigeon2 imu;
31 | /**
32 | * Mutable {@link MutAngularVelocity} for readings.
33 | */
34 | private final MutAngularVelocity yawVel = new MutAngularVelocity(0,
35 | 0,
36 | DegreesPerSecond);
37 | /**
38 | * X Acceleration supplier
39 | */
40 | private final Supplier> xAcc;
41 | /**
42 | * Y Accelleration supplier.
43 | */
44 | private final Supplier> yAcc;
45 | /**
46 | * Z Acceleration supplier.
47 | */
48 | private final Supplier> zAcc;
49 | /**
50 | * Offset for the {@link Pigeon2}.
51 | */
52 | private Rotation3d offset = new Rotation3d();
53 | /**
54 | * Inversion for the gyro
55 | */
56 | private boolean invertedIMU = false;
57 | /**
58 | * {@link Pigeon2} configurator.
59 | */
60 | private Pigeon2Configurator cfg;
61 |
62 | /**
63 | * Generate the SwerveIMU for {@link Pigeon2}.
64 | *
65 | * @param canid CAN ID for the {@link Pigeon2}
66 | * @param canbus CAN Bus name the {@link Pigeon2} resides on.
67 | */
68 | public Pigeon2Swerve(int canid, String canbus)
69 | {
70 | imu = new Pigeon2(canid, canbus);
71 | this.cfg = imu.getConfigurator();
72 | xAcc = imu::getAccelerationX;
73 | yAcc = imu::getAccelerationY;
74 | zAcc = imu::getAccelerationZ;
75 | SmartDashboard.putData(imu);
76 | }
77 |
78 | /**
79 | * Generate the SwerveIMU for {@link Pigeon2}.
80 | *
81 | * @param canid CAN ID for the {@link Pigeon2}
82 | */
83 | public Pigeon2Swerve(int canid)
84 | {
85 | this(canid, "");
86 | }
87 |
88 | @Override
89 | public void close() {
90 | imu.close();
91 | }
92 |
93 |
94 | /**
95 | * Reset {@link Pigeon2} to factory default.
96 | */
97 | @Override
98 | public void factoryDefault()
99 | {
100 | Pigeon2Configuration config = new Pigeon2Configuration();
101 |
102 | // Compass utilization causes readings to jump dramatically in some cases.
103 | cfg.apply(config.Pigeon2Features.withEnableCompass(false));
104 | }
105 |
106 | /**
107 | * Clear sticky faults on {@link Pigeon2}.
108 | */
109 | @Override
110 | public void clearStickyFaults()
111 | {
112 | imu.clearStickyFaults();
113 | }
114 |
115 | /**
116 | * Set the gyro offset.
117 | *
118 | * @param offset gyro offset as a {@link Rotation3d}.
119 | */
120 | public void setOffset(Rotation3d offset)
121 | {
122 | this.offset = offset;
123 | }
124 |
125 | /**
126 | * Set the gyro to invert its default direction
127 | *
128 | * @param invertIMU invert gyro direction
129 | */
130 | public void setInverted(boolean invertIMU)
131 | {
132 | invertedIMU = invertIMU;
133 | }
134 |
135 | /**
136 | * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
137 | *
138 | * @return {@link Rotation3d} from the IMU.
139 | */
140 | @Override
141 | public Rotation3d getRawRotation3d()
142 | {
143 | Rotation3d reading = imu.getRotation3d();
144 | return invertedIMU ? reading.unaryMinus() : reading;
145 | }
146 |
147 | /**
148 | * Fetch the {@link Rotation3d} from the IMU. Robot relative.
149 | *
150 | * @return {@link Rotation3d} from the IMU.
151 | */
152 | @Override
153 | public Rotation3d getRotation3d()
154 | {
155 | return getRawRotation3d().rotateBy(offset.unaryMinus());
156 | }
157 |
158 |
159 | /**
160 | * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
161 | * empty.
162 | *
163 | * @return {@link Translation3d} of the acceleration as an {@link Optional}.
164 | */
165 | @Override
166 | public Optional getAccel()
167 | {
168 | return Optional.of(new Translation3d(xAcc.get().getValueAsDouble(),
169 | yAcc.get().getValueAsDouble(),
170 | zAcc.get().getValueAsDouble()));
171 | }
172 |
173 | @Override
174 | public MutAngularVelocity getYawAngularVelocity()
175 | {
176 | return yawVel.mut_replace(imu.getAngularVelocityZWorld().refresh().getValue());
177 | }
178 |
179 | /**
180 | * Get the instantiated {@link Pigeon2} object.
181 | *
182 | * @return IMU object.
183 | */
184 | @Override
185 | public Object getIMU()
186 | {
187 | return imu;
188 | }
189 | }
190 |
--------------------------------------------------------------------------------
/swervelib/imu/IMUVelocity.java:
--------------------------------------------------------------------------------
1 | package swervelib.imu;
2 |
3 | import edu.wpi.first.hal.HALUtil;
4 | import edu.wpi.first.math.geometry.Rotation2d;
5 | import edu.wpi.first.wpilibj.Notifier;
6 | import swervelib.math.IMULinearMovingAverageFilter;
7 |
8 | /**
9 | * Generic IMU Velocity filter.
10 | */
11 | public class IMUVelocity
12 | {
13 |
14 | /**
15 | * Swerve IMU.
16 | */
17 | private final SwerveIMU gyro;
18 | /**
19 | * Linear filter used to calculate velocity, we use a custom filter class to prevent unwanted operations.
20 | */
21 | private final IMULinearMovingAverageFilter velocityFilter;
22 | /**
23 | * WPILib {@link Notifier} to keep IMU velocity up to date.
24 | */
25 | private final Notifier notifier;
26 |
27 | /**
28 | * Prevents calculation when no previous measurement exists.
29 | */
30 | private boolean firstCycle = true;
31 | /**
32 | * Tracks the previous loop's recorded time.
33 | */
34 | private double timestamp = 0.0;
35 | /**
36 | * Tracks the previous loop's position as a Rotation2d.
37 | */
38 | private Rotation2d position = new Rotation2d();
39 | /**
40 | * The calculated velocity of the robot based on averaged IMU measurements.
41 | */
42 | private double velocity = 0.0;
43 |
44 | /**
45 | * Constructor for the IMU Velocity.
46 | *
47 | * @param gyro The SwerveIMU gyro.
48 | * @param periodSeconds The rate to collect measurements from the gyro, in the form (1/number of samples per second),
49 | * make sure this does not exceed the update rate of your IMU.
50 | * @param averagingTaps The number of samples to used for the moving average linear filter. Higher values will not
51 | * allow the system to update to changes in velocity, lower values may create a less smooth
52 | * signal. Expected taps will probably be ~2-8, with the goal of having the lowest smooth value.
53 | */
54 | public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps)
55 | {
56 | this.gyro = gyro;
57 | velocityFilter = new IMULinearMovingAverageFilter(averagingTaps);
58 | notifier = new Notifier(this::update);
59 | notifier.startPeriodic(periodSeconds);
60 | timestamp = HALUtil.getFPGATime();
61 | }
62 |
63 | /**
64 | * Static factory for IMU Velocity. Supported IMU rates will be as quick as possible but will not exceed 100hz and
65 | * will use 5 taps (supported IMUs are listed in swervelib's IMU folder). Other gyroscopes will default to 50hz and 5
66 | * taps. For custom rates please use the IMUVelocity constructor.
67 | *
68 | * @param gyro The SwerveIMU gyro.
69 | * @return {@link IMUVelocity} for the given gyro with adjusted period readings for velocity.
70 | */
71 | public static IMUVelocity createIMUVelocity(SwerveIMU gyro)
72 | {
73 | double desiredNotifierPeriod = 1.0 / 50.0;
74 | // ADIS16448_IMU ~200HZ:
75 | // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277
76 | if (gyro instanceof ADIS16448Swerve)
77 | {
78 | desiredNotifierPeriod = 1.0 / 100.0;
79 | }
80 | // ADIS16470_IMU 200HZ
81 | // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345
82 | else if (gyro instanceof ADIS16470Swerve)
83 | {
84 | desiredNotifierPeriod = 1.0 / 100.0;
85 | }
86 | // ADXRS450_Gyro 2000HZ?
87 | // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31
88 | else if (gyro instanceof ADXRS450Swerve)
89 | {
90 | desiredNotifierPeriod = 1.0 / 100.0;
91 | }
92 | // NAX (AHRS): 60HZ
93 | // https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61
94 | else if (gyro instanceof NavXSwerve)
95 | {
96 | desiredNotifierPeriod = 1.0 / 60.0;
97 | }
98 | // Pigeon2 100HZ
99 | // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf
100 | else if (gyro instanceof Pigeon2Swerve)
101 | {
102 | desiredNotifierPeriod = 1.0 / 100.0;
103 | }
104 | // Pigeon 100HZ
105 | // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf
106 | else if (gyro instanceof PigeonSwerve)
107 | {
108 | desiredNotifierPeriod = 1.0 / 100.0;
109 | }
110 | return new IMUVelocity(gyro, desiredNotifierPeriod, 5);
111 | }
112 |
113 | /**
114 | * Update the robot's rotational velocity based on the current gyro position.
115 | */
116 | private void update()
117 | {
118 | double newTimestamp = HALUtil.getFPGATime();
119 | Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ());
120 |
121 | synchronized (this)
122 | {
123 | if (!firstCycle)
124 | {
125 | velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp));
126 | }
127 | firstCycle = false;
128 | timestamp = newTimestamp;
129 | position = newPosition;
130 | }
131 | }
132 |
133 | /**
134 | * Get the robot's angular velocity based on averaged meaasurements from the IMU. Velocity is multiplied by 1e+6
135 | * (1,000,000) because the timestamps are in microseconds.
136 | *
137 | * @return robot's angular velocity in rads/s as a double.
138 | */
139 | public synchronized double getVelocity()
140 | {
141 | velocity = velocityFilter.calculate();
142 | return velocity * 1e+6;
143 | }
144 | }
145 |
--------------------------------------------------------------------------------
/swervelib/parser/SwerveModulePhysicalCharacteristics.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser;
2 |
3 | import swervelib.parser.json.modules.ConversionFactorsJson;
4 |
5 | /**
6 | * Configuration class which stores physical characteristics shared between every swerve module.
7 | */
8 | public class SwerveModulePhysicalCharacteristics
9 | {
10 |
11 | /**
12 | * Current limits for the Swerve Module.
13 | */
14 | public final int driveMotorCurrentLimit, angleMotorCurrentLimit;
15 | /**
16 | * The time it takes for the motor to go from 0 to full throttle in seconds.
17 | */
18 | public final double driveMotorRampRate, angleMotorRampRate;
19 | /**
20 | * The minimum voltage to spin the module or wheel.
21 | */
22 | public final double driveFrictionVoltage, angleFrictionVoltage;
23 | /**
24 | * Wheel grip tape coefficient of friction on carpet, as described by the vendor.
25 | */
26 | public final double wheelGripCoefficientOfFriction;
27 | /**
28 | * Steer rotational inertia in (KilogramSquareMeters) kg/m_sq.
29 | */
30 | public final double steerRotationalInertia;
31 | /**
32 | * Robot mass in Kilograms.
33 | */
34 | public final double robotMassKg;
35 | /**
36 | * The voltage to use for the smart motor voltage compensation.
37 | */
38 | public double optimalVoltage;
39 | /**
40 | * The conversion factors for the drive and angle motors, created by
41 | * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and
42 | * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}.
43 | */
44 | public ConversionFactorsJson conversionFactor;
45 |
46 | /**
47 | * Construct the swerve module physical characteristics.
48 | *
49 | * @param conversionFactors The conversion factors for the drive and angle motors, created by
50 | * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double,
51 | * double)} and
52 | * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double,
53 | * double)}.
54 | * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction on carpet given by manufacturer.
55 | * @param optimalVoltage Optimal robot voltage.
56 | * @param driveMotorCurrentLimit Current limit for the drive motor.
57 | * @param angleMotorCurrentLimit Current limit for the angle motor.
58 | * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents
59 | * over drawing power from battery)
60 | * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents
61 | * overdrawing power and power loss).
62 | * @param angleFrictionVoltage Angle motor minimum voltage.
63 | * @param driveFrictionVoltage Drive motor minimum voltage.
64 | * @param steerRotationalInertia Steering rotational inertia in KilogramSquareMeters.
65 | * @param robotMassKg Robot mass in kG.
66 | */
67 | public SwerveModulePhysicalCharacteristics(
68 | ConversionFactorsJson conversionFactors,
69 | double wheelGripCoefficientOfFriction,
70 | double optimalVoltage,
71 | int driveMotorCurrentLimit,
72 | int angleMotorCurrentLimit,
73 | double driveMotorRampRate,
74 | double angleMotorRampRate,
75 | double driveFrictionVoltage,
76 | double angleFrictionVoltage,
77 | double steerRotationalInertia,
78 | double robotMassKg)
79 | {
80 | this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
81 | this.optimalVoltage = optimalVoltage;
82 |
83 | this.conversionFactor = conversionFactors;
84 | // Set the conversion factors to null if they are both 0.
85 | if (conversionFactors != null)
86 | {
87 | if (conversionFactors.isAngleEmpty() && conversionFactors.isDriveEmpty())
88 | {
89 | this.conversionFactor = null;
90 | }
91 | }
92 |
93 | this.driveMotorCurrentLimit = driveMotorCurrentLimit;
94 | this.angleMotorCurrentLimit = angleMotorCurrentLimit;
95 | this.driveMotorRampRate = driveMotorRampRate;
96 | this.angleMotorRampRate = angleMotorRampRate;
97 | this.driveFrictionVoltage = driveFrictionVoltage;
98 | this.angleFrictionVoltage = angleFrictionVoltage;
99 | this.steerRotationalInertia = steerRotationalInertia;
100 | this.robotMassKg = robotMassKg;
101 | }
102 |
103 | /**
104 | * Construct the swerve module physical characteristics. Assume coefficient of friction is 1.19 (taken from blue
105 | * nitrile on carpet from Studica) and optimal voltage is 12v. Assumes the drive motor current limit is 40A, and the
106 | * angle motor current limit is 20A.
107 | *
108 | * @param conversionFactors The conversion factors for the drive and angle motors, created by
109 | * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and
110 | * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}.
111 | * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents over drawing
112 | * power from battery)
113 | * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents overdrawing
114 | * power and power loss).
115 | */
116 | public SwerveModulePhysicalCharacteristics(
117 | ConversionFactorsJson conversionFactors,
118 | double driveMotorRampRate,
119 | double angleMotorRampRate)
120 | {
121 | this(
122 | conversionFactors,
123 | 1.19,
124 | 12,
125 | 40,
126 | 20,
127 | driveMotorRampRate,
128 | angleMotorRampRate,
129 | 0.2,
130 | 0.3,
131 | 0.03,
132 | 50);
133 | }
134 | }
135 |
--------------------------------------------------------------------------------
/swervelib/parser/json/ModuleJson.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser.json;
2 |
3 | import com.revrobotics.spark.SparkMax;
4 | import edu.wpi.first.math.util.Units;
5 | import swervelib.encoders.SparkMaxEncoderSwerve;
6 | import swervelib.encoders.SwerveAbsoluteEncoder;
7 | import swervelib.encoders.ThriftyNovaEncoderSwerve;
8 | import swervelib.motors.SwerveMotor;
9 | import swervelib.motors.ThriftyNovaSwerve;
10 | import swervelib.parser.PIDFConfig;
11 | import swervelib.parser.SwerveModuleConfiguration;
12 | import swervelib.parser.SwerveModulePhysicalCharacteristics;
13 | import swervelib.parser.json.modules.BoolMotorJson;
14 | import swervelib.parser.json.modules.ConversionFactorsJson;
15 | import swervelib.parser.json.modules.LocationJson;
16 |
17 | /**
18 | * {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data.
19 | */
20 | public class ModuleJson
21 | {
22 |
23 | /**
24 | * Drive motor device configuration.
25 | */
26 | public DeviceJson drive;
27 | /**
28 | * Angle motor device configuration.
29 | */
30 | public DeviceJson angle;
31 | /**
32 | * Conversion Factors composition. Auto-calculates the conversion factors.
33 | */
34 | public ConversionFactorsJson conversionFactors = new ConversionFactorsJson();
35 | /**
36 | * Absolute encoder device configuration.
37 | */
38 | public DeviceJson encoder;
39 | /**
40 | * Defines which motors are inverted.
41 | */
42 | public BoolMotorJson inverted;
43 | /**
44 | * Absolute encoder offset from 0 in degrees.
45 | */
46 | public double absoluteEncoderOffset;
47 | /**
48 | * Absolute encoder inversion state.
49 | */
50 | public boolean absoluteEncoderInverted = false;
51 | /**
52 | * The location of the swerve module from the center of the robot in inches.
53 | */
54 | public LocationJson location;
55 | /**
56 | * Should do cosine compensation when not pointing correct direction;.
57 | */
58 | public boolean useCosineCompensator = true;
59 |
60 | /**
61 | * Create the swerve module configuration based off of parsed data.
62 | *
63 | * @param anglePIDF The PIDF values for the angle motor.
64 | * @param velocityPIDF The velocity PIDF values for the drive motor.
65 | * @param physicalCharacteristics Physical characteristics of the swerve module.
66 | * @param name Module json filename.
67 | * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data.
68 | */
69 | public SwerveModuleConfiguration createModuleConfiguration(
70 | PIDFConfig anglePIDF,
71 | PIDFConfig velocityPIDF,
72 | SwerveModulePhysicalCharacteristics physicalCharacteristics,
73 | String name)
74 | {
75 | SwerveMotor angleMotor = angle.createMotor(false);
76 | SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor);
77 |
78 | //Throw an error if module locations are improperly set
79 | if (location.front == 0 && location.left == 0)
80 | {
81 | throw new RuntimeException("Improper Module Location Settings!\n" +
82 | "Your module location is set to 0 for both 'front' and 'left' values.\n" +
83 | "Set the distance from the center of the robot to the center of the wheel in your module JSON file!");
84 | }
85 |
86 | // Set the conversion factors to null if they are both 0.
87 | if (!conversionFactors.works() && physicalCharacteristics.conversionFactor == null)
88 | {
89 | throw new RuntimeException("No Conversion Factor configured! Please create SwerveDrive using \n" +
90 | "SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" +
91 | "OR\n" +
92 | "SwerveParser.createSwerveDrive(maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" +
93 | "OR\n" +
94 | "Set the conversion factor in physicalproperties.json OR the module JSON file." +
95 | "REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n");
96 | } else if (physicalCharacteristics.conversionFactor.works() && !conversionFactors.works())
97 | {
98 | conversionFactors = physicalCharacteristics.conversionFactor;
99 | } else if (physicalCharacteristics.conversionFactor.works())
100 | // If both are defined, override 0 with the physical characterstics input.
101 | {
102 | conversionFactors.angle = conversionFactors.isAngleEmpty() ? physicalCharacteristics.conversionFactor.angle
103 | : conversionFactors.angle;
104 | conversionFactors.drive = conversionFactors.isDriveEmpty() ? physicalCharacteristics.conversionFactor.drive
105 | : conversionFactors.drive;
106 | }
107 |
108 | if (conversionFactors.isDriveEmpty() || conversionFactors.isAngleEmpty())
109 | {
110 | throw new RuntimeException(
111 | "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files.");
112 | }
113 |
114 | // Backwards compatibility, auto-optimization.
115 | if (conversionFactors.angle.factor == 360 && absEncoder != null &&
116 | (absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax))
117 | {
118 | angleMotor.setAbsoluteEncoder(absEncoder);
119 | } else if ((absEncoder instanceof ThriftyNovaEncoderSwerve && angleMotor instanceof ThriftyNovaSwerve))
120 | {
121 | angleMotor.setAbsoluteEncoder(absEncoder);
122 | }
123 |
124 | return new SwerveModuleConfiguration(
125 | drive.createMotor(true),
126 | angleMotor,
127 | conversionFactors,
128 | absEncoder,
129 | absoluteEncoderOffset,
130 | Units.inchesToMeters(Math.round(location.front)),
131 | Units.inchesToMeters(Math.round(location.left)),
132 | anglePIDF,
133 | velocityPIDF,
134 | physicalCharacteristics,
135 | absoluteEncoderInverted,
136 | inverted.drive,
137 | inverted.angle,
138 | name.replaceAll("\\.json", ""),
139 | useCosineCompensator);
140 | }
141 | }
142 |
--------------------------------------------------------------------------------
/swervelib/motors/SwerveMotor.java:
--------------------------------------------------------------------------------
1 | package swervelib.motors;
2 |
3 | import edu.wpi.first.math.system.plant.DCMotor;
4 | import swervelib.encoders.SwerveAbsoluteEncoder;
5 | import swervelib.parser.PIDFConfig;
6 |
7 | /**
8 | * Swerve motor abstraction which defines a standard interface for motors within a swerve module.
9 | */
10 | public abstract class SwerveMotor implements AutoCloseable
11 | {
12 |
13 | @Override
14 | public abstract void close();
15 |
16 | /**
17 | * The maximum amount of times the swerve motor will attempt to configure a motor if failures occur.
18 | */
19 | public final int maximumRetries = 5;
20 | /**
21 | * Sim motor to use, defaulted in {@link SwerveMotor#getSimMotor()}, but can be overridden here.
NOTE: This will
22 | * not change the simulation motor type! It is intended for use only if you are utilizing Feedforwards from
23 | * PathPlanner.
24 | */
25 | public DCMotor simMotor;
26 | /**
27 | * Whether the swerve motor is a drive motor.
28 | */
29 | protected boolean isDriveMotor;
30 |
31 | /**
32 | * Configure the factory defaults.
33 | */
34 | public abstract void factoryDefaults();
35 |
36 | /**
37 | * Clear the sticky faults on the motor controller.
38 | */
39 | public abstract void clearStickyFaults();
40 |
41 | /**
42 | * Set the absolute encoder to be a compatible absolute encoder.
43 | *
44 | * @param encoder The encoder to use.
45 | * @return The {@link SwerveMotor} for single line configuration.
46 | */
47 | public abstract SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder);
48 |
49 | /**
50 | * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
51 | *
52 | * @param positionConversionFactor The conversion factor to apply for position.
53 | */
54 | public abstract void configureIntegratedEncoder(double positionConversionFactor);
55 |
56 | /**
57 | * Configure the PIDF values for the closed loop controller. 0 is disabled or off.
58 | *
59 | * @param config Configuration class holding the PIDF values.
60 | */
61 | public abstract void configurePIDF(PIDFConfig config);
62 |
63 | /**
64 | * Configure the PID wrapping for the position closed loop controller.
65 | *
66 | * @param minInput Minimum PID input.
67 | * @param maxInput Maximum PID input.
68 | */
69 | public abstract void configurePIDWrapping(double minInput, double maxInput);
70 |
71 | /**
72 | * Disable PID Wrapping on the motor.
73 | */
74 | public abstract void disablePIDWrapping();
75 |
76 | /**
77 | * Set the idle mode.
78 | *
79 | * @param isBrakeMode Set the brake mode.
80 | */
81 | public abstract void setMotorBrake(boolean isBrakeMode);
82 |
83 | /**
84 | * Set the motor to be inverted.
85 | *
86 | * @param inverted State of inversion.
87 | */
88 | public abstract void setInverted(boolean inverted);
89 |
90 | /**
91 | * Save the configurations from flash to EEPROM.
92 | */
93 | public abstract void burnFlash();
94 |
95 | /**
96 | * Set the percentage output.
97 | *
98 | * @param percentOutput percent out for the motor controller.
99 | */
100 | public abstract void set(double percentOutput);
101 |
102 | /**
103 | * Set the closed loop PID controller reference point.
104 | *
105 | * @param setpoint Setpoint in meters per second or angle in degrees.
106 | * @param feedforward Feedforward in volt-meter-per-second or kV.
107 | */
108 | public abstract void setReference(double setpoint, double feedforward);
109 |
110 | /**
111 | * Set the closed loop PID controller reference point.
112 | *
113 | * @param setpoint Setpoint in meters per second or angle in degrees.
114 | * @param feedforward Feedforward in volt-meter-per-second or kV.
115 | * @param position Only used on the angle motor, the position of the motor in degrees.
116 | */
117 | public abstract void setReference(double setpoint, double feedforward, double position);
118 |
119 | /**
120 | * Get the voltage output of the motor controller.
121 | *
122 | * @return Voltage output.
123 | */
124 | public abstract double getVoltage();
125 |
126 | /**
127 | * Set the voltage of the motor.
128 | *
129 | * @param voltage Voltage to set.
130 | */
131 | public abstract void setVoltage(double voltage);
132 |
133 | /**
134 | * Get the applied dutycycle output.
135 | *
136 | * @return Applied dutycycle output to the motor.
137 | */
138 | public abstract double getAppliedOutput();
139 |
140 | /**
141 | * Get the velocity of the integrated encoder.
142 | *
143 | * @return velocity in meters per second or degrees per second.
144 | */
145 | public abstract double getVelocity();
146 |
147 | /**
148 | * Get the position of the integrated encoder.
149 | *
150 | * @return Position in meters or degrees.
151 | */
152 | public abstract double getPosition();
153 |
154 | /**
155 | * Set the integrated encoder position.
156 | *
157 | * @param position Integrated encoder position. Should be angle in degrees or meters per second.
158 | */
159 | public abstract void setPosition(double position);
160 |
161 | /**
162 | * Set the voltage compensation for the swerve module motor.
163 | *
164 | * @param nominalVoltage Nominal voltage for operation to output to.
165 | */
166 | public abstract void setVoltageCompensation(double nominalVoltage);
167 |
168 | /**
169 | * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
170 | * voltage compensation. This is useful to protect the motor from current spikes.
171 | *
172 | * @param currentLimit Current limit in AMPS at free speed.
173 | */
174 | public abstract void setCurrentLimit(int currentLimit);
175 |
176 | /**
177 | * Set the maximum rate the open/closed loop output can change by.
178 | *
179 | * @param rampRate Time in seconds to go from 0 to full throttle.
180 | */
181 | public abstract void setLoopRampRate(double rampRate);
182 |
183 | /**
184 | * Get the motor object from the module.
185 | *
186 | * @return Motor object.
187 | */
188 | public abstract Object getMotor();
189 |
190 | /**
191 | * Get the {@link DCMotor} of the motor class.
192 | *
193 | * @return {@link DCMotor} of this type.
194 | */
195 | public abstract DCMotor getSimMotor();
196 |
197 | /**
198 | * Queries whether the absolute encoder is directly attached to the motor controller.
199 | *
200 | * @return connected absolute encoder state.
201 | */
202 | public abstract boolean usingExternalFeedbackSensor();
203 | }
204 |
--------------------------------------------------------------------------------
/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import com.revrobotics.REVLibError;
4 | import com.revrobotics.spark.SparkAnalogSensor;
5 | import com.revrobotics.spark.SparkMax;
6 | import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
7 | import com.revrobotics.spark.config.SparkMaxConfig;
8 | import edu.wpi.first.wpilibj.Alert;
9 | import edu.wpi.first.wpilibj.Alert.AlertType;
10 | import java.util.function.Supplier;
11 | import swervelib.motors.SparkMaxBrushedMotorSwerve;
12 | import swervelib.motors.SparkMaxSwerve;
13 | import swervelib.motors.SwerveMotor;
14 |
15 | /**
16 | * SparkMax absolute encoder, attached through the data port analog pin.
17 | */
18 | public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
19 | {
20 |
21 | /**
22 | * {@link swervelib.motors.SparkMaxSwerve} or {@link swervelib.motors.SparkMaxBrushedMotorSwerve} object.
23 | */
24 | private final SwerveMotor sparkMax;
25 | /**
26 | * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
27 | */
28 | public SparkAnalogSensor encoder;
29 | /**
30 | * An {@link Alert} for if there is a failure configuring the encoder.
31 | */
32 | private Alert failureConfiguring;
33 | /**
34 | * An {@link Alert} for if the absolute encoder does not support integrated offsets.
35 | */
36 | private Alert doesNotSupportIntegratedOffsets;
37 |
38 | /**
39 | * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link SparkMax} motor data port
40 | * analog pin.
41 | *
42 | * @param motor Motor to create the encoder from.
43 | * @param maxVoltage Maximum voltage for analog input reading.
44 | */
45 | public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage)
46 | {
47 | if (motor.getMotor() instanceof SparkMax)
48 | {
49 | sparkMax = motor;
50 | encoder = ((SparkMax) motor.getMotor()).getAnalog();
51 | setConversionFactor(360.0 / maxVoltage);
52 | } else
53 | {
54 | throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
55 | }
56 | failureConfiguring = new Alert(
57 | "Encoders",
58 | "Failure configuring SparkMax Analog Encoder",
59 | AlertType.kWarning);
60 | doesNotSupportIntegratedOffsets = new Alert(
61 | "Encoders",
62 | "SparkMax Analog Sensors do not support integrated offsets",
63 | AlertType.kWarning);
64 |
65 | }
66 |
67 | @Override
68 | public void close()
69 | {
70 | // SPARK MAX Analog encoder gets closed with the motor
71 | // I don't think an encoder getting closed should
72 | // close the entire motor so i will keep this empty
73 | // sparkMax.close();
74 | }
75 |
76 | /**
77 | * Run the configuration until it succeeds or times out.
78 | *
79 | * @param config Lambda supplier returning the error state.
80 | */
81 | private void configureSparkMax(Supplier config)
82 | {
83 | for (int i = 0; i < maximumRetries; i++)
84 | {
85 | if (config.get() == REVLibError.kOk)
86 | {
87 | return;
88 | }
89 | }
90 | failureConfiguring.set(true);
91 | }
92 |
93 | /**
94 | * Set the conversion factor of the {@link SparkMaxAnalogEncoderSwerve}.
95 | *
96 | * @param conversionFactor Position conversion factor from ticks to unit.
97 | */
98 | public void setConversionFactor(double conversionFactor)
99 | {
100 | SparkMaxConfig cfg = null;
101 | if (sparkMax instanceof SparkMaxSwerve)
102 | {
103 | cfg = ((SparkMaxSwerve) sparkMax).getConfig();
104 |
105 | } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
106 | {
107 | cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
108 | }
109 | if (cfg != null)
110 | {
111 | cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
112 |
113 | cfg.signals
114 | .analogVelocityAlwaysOn(true)
115 | .analogVoltageAlwaysOn(true)
116 | .analogPositionAlwaysOn(true)
117 | .analogVoltagePeriodMs(20)
118 | .analogPositionPeriodMs(20)
119 | .analogVelocityPeriodMs(20);
120 |
121 | cfg.analogSensor
122 | .positionConversionFactor(conversionFactor)
123 | .velocityConversionFactor(conversionFactor / 60);
124 | }
125 | if (sparkMax instanceof SparkMaxSwerve)
126 | {
127 | ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
128 | } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
129 | {
130 | ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
131 | }
132 |
133 | }
134 |
135 |
136 | /**
137 | * Reset the encoder to factory defaults.
138 | */
139 | @Override
140 | public void factoryDefault()
141 | {
142 | // Do nothing
143 | }
144 |
145 | /**
146 | * Clear sticky faults on the encoder.
147 | */
148 | @Override
149 | public void clearStickyFaults()
150 | {
151 | // Do nothing
152 | }
153 |
154 | /**
155 | * Configure the absolute encoder to read from [0, 360) per second.
156 | *
157 | * @param inverted Whether the encoder is inverted.
158 | */
159 | @Override
160 | public void configure(boolean inverted)
161 | {
162 | if (sparkMax instanceof SparkMaxSwerve)
163 | {
164 | SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
165 | cfg.analogSensor.inverted(inverted);
166 | ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
167 | } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
168 | {
169 | SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
170 | cfg.analogSensor.inverted(inverted);
171 | ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
172 | }
173 | }
174 |
175 | /**
176 | * Get the absolute position of the encoder.
177 | *
178 | * @return Absolute position in degrees from [0, 360).
179 | */
180 | @Override
181 | public double getAbsolutePosition()
182 | {
183 | return encoder.getPosition();
184 | }
185 |
186 | /**
187 | * Get the instantiated absolute encoder Object.
188 | *
189 | * @return Absolute encoder object.
190 | */
191 | @Override
192 | public Object getAbsoluteEncoder()
193 | {
194 | return encoder;
195 | }
196 |
197 | /**
198 | * Sets the Absolute Encoder offset at the Encoder Level.
199 | *
200 | * @param offset the offset the Absolute Encoder uses as the zero point.
201 | * @return if setting Absolute Encoder Offset was successful or not.
202 | */
203 | @Override
204 | public boolean setAbsoluteEncoderOffset(double offset)
205 | {
206 | doesNotSupportIntegratedOffsets.set(true);
207 | return false;
208 | }
209 |
210 | /**
211 | * Get the velocity in degrees/sec.
212 | *
213 | * @return velocity in degrees/sec.
214 | */
215 | @Override
216 | public double getVelocity()
217 | {
218 | return encoder.getVelocity();
219 | }
220 | }
221 |
--------------------------------------------------------------------------------
/swervelib/parser/SwerveModuleConfiguration.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser;
2 |
3 | import edu.wpi.first.math.geometry.Translation2d;
4 | import swervelib.encoders.SwerveAbsoluteEncoder;
5 | import swervelib.motors.SwerveMotor;
6 | import swervelib.parser.json.modules.ConversionFactorsJson;
7 |
8 | /**
9 | * Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}.
10 | */
11 | public class SwerveModuleConfiguration
12 | {
13 |
14 | /**
15 | * Conversion factor for drive motor onboard PID's and angle PID's. Use
16 | * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and
17 | * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to calculate the
18 | * conversion factors.
19 | */
20 | public final ConversionFactorsJson conversionFactors;
21 | /**
22 | * Angle offset in degrees for the Swerve Module.
23 | */
24 | public final double angleOffset;
25 | /**
26 | * Whether the absolute encoder is inverted.
27 | */
28 | public final boolean absoluteEncoderInverted;
29 | /**
30 | * State of inversion of the drive motor.
31 | */
32 | public final boolean driveMotorInverted;
33 | /**
34 | * State of inversion of the angle motor.
35 | */
36 | public final boolean angleMotorInverted;
37 | /**
38 | * PIDF configuration options for the angle motor closed-loop PID controller.
39 | */
40 | public PIDFConfig anglePIDF;
41 | /**
42 | * PIDF configuration options for the drive motor closed-loop PID controller.
43 | */
44 | public PIDFConfig velocityPIDF;
45 | /**
46 | * Swerve module location relative to the robot.
47 | */
48 | public Translation2d moduleLocation;
49 | /**
50 | * Physical characteristics of the swerve module.
51 | */
52 | public SwerveModulePhysicalCharacteristics physicalCharacteristics;
53 | /**
54 | * The drive motor and angle motor of this swerve module.
55 | */
56 | public SwerveMotor driveMotor, angleMotor;
57 | /**
58 | * The Absolute Encoder for the swerve module.
59 | */
60 | public SwerveAbsoluteEncoder absoluteEncoder;
61 | /**
62 | * Name for the swerve module for telemetry.
63 | */
64 | public String name;
65 | /**
66 | * Should do cosine compensation when not pointing correct direction;.
67 | */
68 | public boolean useCosineCompensator;
69 |
70 | /**
71 | * Construct a configuration object for swerve modules.
72 | *
73 | * @param driveMotor Drive {@link SwerveMotor}.
74 | * @param angleMotor Angle {@link SwerveMotor}
75 | * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}.
76 | * @param angleOffset Absolute angle offset to 0.
77 | * @param absoluteEncoderInverted Absolute encoder inverted.
78 | * @param angleMotorInverted State of inversion of the angle motor.
79 | * @param driveMotorInverted Drive motor inverted.
80 | * @param xMeters Module location in meters from the center horizontally.
81 | * @param yMeters Module location in meters from center vertically.
82 | * @param anglePIDF Angle PIDF configuration.
83 | * @param velocityPIDF Velocity PIDF configuration.
84 | * @param physicalCharacteristics Physical characteristics of the swerve module.
85 | * @param name The name for the swerve module.
86 | * @param conversionFactors Conversion factors to be applied to the drive and angle motors.
87 | * @param useCosineCompensator Should use cosineCompensation.
88 | */
89 | public SwerveModuleConfiguration(
90 | SwerveMotor driveMotor,
91 | SwerveMotor angleMotor,
92 | ConversionFactorsJson conversionFactors,
93 | SwerveAbsoluteEncoder absoluteEncoder,
94 | double angleOffset,
95 | double xMeters,
96 | double yMeters,
97 | PIDFConfig anglePIDF,
98 | PIDFConfig velocityPIDF,
99 | SwerveModulePhysicalCharacteristics physicalCharacteristics,
100 | boolean absoluteEncoderInverted,
101 | boolean driveMotorInverted,
102 | boolean angleMotorInverted,
103 | String name,
104 | boolean useCosineCompensator)
105 | {
106 | this.driveMotor = driveMotor;
107 | this.angleMotor = angleMotor;
108 | this.conversionFactors = conversionFactors;
109 | this.absoluteEncoder = absoluteEncoder;
110 | this.angleOffset = angleOffset;
111 | this.absoluteEncoderInverted = absoluteEncoderInverted;
112 | this.driveMotorInverted = driveMotorInverted;
113 | this.angleMotorInverted = angleMotorInverted;
114 | this.moduleLocation = new Translation2d(xMeters, yMeters);
115 | this.anglePIDF = anglePIDF;
116 | this.velocityPIDF = velocityPIDF;
117 | this.physicalCharacteristics = physicalCharacteristics;
118 | this.name = name;
119 | this.useCosineCompensator = useCosineCompensator;
120 | }
121 |
122 | /**
123 | * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive motor are not
124 | * inverted.
125 | *
126 | * @param driveMotor Drive {@link SwerveMotor}.
127 | * @param angleMotor Angle {@link SwerveMotor}
128 | * @param conversionFactors Conversion factors for angle/azimuth motors drive factors.
129 | * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}.
130 | * @param angleOffset Absolute angle offset to 0.
131 | * @param xMeters Module location in meters from the center horizontally.
132 | * @param yMeters Module location in meters from center vertically.
133 | * @param anglePIDF Angle PIDF configuration.
134 | * @param velocityPIDF Velocity PIDF configuration.
135 | * @param physicalCharacteristics Physical characteristics of the swerve module.
136 | * @param name Name for the module.
137 | * @param useCosineCompensator Should use cosineCompensation.
138 | */
139 | public SwerveModuleConfiguration(
140 | SwerveMotor driveMotor,
141 | SwerveMotor angleMotor,
142 | ConversionFactorsJson conversionFactors,
143 | SwerveAbsoluteEncoder absoluteEncoder,
144 | double angleOffset,
145 | double xMeters,
146 | double yMeters,
147 | PIDFConfig anglePIDF,
148 | PIDFConfig velocityPIDF,
149 | SwerveModulePhysicalCharacteristics physicalCharacteristics,
150 | String name,
151 | boolean useCosineCompensator)
152 | {
153 | this(
154 | driveMotor,
155 | angleMotor,
156 | conversionFactors,
157 | absoluteEncoder,
158 | angleOffset,
159 | xMeters,
160 | yMeters,
161 | anglePIDF,
162 | velocityPIDF,
163 | physicalCharacteristics,
164 | false,
165 | false,
166 | false,
167 | name,
168 | useCosineCompensator);
169 | }
170 |
171 |
172 | }
173 |
--------------------------------------------------------------------------------
/swervelib/encoders/SparkMaxEncoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import com.revrobotics.AbsoluteEncoder;
4 | import com.revrobotics.REVLibError;
5 | import com.revrobotics.spark.SparkAbsoluteEncoder;
6 | import com.revrobotics.spark.SparkMax;
7 | import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
8 | import com.revrobotics.spark.config.SparkMaxConfig;
9 | import edu.wpi.first.wpilibj.Alert;
10 | import edu.wpi.first.wpilibj.Alert.AlertType;
11 | import java.util.function.Supplier;
12 | import swervelib.motors.SparkMaxBrushedMotorSwerve;
13 | import swervelib.motors.SparkMaxSwerve;
14 | import swervelib.motors.SwerveMotor;
15 |
16 | /**
17 | * SparkMax absolute encoder, attached through the data port.
18 | */
19 | public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
20 | {
21 |
22 | /**
23 | * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
24 | */
25 | public SparkAbsoluteEncoder encoder;
26 | /**
27 | * An {@link Alert} for if there is a failure configuring the encoder.
28 | */
29 | private Alert failureConfiguring;
30 | /**
31 | * An {@link Alert} for if there is a failure configuring the encoder offset.
32 | */
33 | private Alert offsetFailure;
34 | /**
35 | * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance.
36 | */
37 | private SwerveMotor sparkMax;
38 |
39 | /**
40 | * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link com.revrobotics.spark.SparkMax}
41 | * motor.
42 | *
43 | * @param motor Motor to create the encoder from.
44 | * @param conversionFactor The conversion factor to set if the output is not from 0 to 360.
45 | */
46 | public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor)
47 | {
48 | failureConfiguring = new Alert(
49 | "Encoders",
50 | "Failure configuring SparkMax Absolute Encoder",
51 | AlertType.kWarning);
52 | offsetFailure = new Alert(
53 | "Encoders",
54 | "Failure to set Absolute Encoder Offset",
55 | AlertType.kWarning);
56 | if (motor.getMotor() instanceof SparkMax)
57 | {
58 | sparkMax = motor;
59 | encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder();
60 | setConversionFactor(conversionFactor);
61 | } else
62 | {
63 | throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
64 | }
65 | }
66 |
67 | @Override
68 | public void close()
69 | {
70 | // SPARK MAX encoder gets closed with the motor
71 | // I don't think an encoder getting closed should
72 | // close the entire motor so i will keep this empty
73 | // sparkFlex.close();
74 | }
75 |
76 | /**
77 | * Run the configuration until it succeeds or times out.
78 | *
79 | * @param config Lambda supplier returning the error state.
80 | */
81 | private void configureSparkMax(Supplier config)
82 | {
83 | for (int i = 0; i < maximumRetries; i++)
84 | {
85 | if (config.get() == REVLibError.kOk)
86 | {
87 | return;
88 | }
89 | }
90 | failureConfiguring.set(true);
91 | }
92 |
93 | /**
94 | * Reset the encoder to factory defaults.
95 | */
96 | @Override
97 | public void factoryDefault()
98 | {
99 | // Do nothing
100 | }
101 |
102 | /**
103 | * Clear sticky faults on the encoder.
104 | */
105 | @Override
106 | public void clearStickyFaults()
107 | {
108 | // Do nothing
109 | }
110 |
111 | /**
112 | * Configure the absolute encoder to read from [0, 360) per second.
113 | *
114 | * @param inverted Whether the encoder is inverted.
115 | */
116 | @Override
117 | public void configure(boolean inverted)
118 | {
119 | if (sparkMax instanceof SparkMaxSwerve)
120 | {
121 | SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
122 | cfg.absoluteEncoder.inverted(inverted);
123 | ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
124 | } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
125 | {
126 | SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
127 | cfg.absoluteEncoder.inverted(inverted);
128 | ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
129 | }
130 | }
131 |
132 |
133 | /**
134 | * Set the conversion factor of the {@link SparkMaxEncoderSwerve}.
135 | *
136 | * @param conversionFactor Position conversion factor from ticks to unit.
137 | */
138 | public void setConversionFactor(double conversionFactor)
139 | {
140 | // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
141 | // This needs to be set to 20ms or under to properly update the swerve module position for odometry
142 | // Configuration taken from 3005, the team who helped develop the Max Swerve:
143 | // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
144 | // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
145 | // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
146 | // with limited testing 19ms did not return the same value while the module was constatntly rotating.
147 |
148 | SparkMaxConfig cfg = null;
149 | if (sparkMax instanceof SparkMaxSwerve)
150 | {
151 | cfg = ((SparkMaxSwerve) sparkMax).getConfig();
152 |
153 | } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
154 | {
155 | cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
156 | }
157 | if (cfg != null)
158 | {
159 | cfg.signals
160 | .absoluteEncoderPositionAlwaysOn(true)
161 | .absoluteEncoderPositionPeriodMs(20);
162 |
163 | cfg.absoluteEncoder
164 | .positionConversionFactor(conversionFactor)
165 | .velocityConversionFactor(conversionFactor / 60);
166 | }
167 | if (sparkMax instanceof SparkMaxSwerve)
168 | {
169 | ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
170 | } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
171 | {
172 | ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
173 | }
174 |
175 | }
176 |
177 |
178 | /**
179 | * Get the absolute position of the encoder.
180 | *
181 | * @return Absolute position in degrees from [0, 360).
182 | */
183 | @Override
184 | public double getAbsolutePosition()
185 | {
186 | return encoder.getPosition();
187 | }
188 |
189 | /**
190 | * Get the instantiated absolute encoder Object.
191 | *
192 | * @return Absolute encoder object.
193 | */
194 | @Override
195 | public Object getAbsoluteEncoder()
196 | {
197 | return encoder;
198 | }
199 |
200 | /**
201 | * Sets the Absolute Encoder Offset inside of the SparkMax's Memory.
202 | *
203 | * @param offset the offset the Absolute Encoder uses as the zero point.
204 | * @return if setting Absolute Encoder Offset was successful or not.
205 | */
206 | @Override
207 | public boolean setAbsoluteEncoderOffset(double offset)
208 | {
209 | if (sparkMax instanceof SparkMaxSwerve)
210 | {
211 | SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
212 | cfg.absoluteEncoder.zeroOffset(offset);
213 | ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
214 | return true;
215 | } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
216 | {
217 | SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
218 | cfg.absoluteEncoder.zeroOffset(offset);
219 | ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
220 | }
221 | return false;
222 | }
223 |
224 | /**
225 | * Get the velocity in degrees/sec.
226 | *
227 | * @return velocity in degrees/sec.
228 | */
229 | @Override
230 | public double getVelocity()
231 | {
232 | return encoder.getVelocity();
233 | }
234 | }
235 |
--------------------------------------------------------------------------------
/swervelib/encoders/CANCoderSwerve.java:
--------------------------------------------------------------------------------
1 | package swervelib.encoders;
2 |
3 | import static edu.wpi.first.units.Units.DegreesPerSecond;
4 | import static edu.wpi.first.units.Units.Milliseconds;
5 | import static edu.wpi.first.units.Units.Rotations;
6 | import static edu.wpi.first.units.Units.Seconds;
7 |
8 | import com.ctre.phoenix6.StatusCode;
9 | import com.ctre.phoenix6.StatusSignal;
10 | import com.ctre.phoenix6.configs.CANcoderConfiguration;
11 | import com.ctre.phoenix6.configs.CANcoderConfigurator;
12 | import com.ctre.phoenix6.hardware.CANcoder;
13 | import com.ctre.phoenix6.signals.MagnetHealthValue;
14 | import com.ctre.phoenix6.signals.SensorDirectionValue;
15 | import edu.wpi.first.units.measure.Angle;
16 | import edu.wpi.first.units.measure.AngularVelocity;
17 | import edu.wpi.first.wpilibj.Alert;
18 | import edu.wpi.first.wpilibj.Alert.AlertType;
19 |
20 | /**
21 | * Swerve Absolute Encoder for CTRE CANCoders.
22 | */
23 | public class CANCoderSwerve extends SwerveAbsoluteEncoder
24 | {
25 |
26 | /**
27 | * Wait time for status frames to show up.
28 | */
29 | public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(1).in(Seconds);
30 | /**
31 | * An {@link Alert} for if the CANCoder magnet field is less than ideal.
32 | */
33 | private final Alert magnetFieldLessThanIdeal;
34 | /**
35 | * An {@link Alert} for if the CANCoder reading is faulty.
36 | */
37 | private final Alert readingFaulty;
38 | /**
39 | * An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored.
40 | */
41 | private final Alert readingIgnored;
42 | /**
43 | * An {@link Alert} for if the absolute encoder offset cannot be set.
44 | */
45 | private final Alert cannotSetOffset;
46 | /**
47 | * Magnet Health status signal for the CANCoder.
48 | */
49 | private final StatusSignal magnetHealth;
50 | /**
51 | * CANCoder reading cache.
52 | */
53 | private final StatusSignal angle;
54 | /**
55 | * Angular velocity of the {@link CANcoder}.
56 | */
57 | private final StatusSignal velocity;
58 | /**
59 | * CANCoder with WPILib sendable and support.
60 | */
61 | public CANcoder encoder;
62 | /**
63 | * {@link CANcoder} Configurator objet for this class.
64 | */
65 | private CANcoderConfigurator config;
66 | /**
67 | * {@link CANcoderConfiguration} object for the CANcoder.
68 | */
69 | private CANcoderConfiguration cfg = new CANcoderConfiguration();
70 |
71 | /**
72 | * Initialize the CANCoder on the standard CANBus.
73 | *
74 | * @param id CAN ID.
75 | */
76 | public CANCoderSwerve(int id)
77 | {
78 | // Empty string uses the default canbus for the system
79 | this(id, "");
80 | }
81 |
82 | /**
83 | * Initialize the CANCoder on the CANivore.
84 | *
85 | * @param id CAN ID of the {@link CANcoder}.
86 | * @param canbus CAN bus to initialize it on. Should be "rio" or "" if the RIO CANbus, else is the CANivore name.
87 | */
88 | public CANCoderSwerve(int id, String canbus)
89 | {
90 | encoder = new CANcoder(id, canbus);
91 | config = encoder.getConfigurator();
92 | magnetHealth = encoder.getMagnetHealth();
93 | angle = encoder.getAbsolutePosition();
94 | velocity = encoder.getVelocity();
95 | magnetFieldLessThanIdeal = new Alert(
96 | "Encoders",
97 | "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.",
98 | AlertType.kWarning);
99 | readingFaulty = new Alert(
100 | "Encoders",
101 | "CANCoder " + encoder.getDeviceID() + " reading was faulty.",
102 | AlertType.kWarning);
103 | readingIgnored = new Alert(
104 | "Encoders",
105 | "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.",
106 | AlertType.kWarning);
107 | cannotSetOffset = new Alert(
108 | "Encoders",
109 | "Failure to set CANCoder "
110 | + encoder.getDeviceID()
111 | + " Absolute Encoder Offset",
112 | AlertType.kWarning);
113 | }
114 |
115 | @Override
116 | public void close()
117 | {
118 | encoder.close();
119 | }
120 |
121 | /**
122 | * Reset the encoder to factory defaults.
123 | */
124 | @Override
125 | public void factoryDefault()
126 | {
127 | cfg = new CANcoderConfiguration();
128 | config.apply(cfg);
129 | }
130 |
131 | /**
132 | * Clear sticky faults on the encoder.
133 | */
134 | @Override
135 | public void clearStickyFaults()
136 | {
137 | encoder.clearStickyFaults();
138 | }
139 |
140 | /**
141 | * Configure the absolute encoder to read from [0, 360) per second.
142 | *
143 | * @param inverted Whether the encoder is inverted.
144 | */
145 | @Override
146 | public void configure(boolean inverted)
147 | {
148 | config.refresh(cfg.MagnetSensor);
149 | config.apply(cfg.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1))
150 | .withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive
151 | : SensorDirectionValue.CounterClockwise_Positive));
152 | }
153 |
154 |
155 | /**
156 | * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on erroneous readings.
157 | *
158 | * @return Absolute position in degrees from [0, 360).
159 | */
160 | @Override
161 | public double getAbsolutePosition()
162 | {
163 | readingError = false;
164 | MagnetHealthValue strength = magnetHealth.refresh().getValue();
165 | angle.refresh();
166 |
167 | magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green);
168 | if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red)
169 | {
170 | readingError = true;
171 | readingFaulty.set(true);
172 | return 0;
173 | } else
174 | {
175 | readingFaulty.set(false);
176 | }
177 |
178 | // Taken from democat's library.
179 | // Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74
180 | for (int i = 0; i < maximumRetries; i++)
181 | {
182 | if (angle.getStatus() == StatusCode.OK)
183 | {
184 | break;
185 | }
186 | angle.waitForUpdate(STATUS_TIMEOUT_SECONDS);
187 | }
188 | if (angle.getStatus() != StatusCode.OK)
189 | {
190 | readingError = true;
191 | readingIgnored.set(true);
192 | } else
193 | {
194 | readingIgnored.set(false);
195 | }
196 | // Convert from Rotations to Degrees.
197 | return angle.getValueAsDouble() * 360;
198 | }
199 |
200 | /**
201 | * Get the instantiated absolute encoder Object.
202 | *
203 | * @return Absolute encoder object.
204 | */
205 | @Override
206 | public Object getAbsoluteEncoder()
207 | {
208 | return encoder;
209 | }
210 |
211 | /**
212 | * Sets the Absolute Encoder Offset within the CANcoder's Memory.
213 | *
214 | * @param offset the offset the Absolute Encoder uses as the zero point in degrees.
215 | * @return if setting Absolute Encoder Offset was successful or not.
216 | */
217 | @Override
218 | public boolean setAbsoluteEncoderOffset(double offset)
219 | {
220 | StatusCode error = config.refresh(cfg.MagnetSensor);
221 | if (error != StatusCode.OK)
222 | {
223 | return false;
224 | }
225 |
226 | error = config.apply(cfg.MagnetSensor.withMagnetOffset(offset / 360));
227 | cannotSetOffset.setText(
228 | "Failure to set CANCoder "
229 | + encoder.getDeviceID()
230 | + " Absolute Encoder Offset Error: "
231 | + error);
232 | if (error == StatusCode.OK)
233 | {
234 | cannotSetOffset.set(false);
235 | return true;
236 | }
237 | cannotSetOffset.set(true);
238 | return false;
239 | }
240 |
241 | /**
242 | * Get the velocity in degrees/sec.
243 | *
244 | * @return velocity in degrees/sec.
245 | */
246 | @Override
247 | public double getVelocity()
248 | {
249 | return velocity.refresh().getValue().in(DegreesPerSecond);
250 | }
251 | }
252 |
--------------------------------------------------------------------------------
/swervelib/telemetry/Alert.java:
--------------------------------------------------------------------------------
1 | // Copyright (c) 2023 FRC 6328
2 | // http://github.com/Mechanical-Advantage
3 | //
4 | // Use of this source code is governed by an MIT-style
5 | // license that can be found below.
6 |
7 | // MIT License
8 | //
9 | // Copyright (c) 2023 FRC 6328
10 | //
11 | // Permission is hereby granted, free of charge, to any person obtaining a copy
12 | // of this software and associated documentation files (the "Software"), to deal
13 | // in the Software without restriction, including without limitation the rights
14 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15 | // copies of the Software, and to permit persons to whom the Software is
16 | // furnished to do so, subject to the following conditions:
17 | //
18 | // The above copyright notice and this permission notice shall be included in all
19 | // copies or substantial portions of the Software.
20 | //
21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
22 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
23 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
24 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
25 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
26 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
27 | // SOFTWARE.
28 |
29 | package swervelib.telemetry;
30 |
31 | import edu.wpi.first.util.sendable.Sendable;
32 | import edu.wpi.first.util.sendable.SendableBuilder;
33 | import edu.wpi.first.wpilibj.DriverStation;
34 | import edu.wpi.first.wpilibj.Timer;
35 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
36 | import java.util.ArrayList;
37 | import java.util.HashMap;
38 | import java.util.List;
39 | import java.util.Map;
40 |
41 | /**
42 | * Class for managing persistent alerts to be sent over NetworkTables.
43 | */
44 | public class Alert
45 | {
46 |
47 | /**
48 | * Group of the alert.
49 | */
50 | private static Map groups = new HashMap();
51 | /**
52 | * Type of the Alert to raise.
53 | */
54 | private final AlertType type;
55 | /**
56 | * Activation state of alert.
57 | */
58 | private boolean active = false;
59 | /**
60 | * When the alert was raised.
61 | */
62 | private double activeStartTime = 0.0;
63 | /**
64 | * Text of the alert.
65 | */
66 | private String text;
67 |
68 | /**
69 | * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, the appropriate
70 | * entries will be added to NetworkTables.
71 | *
72 | * @param text Text to be displayed when the alert is active.
73 | * @param type Alert level specifying urgency.
74 | */
75 | public Alert(String text, AlertType type)
76 | {
77 | this("Alerts", text, type);
78 | }
79 |
80 | /**
81 | * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate entries will be added to
82 | * NetworkTables.
83 | *
84 | * @param group Group identifier, also used as NetworkTables title
85 | * @param text Text to be displayed when the alert is active.
86 | * @param type Alert level specifying urgency.
87 | */
88 | public Alert(String group, String text, AlertType type)
89 | {
90 | if (!groups.containsKey(group))
91 | {
92 | groups.put(group, new SendableAlerts());
93 | SmartDashboard.putData(group, groups.get(group));
94 | }
95 |
96 | this.text = text;
97 | this.type = type;
98 | groups.get(group).alerts.add(this);
99 | }
100 |
101 | /**
102 | * Sets whether the alert should currently be displayed. When activated, the alert text will also be sent to the
103 | * console.
104 | *
105 | * @param active Set the alert as active and report it to the driver station.
106 | */
107 | public void set(boolean active)
108 | {
109 | if (active && !this.active)
110 | {
111 | activeStartTime = Timer.getFPGATimestamp();
112 | printAlert(text);
113 | }
114 | this.active = active;
115 | }
116 |
117 | /**
118 | * Updates current alert text.
119 | *
120 | * @param text The text for the alert.
121 | */
122 | public void setText(String text)
123 | {
124 | if (active && !text.equals(this.text))
125 | {
126 | printAlert(text);
127 | }
128 | this.text = text;
129 | }
130 |
131 |
132 | /**
133 | * Print the alert message.
134 | *
135 | * @param text Text to print.
136 | */
137 | private void printAlert(String text)
138 | {
139 | switch (type)
140 | {
141 | case ERROR:
142 | DriverStation.reportError(text, false);
143 | break;
144 | case ERROR_TRACE:
145 | DriverStation.reportError(text, true);
146 | break;
147 | case WARNING:
148 | DriverStation.reportWarning(text, false);
149 | break;
150 | case WARNING_TRACE:
151 | DriverStation.reportWarning(text, true);
152 | break;
153 | case INFO:
154 | System.out.println(text);
155 | break;
156 | }
157 | }
158 |
159 | /**
160 | * Represents an alert's level of urgency.
161 | */
162 | public static enum AlertType
163 | {
164 | /**
165 | * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which
166 | * will seriously affect the robot's functionality and thus require immediate attention.
167 | */
168 | ERROR,
169 | /**
170 | * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which
171 | * will seriously affect the robot's functionality and thus require immediate attention. Trace printed to driver
172 | * station console.
173 | */
174 | ERROR_TRACE,
175 |
176 | /**
177 | * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems
178 | * which could affect the robot's functionality but do not necessarily require immediate attention.
179 | */
180 | WARNING,
181 | /**
182 | * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems
183 | * which could affect the robot's functionality but do not necessarily require immediate attention. Trace printed to
184 | * driver station console.
185 | */
186 | WARNING_TRACE,
187 | /**
188 | * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type for problems which
189 | * are unlikely to affect the robot's functionality, or any other alerts which do not fall under "ERROR" or
190 | * "WARNING".
191 | */
192 | INFO
193 | }
194 |
195 | /**
196 | * Sendable alert for advantage scope.
197 | */
198 | private static class SendableAlerts implements Sendable
199 | {
200 |
201 | /**
202 | * Alert list for sendable.
203 | */
204 | public final List alerts = new ArrayList<>();
205 |
206 | /**
207 | * Get alerts based off of type.
208 | *
209 | * @param type Type of alert to fetch.
210 | * @return Active alert strings.
211 | */
212 | public String[] getStrings(AlertType type)
213 | {
214 | List alertStrings = new ArrayList<>();
215 | for (Alert alert : alerts)
216 | {
217 | if (alert.type == type && alert.active)
218 | {
219 | alertStrings.add(alert.text);
220 | }
221 | }
222 | // alertStrings.sort((a1, a2) -> (int) (a2.activeStartTime - a1.activeStartTime));
223 | return alertStrings.toArray(new String[alertStrings.size()]);
224 | }
225 |
226 | @Override
227 | public void initSendable(SendableBuilder builder)
228 | {
229 | builder.setSmartDashboardType("Alerts");
230 | builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null);
231 | builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null);
232 | builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null);
233 | builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null);
234 | builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null);
235 | }
236 | }
237 | }
--------------------------------------------------------------------------------
/swervelib/parser/SwerveParser.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser;
2 |
3 | import com.fasterxml.jackson.databind.DeserializationFeature;
4 | import com.fasterxml.jackson.databind.JsonNode;
5 | import com.fasterxml.jackson.databind.ObjectMapper;
6 | import edu.wpi.first.math.geometry.Pose2d;
7 | import java.io.File;
8 | import java.io.IOException;
9 | import java.util.HashMap;
10 | import swervelib.SwerveDrive;
11 | import swervelib.SwerveModule;
12 | import swervelib.math.SwerveMath;
13 | import swervelib.parser.json.ControllerPropertiesJson;
14 | import swervelib.parser.json.ModuleJson;
15 | import swervelib.parser.json.PIDFPropertiesJson;
16 | import swervelib.parser.json.PhysicalPropertiesJson;
17 | import swervelib.parser.json.SwerveDriveJson;
18 |
19 | /**
20 | * Helper class used to parse the JSON directory with specified configuration options.
21 | */
22 | public class SwerveParser
23 | {
24 |
25 | /**
26 | * Module number mapped to the JSON name.
27 | */
28 | private static final HashMap moduleConfigs = new HashMap<>();
29 | /**
30 | * Parsed swervedrive.json
31 | */
32 | public static SwerveDriveJson swerveDriveJson;
33 | /**
34 | * Parsed controllerproperties.json
35 | */
36 | public static ControllerPropertiesJson controllerPropertiesJson;
37 | /**
38 | * Parsed modules/pidfproperties.json
39 | */
40 | public static PIDFPropertiesJson pidfPropertiesJson;
41 | /**
42 | * Parsed modules/physicalproperties.json
43 | */
44 | public static PhysicalPropertiesJson physicalPropertiesJson;
45 | /**
46 | * Array holding the module jsons given in {@link SwerveDriveJson}.
47 | */
48 | public static ModuleJson[] moduleJsons;
49 |
50 | /**
51 | * Construct a swerve parser. Will throw an error if there is a missing file.
52 | *
53 | * @param directory Directory with swerve configurations.
54 | * @throws IOException if a file doesn't exist.
55 | */
56 | public SwerveParser(File directory) throws IOException
57 | {
58 | checkDirectory(directory);
59 | swerveDriveJson =
60 | new ObjectMapper()
61 | .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
62 | .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
63 | controllerPropertiesJson =
64 | new ObjectMapper()
65 | .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
66 | .readValue(
67 | new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class);
68 | pidfPropertiesJson =
69 | new ObjectMapper()
70 | .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
71 | .readValue(
72 | new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
73 | physicalPropertiesJson =
74 | new ObjectMapper()
75 | .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
76 | .readValue(
77 | new File(directory, "modules/physicalproperties.json"),
78 | PhysicalPropertiesJson.class);
79 | moduleJsons = new ModuleJson[swerveDriveJson.modules.length];
80 | for (int i = 0; i < moduleJsons.length; i++)
81 | {
82 | moduleConfigs.put(swerveDriveJson.modules[i], i);
83 | File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]);
84 | assert moduleFile.exists();
85 | moduleJsons[i] = new ObjectMapper()
86 | .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
87 | .readValue(moduleFile, ModuleJson.class);
88 | }
89 | }
90 |
91 | /**
92 | * Get the swerve module by the json name.
93 | *
94 | * @param name JSON name.
95 | * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from.
96 | * @return {@link SwerveModuleConfiguration} based on the file.
97 | */
98 | public static SwerveModule getModuleConfigurationByName(
99 | String name, SwerveDriveConfiguration driveConfiguration)
100 | {
101 | return driveConfiguration.modules[moduleConfigs.get(name + ".json")];
102 | }
103 |
104 | /**
105 | * Open JSON file.
106 | *
107 | * @param file JSON File to open.
108 | * @return JsonNode of file.
109 | */
110 | private JsonNode openJson(File file)
111 | {
112 | try
113 | {
114 | return new ObjectMapper().readTree(file);
115 | } catch (IOException e)
116 | {
117 | throw new RuntimeException(e);
118 | }
119 | }
120 |
121 | /**
122 | * Check directory structure.
123 | *
124 | * @param directory JSON Configuration Directory
125 | */
126 | private void checkDirectory(File directory)
127 | {
128 | assert new File(directory, "swervedrive.json").exists();
129 | assert new File(directory, "controllerproperties.json").exists();
130 | assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory();
131 | assert new File(directory, "modules/pidfproperties.json").exists();
132 | assert new File(directory, "modules/physicalproperties.json").exists();
133 | }
134 |
135 | /**
136 | * Create {@link SwerveDrive} from JSON configuration directory.
137 | *
138 | * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular acceleration used in
139 | * {@link swervelib.SwerveController} and drive feedforward in
140 | * {@link SwerveMath#createDriveFeedforward(double, double, double)}.
141 | * @return {@link SwerveDrive} instance.
142 | */
143 | public SwerveDrive createSwerveDrive(double maxSpeed)
144 | {
145 | return createSwerveDrive(maxSpeed, Pose2d.kZero);
146 | }
147 |
148 | /**
149 | * Create {@link SwerveDrive} from JSON configuration directory.
150 | *
151 | * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular
152 | * acceleration used in {@link swervelib.SwerveController} and drive feedforward in
153 | * {@link SwerveMath#createDriveFeedforward(double, double, double)}.
154 | * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop
155 | * units to degrees, usually created using
156 | * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}.
157 | * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to
158 | * meters per rotation, usually created using
159 | * {@link SwerveMath#calculateMetersPerRotation(double, double, double)}.
160 | * @return {@link SwerveDrive} instance.
161 | */
162 | public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion)
163 | {
164 | physicalPropertiesJson.conversionFactors.angle.factor = angleMotorConversionFactor;
165 | physicalPropertiesJson.conversionFactors.drive.factor = driveMotorConversion;
166 | return createSwerveDrive(maxSpeed, Pose2d.kZero);
167 | }
168 |
169 | /**
170 | * Create {@link SwerveDrive} from JSON configuration directory.
171 | *
172 | * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in
173 | * {@link swervelib.SwerveController} of the robot
174 | * @param initialPose {@link Pose2d} initial pose.
175 | * @return {@link SwerveDrive} instance.
176 | */
177 | public SwerveDrive createSwerveDrive(double maxSpeed, Pose2d initialPose)
178 | {
179 | SwerveModuleConfiguration[] moduleConfigurations =
180 | new SwerveModuleConfiguration[moduleJsons.length];
181 | for (int i = 0; i < moduleConfigurations.length; i++)
182 | {
183 | ModuleJson module = moduleJsons[i];
184 | moduleConfigurations[i] =
185 | module.createModuleConfiguration(
186 | pidfPropertiesJson.angle,
187 | pidfPropertiesJson.drive,
188 | physicalPropertiesJson.createPhysicalProperties(),
189 | swerveDriveJson.modules[i]);
190 | }
191 | SwerveDriveConfiguration swerveDriveConfiguration =
192 | new SwerveDriveConfiguration(
193 | moduleConfigurations,
194 | swerveDriveJson.imu.createIMU(),
195 | swerveDriveJson.invertedIMU,
196 | physicalPropertiesJson.createPhysicalProperties());
197 |
198 | return new SwerveDrive(
199 | swerveDriveConfiguration,
200 | controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed),
201 | maxSpeed,
202 | initialPose);
203 | }
204 | }
205 |
--------------------------------------------------------------------------------
/swervelib/SwerveController.java:
--------------------------------------------------------------------------------
1 | package swervelib;
2 |
3 | import edu.wpi.first.math.controller.PIDController;
4 | import edu.wpi.first.math.filter.SlewRateLimiter;
5 | import edu.wpi.first.math.geometry.Translation2d;
6 | import edu.wpi.first.math.kinematics.ChassisSpeeds;
7 | import swervelib.parser.SwerveControllerConfiguration;
8 |
9 | /**
10 | * Controller class used to convert raw inputs into robot speeds.
11 | */
12 | public class SwerveController
13 | {
14 |
15 | /**
16 | * {@link SwerveControllerConfiguration} object storing data to generate the {@link PIDController} for controlling the
17 | * robot heading, and deadband for heading joystick.
18 | */
19 | public final SwerveControllerConfiguration config;
20 | /**
21 | * PID Controller for the robot heading.
22 | */
23 | public final PIDController thetaController; // TODO: Switch to ProfilePIDController
24 | /**
25 | * Last angle as a scalar [-1,1] the robot was set to.
26 | */
27 | public double lastAngleScalar;
28 | /**
29 | * {@link SlewRateLimiter} for movement in the X direction in meters/second.
30 | */
31 | public SlewRateLimiter xLimiter = null;
32 | /**
33 | * {@link SlewRateLimiter} for movement in the Y direction in meters/second.
34 | */
35 | public SlewRateLimiter yLimiter = null;
36 | /**
37 | * {@link SlewRateLimiter} for angular movement in radians/second.
38 | */
39 | public SlewRateLimiter angleLimiter = null;
40 |
41 | /**
42 | * Construct the SwerveController object which is used for determining the speeds of the robot based on controller
43 | * input.
44 | *
45 | * @param cfg {@link SwerveControllerConfiguration} containing the PIDF variables for the heading PIDF.
46 | */
47 | public SwerveController(SwerveControllerConfiguration cfg)
48 | {
49 | config = cfg;
50 | thetaController = config.headingPIDF.createPIDController();
51 | thetaController.enableContinuousInput(-Math.PI, Math.PI);
52 | lastAngleScalar = 0;
53 | }
54 |
55 | /**
56 | * Helper function to get the {@link Translation2d} of the chassis speeds given the {@link ChassisSpeeds}.
57 | *
58 | * @param speeds Chassis speeds.
59 | * @return {@link Translation2d} of the speed the robot is going in.
60 | */
61 | public static Translation2d getTranslation2d(ChassisSpeeds speeds)
62 | {
63 | return new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
64 | }
65 |
66 | /**
67 | * Add slew rate limiters to all controls. This prevents the robot from ramping up too much. To disable a
68 | * {@link SlewRateLimiter} set the desired one to null.
69 | *
70 | * @param x The {@link SlewRateLimiter} for the X velocity in meters/second.
71 | * @param y The {@link SlewRateLimiter} for the Y velocity in meters/second.
72 | * @param angle The {@link SlewRateLimiter} for the angular velocity in radians/second.
73 | */
74 | public void addSlewRateLimiters(SlewRateLimiter x, SlewRateLimiter y, SlewRateLimiter angle)
75 | {
76 | xLimiter = x;
77 | yLimiter = y;
78 | angleLimiter = angle;
79 | }
80 |
81 | /**
82 | * Calculate the hypot deadband and check if the joystick is within it.
83 | *
84 | * @param x The x value for the joystick in which the deadband should be applied.
85 | * @param y The y value for the joystick in which the deadband should be applied.
86 | * @return Whether the values are within the deadband from
87 | * {@link SwerveControllerConfiguration#angleJoyStickRadiusDeadband}.
88 | */
89 | public boolean withinHypotDeadband(double x, double y)
90 | {
91 | return Math.hypot(x, y) < config.angleJoyStickRadiusDeadband;
92 | }
93 |
94 | /**
95 | * Get the chassis speeds based on controller input of 1 joystick [-1,1] and an angle.
96 | *
97 | * @param xInput X joystick input for the robot to move in the X direction. X = xInput * maxSpeed
98 | * @param yInput Y joystick input for the robot to move in the Y direction. Y = yInput *
99 | * maxSpeed;
100 | * @param angle The desired angle of the robot in radians.
101 | * @param currentHeadingAngleRadians The current robot heading in radians.
102 | * @param maxSpeed Maximum speed in meters per second.
103 | * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
104 | */
105 | public ChassisSpeeds getTargetSpeeds(
106 | double xInput, double yInput, double angle, double currentHeadingAngleRadians, double maxSpeed)
107 | {
108 | // Convert joystick inputs to m/s by scaling by max linear speed. Also uses a cubic function
109 | // to allow for precise control and fast movement.
110 | double x = xInput * maxSpeed;
111 | double y = yInput * maxSpeed;
112 |
113 | return getRawTargetSpeeds(x, y, angle, currentHeadingAngleRadians);
114 | }
115 |
116 | /**
117 | * Get the angle in radians based off of the heading joysticks.
118 | *
119 | * @param headingX X joystick which controls the angle of the robot.
120 | * @param headingY Y joystick which controls the angle of the robot.
121 | * @return angle in radians from the joystick.
122 | */
123 | public double getJoystickAngle(double headingX, double headingY)
124 | {
125 | lastAngleScalar =
126 | withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY);
127 | return lastAngleScalar;
128 | }
129 |
130 | /**
131 | * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for
132 | * the angle of the robot.
133 | *
134 | * @param xInput X joystick input for the robot to move in the X direction.
135 | * @param yInput Y joystick input for the robot to move in the Y direction.
136 | * @param headingX X joystick which controls the angle of the robot.
137 | * @param headingY Y joystick which controls the angle of the robot.
138 | * @param currentHeadingAngleRadians The current robot heading in radians.
139 | * @param maxSpeed Maximum speed of the drive motors in meters per second, multiplier of the xInput
140 | * and yInput.
141 | * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive.
142 | */
143 | public ChassisSpeeds getTargetSpeeds(
144 | double xInput,
145 | double yInput,
146 | double headingX,
147 | double headingY,
148 | double currentHeadingAngleRadians,
149 | double maxSpeed)
150 | {
151 | // Converts the horizontal and vertical components to the commanded angle, in radians, unless
152 | // the joystick is near
153 | // the center (i. e. has been released), in which case the angle is held at the last valid
154 | // joystick input (hold
155 | // position when stick released).
156 | double angle =
157 | withinHypotDeadband(headingX, headingY) ? lastAngleScalar : Math.atan2(headingX, headingY);
158 | ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians, maxSpeed);
159 |
160 | // Used for the position hold feature
161 | lastAngleScalar = angle;
162 |
163 | return speeds;
164 | }
165 |
166 | /**
167 | * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in radians.
168 | *
169 | * @param xSpeed X speed in meters per second.
170 | * @param ySpeed Y speed in meters per second.
171 | * @param omega Angular velocity in radians/second.
172 | * @return {@link ChassisSpeeds} the robot should move to.
173 | */
174 | public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double omega)
175 | {
176 | if (xLimiter != null)
177 | {
178 | xSpeed = xLimiter.calculate(xSpeed);
179 | }
180 | if (yLimiter != null)
181 | {
182 | ySpeed = yLimiter.calculate(ySpeed);
183 | }
184 | if (angleLimiter != null)
185 | {
186 | omega = angleLimiter.calculate(omega);
187 | }
188 |
189 | return new ChassisSpeeds(xSpeed, ySpeed, omega);
190 | }
191 |
192 | /**
193 | * Get the {@link ChassisSpeeds} based of raw speeds desired in meters/second and heading in radians.
194 | *
195 | * @param xSpeed X speed in meters per second.
196 | * @param ySpeed Y speed in meters per second.
197 | * @param targetHeadingAngleRadians Target heading in radians.
198 | * @param currentHeadingAngleRadians Current heading in radians.
199 | * @return {@link ChassisSpeeds} the robot should move to.
200 | */
201 | public ChassisSpeeds getRawTargetSpeeds(double xSpeed, double ySpeed, double targetHeadingAngleRadians,
202 | double currentHeadingAngleRadians)
203 | {
204 | // Calculates an angular rate using a PIDController and the commanded angle. Returns a value between -1 and 1
205 | // which is then scaled to be between -maxAngularVelocity and +maxAngularVelocity.
206 | return getRawTargetSpeeds(xSpeed, ySpeed,
207 | thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) *
208 | config.maxAngularVelocity);
209 | }
210 |
211 | /**
212 | * Calculate the angular velocity given the current and target heading angle in radians.
213 | *
214 | * @param currentHeadingAngleRadians The current heading of the robot in radians.
215 | * @param targetHeadingAngleRadians The target heading of the robot in radians.
216 | * @return Angular velocity in radians per second.
217 | */
218 | public double headingCalculate(double currentHeadingAngleRadians, double targetHeadingAngleRadians)
219 | {
220 | return thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) * config.maxAngularVelocity;
221 | }
222 |
223 | /**
224 | * Set a new maximum angular velocity that is different from the auto-generated one. Modified the
225 | * {@link SwerveControllerConfiguration#maxAngularVelocity} field which is used in the {@link SwerveController} class
226 | * for {@link ChassisSpeeds} generation.
227 | *
228 | * @param angularVelocity Angular velocity in radians per second.
229 | */
230 | public void setMaximumChassisAngularVelocity(double angularVelocity)
231 | {
232 | config.maxAngularVelocity = angularVelocity;
233 | }
234 | }
235 |
--------------------------------------------------------------------------------
/swervelib/parser/json/DeviceJson.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser.json;
2 |
3 | import static swervelib.telemetry.SwerveDriveTelemetry.canIdWarning;
4 | import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning;
5 | import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning;
6 |
7 | import com.ctre.phoenix.motorcontrol.FeedbackDevice;
8 | import com.studica.frc.AHRS.NavXComType;
9 | import edu.wpi.first.math.system.plant.DCMotor;
10 | import edu.wpi.first.wpilibj.DriverStation;
11 | import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
12 | import swervelib.encoders.CANCoderSwerve;
13 | import swervelib.encoders.CanAndMagSwerve;
14 | import swervelib.encoders.DIODutyCycleEncoderSwerve;
15 | import swervelib.encoders.SparkFlexEncoderSwerve;
16 | import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
17 | import swervelib.encoders.SparkMaxEncoderSwerve;
18 | import swervelib.encoders.SwerveAbsoluteEncoder;
19 | import swervelib.encoders.TalonSRXEncoderSwerve;
20 | import swervelib.encoders.ThriftyNovaEncoderSwerve;
21 | import swervelib.imu.ADIS16448Swerve;
22 | import swervelib.imu.ADIS16470Swerve;
23 | import swervelib.imu.ADXRS450Swerve;
24 | import swervelib.imu.AnalogGyroSwerve;
25 | import swervelib.imu.CanandgyroSwerve;
26 | import swervelib.imu.NavXSwerve;
27 | import swervelib.imu.Pigeon2Swerve;
28 | import swervelib.imu.PigeonSwerve;
29 | import swervelib.imu.PigeonViaTalonSRXSwerve;
30 | import swervelib.imu.SwerveIMU;
31 | import swervelib.motors.SparkFlexSwerve;
32 | import swervelib.motors.SparkMaxBrushedMotorSwerve;
33 | import swervelib.motors.SparkMaxBrushedMotorSwerve.Type;
34 | import swervelib.motors.SparkMaxSwerve;
35 | import swervelib.motors.SwerveMotor;
36 | import swervelib.motors.TalonFXSSwerve;
37 | import swervelib.motors.TalonFXSwerve;
38 | import swervelib.motors.TalonSRXSwerve;
39 | import swervelib.motors.ThriftyNovaSwerve;
40 |
41 | /**
42 | * Device JSON parsed class. Used to access the JSON data.
43 | */
44 | public class DeviceJson
45 | {
46 |
47 | /**
48 | * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx
49 | */
50 | public String type;
51 | /**
52 | * The CAN ID or pin ID of the device.
53 | */
54 | public int id;
55 | /**
56 | * The CAN bus name which the device resides on if using CAN.
57 | */
58 | public String canbus = "";
59 |
60 | /**
61 | * Create a {@link SwerveAbsoluteEncoder} from the current configuration.
62 | *
63 | * @param motor {@link SwerveMotor} of which attached encoders will be created from, only used when the type is
64 | * "attached" or "canandencoder".
65 | * @return {@link SwerveAbsoluteEncoder} given.
66 | */
67 | public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor)
68 | {
69 | if (id > 40)
70 | {
71 | canIdWarning.set(true);
72 | }
73 | switch (type)
74 | {
75 | case "none":
76 | return null;
77 | case "integrated":
78 | case "attached":
79 | case "canandmag":
80 | case "canandcoder":
81 | return new SparkMaxEncoderSwerve(motor, 360);
82 | case "sparkmax_analog":
83 | return new SparkMaxAnalogEncoderSwerve(motor, 3.3);
84 | case "sparkmax_analog5v":
85 | return new SparkMaxAnalogEncoderSwerve(motor, 5);
86 | case "sparkflex_integrated":
87 | case "sparkflex_attached":
88 | case "sparkflex_canandmag":
89 | case "sparkflex_canandcoder":
90 | return new SparkFlexEncoderSwerve(motor, 360);
91 | case "canandcoder_can":
92 | case "canandmag_can":
93 | return new CanAndMagSwerve(id);
94 | case "ctre_mag":
95 | case "rev_hex":
96 | case "throughbore":
97 | case "am_mag":
98 | case "dutycycle":
99 | return new DIODutyCycleEncoderSwerve(id);
100 | case "thrifty":
101 | case "ma3":
102 | case "analog":
103 | return new AnalogAbsoluteEncoderSwerve(id);
104 | case "cancoder":
105 | return new CANCoderSwerve(id, canbus != null ? canbus : "");
106 | case "srxmag_standalone":
107 | return new TalonSRXEncoderSwerve(new TalonSRXSwerve(id, false, DCMotor.getCIM(1)),
108 | FeedbackDevice.PulseWidthEncodedPosition);
109 | case "talonsrx_pwm":
110 | return new TalonSRXEncoderSwerve(motor, FeedbackDevice.PulseWidthEncodedPosition);
111 | case "talonsrx_analog":
112 | return new TalonSRXEncoderSwerve(motor, FeedbackDevice.Analog);
113 | case "thrifty_nova":
114 | return new ThriftyNovaEncoderSwerve(motor);
115 | default:
116 | throw new RuntimeException(type + " is not a recognized absolute encoder type.");
117 | }
118 | }
119 |
120 | /**
121 | * Create a {@link SwerveIMU} from the given configuration.
122 | *
123 | * @return {@link SwerveIMU} given.
124 | */
125 | public SwerveIMU createIMU()
126 | {
127 | if (id > 40)
128 | {
129 | canIdWarning.set(true);
130 | }
131 | switch (type)
132 | {
133 | case "adis16448":
134 | return new ADIS16448Swerve();
135 | case "adis16470":
136 | return new ADIS16470Swerve();
137 | case "adxrs450":
138 | return new ADXRS450Swerve();
139 | case "analog":
140 | return new AnalogGyroSwerve(id);
141 | case "canandgyro":
142 | return new CanandgyroSwerve(id);
143 | case "navx":
144 | case "navx_spi":
145 | return new NavXSwerve(NavXComType.kMXP_SPI);
146 | case "navx_i2c":
147 | DriverStation.reportWarning(
148 | "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " +
149 | "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" +
150 | ".html#onboard-i2c-causing-system-lockups",
151 | false);
152 | i2cLockupWarning.set(true);
153 | return new NavXSwerve(NavXComType.kI2C);
154 | case "navx_usb":
155 | DriverStation.reportWarning("WARNING: There is issues when using USB camera's and the NavX like this!\n" +
156 | "https://pdocs.kauailabs.com/navx-mxp/guidance/selecting-an-interface/", false);
157 | serialCommsIssueWarning.set(true);
158 | return new NavXSwerve(NavXComType.kUSB1);
159 | case "navx_mxp_serial":
160 | serialCommsIssueWarning.set(true);
161 | return new NavXSwerve(NavXComType.kMXP_UART);
162 | case "pigeon":
163 | return new PigeonSwerve(id);
164 | case "pigeon_via_talonsrx":
165 | return new PigeonViaTalonSRXSwerve(id);
166 | case "pigeon2":
167 | return new Pigeon2Swerve(id, canbus != null ? canbus : "");
168 | default:
169 | throw new RuntimeException(type + " is not a recognized imu/gyroscope type.");
170 | }
171 | }
172 |
173 | /**
174 | * Create a {@link SwerveMotor} from the given configuration.
175 | *
176 | * @param isDriveMotor If the motor being generated is a drive motor.
177 | * @return {@link SwerveMotor} given.
178 | */
179 | public SwerveMotor createMotor(boolean isDriveMotor)
180 | {
181 | if (id > 40)
182 | {
183 | canIdWarning.set(true);
184 | }
185 | switch (type)
186 | {
187 | case "talonfxs_neo":
188 | return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNEO(1));
189 | case "talonfxs_neo550":
190 | return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeo550(1));
191 | case "talonfxs_vortex":
192 | return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeoVortex(1));
193 | case "talonfxs_minion":
194 | throw new UnsupportedOperationException("Cannot create minion combination yet"); //new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeoVortex(1));
195 | case "sparkmax_neo":
196 | case "neo":
197 | case "sparkmax":
198 | return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNEO(1));
199 | case "sparkmax_vortex":
200 | return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1));
201 | case "sparkmax_minion":
202 | throw new UnsupportedOperationException("Cannot create minion combination yet");
203 | case "sparkmax_neo550":
204 | case "neo550":
205 | return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
206 | case "sparkflex_vortex":
207 | case "vortex":
208 | case "sparkflex":
209 | return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1));
210 | case "sparkflex_neo":
211 | return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNEO(1));
212 | case "sparkflex_neo550":
213 | return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
214 | case "sparkflex_minion":
215 | throw new UnsupportedOperationException("Cannot create minion combination yet");
216 | case "falcon500":
217 | case "falcon":
218 | return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getFalcon500(1));
219 | case "falcon500foc":
220 | return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getFalcon500Foc(1));
221 | case "krakenx60":
222 | case "talonfx":
223 | return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getKrakenX60(1));
224 | case "krakenx60foc":
225 | return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getKrakenX60Foc(1));
226 | case "talonsrx":
227 | return new TalonSRXSwerve(id, isDriveMotor, DCMotor.getCIM(1));
228 | case "sparkmax_brushed":
229 | if (canbus == null)
230 | {
231 | canbus = "";
232 | }
233 | switch (canbus)
234 | {
235 | case "greyhill_63r256":
236 | return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false, DCMotor.getCIM(1));
237 | case "srx_mag_encoder":
238 | return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false, DCMotor.getCIM(1));
239 | case "throughbore":
240 | return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false, DCMotor.getCIM(1));
241 | case "throughbore_dataport":
242 | return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true, DCMotor.getCIM(1));
243 | case "greyhill_63r256_dataport":
244 | return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true, DCMotor.getCIM(1));
245 | case "srx_mag_encoder_dataport":
246 | return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true, DCMotor.getCIM(1));
247 | default:
248 | if (isDriveMotor)
249 | {
250 | throw new RuntimeException("Spark MAX " + id + " MUST have a encoder attached to the motor controller.");
251 | }
252 | // We are creating a motor for an angle motor which will use the absolute encoder attached to the data port.
253 | return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false, DCMotor.getCIM(1));
254 | }
255 | case "nova_neo":
256 | return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNEO(1));
257 | case "nova_neo550":
258 | return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
259 | case "nova_vortex":
260 | return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1));
261 | case "nova_minion":
262 | throw new UnsupportedOperationException("Cannot create minion combination");//return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getMinion(1));
263 | default:
264 | throw new RuntimeException(type + " is not a recognized motor type.");
265 | }
266 | }
267 | }
268 |
--------------------------------------------------------------------------------
/LICENSE.md:
--------------------------------------------------------------------------------
1 | Apache License
2 | Version 2.0, January 2004
3 | http://www.apache.org/licenses/
4 |
5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
6 |
7 | 1. Definitions.
8 |
9 | "License" shall mean the terms and conditions for use, reproduction,
10 | and distribution as defined by Sections 1 through 9 of this document.
11 |
12 | "Licensor" shall mean the copyright owner or entity authorized by
13 | the copyright owner that is granting the License.
14 |
15 | "Legal Entity" shall mean the union of the acting entity and all
16 | other entities that control, are controlled by, or are under common
17 | control with that entity. For the purposes of this definition,
18 | "control" means (i) the power, direct or indirect, to cause the
19 | direction or management of such entity, whether by contract or
20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the
21 | outstanding shares, or (iii) beneficial ownership of such entity.
22 |
23 | "You" (or "Your") shall mean an individual or Legal Entity
24 | exercising permissions granted by this License.
25 |
26 | "Source" form shall mean the preferred form for making modifications,
27 | including but not limited to software source code, documentation
28 | source, and configuration files.
29 |
30 | "Object" form shall mean any form resulting from mechanical
31 | transformation or translation of a Source form, including but
32 | not limited to compiled object code, generated documentation,
33 | and conversions to other media types.
34 |
35 | "Work" shall mean the work of authorship, whether in Source or
36 | Object form, made available under the License, as indicated by a
37 | copyright notice that is included in or attached to the work
38 | (an example is provided in the Appendix below).
39 |
40 | "Derivative Works" shall mean any work, whether in Source or Object
41 | form, that is based on (or derived from) the Work and for which the
42 | editorial revisions, annotations, elaborations, or other modifications
43 | represent, as a whole, an original work of authorship. For the purposes
44 | of this License, Derivative Works shall not include works that remain
45 | separable from, or merely link (or bind by name) to the interfaces of,
46 | the Work and Derivative Works thereof.
47 |
48 | "Contribution" shall mean any work of authorship, including
49 | the original version of the Work and any modifications or additions
50 | to that Work or Derivative Works thereof, that is intentionally
51 | submitted to Licensor for inclusion in the Work by the copyright owner
52 | or by an individual or Legal Entity authorized to submit on behalf of
53 | the copyright owner. For the purposes of this definition, "submitted"
54 | means any form of electronic, verbal, or written communication sent
55 | to the Licensor or its representatives, including but not limited to
56 | communication on electronic mailing lists, source code control systems,
57 | and issue tracking systems that are managed by, or on behalf of, the
58 | Licensor for the purpose of discussing and improving the Work, but
59 | excluding communication that is conspicuously marked or otherwise
60 | designated in writing by the copyright owner as "Not a Contribution."
61 |
62 | "Contributor" shall mean Licensor and any individual or Legal Entity
63 | on behalf of whom a Contribution has been received by Licensor and
64 | subsequently incorporated within the Work.
65 |
66 | 2. Grant of Copyright License. Subject to the terms and conditions of
67 | this License, each Contributor hereby grants to You a perpetual,
68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
69 | copyright license to reproduce, prepare Derivative Works of,
70 | publicly display, publicly perform, sublicense, and distribute the
71 | Work and such Derivative Works in Source or Object form.
72 |
73 | 3. Grant of Patent License. Subject to the terms and conditions of
74 | this License, each Contributor hereby grants to You a perpetual,
75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
76 | (except as stated in this section) patent license to make, have made,
77 | use, offer to sell, sell, import, and otherwise transfer the Work,
78 | where such license applies only to those patent claims licensable
79 | by such Contributor that are necessarily infringed by their
80 | Contribution(s) alone or by combination of their Contribution(s)
81 | with the Work to which such Contribution(s) was submitted. If You
82 | institute patent litigation against any entity (including a
83 | cross-claim or counterclaim in a lawsuit) alleging that the Work
84 | or a Contribution incorporated within the Work constitutes direct
85 | or contributory patent infringement, then any patent licenses
86 | granted to You under this License for that Work shall terminate
87 | as of the date such litigation is filed.
88 |
89 | 4. Redistribution. You may reproduce and distribute copies of the
90 | Work or Derivative Works thereof in any medium, with or without
91 | modifications, and in Source or Object form, provided that You
92 | meet the following conditions:
93 |
94 | (a) You must give any other recipients of the Work or
95 | Derivative Works a copy of this License; and
96 |
97 | (b) You must cause any modified files to carry prominent notices
98 | stating that You changed the files; and
99 |
100 | (c) You must retain, in the Source form of any Derivative Works
101 | that You distribute, all copyright, patent, trademark, and
102 | attribution notices from the Source form of the Work,
103 | excluding those notices that do not pertain to any part of
104 | the Derivative Works; and
105 |
106 | (d) If the Work includes a "NOTICE" text file as part of its
107 | distribution, then any Derivative Works that You distribute must
108 | include a readable copy of the attribution notices contained
109 | within such NOTICE file, excluding those notices that do not
110 | pertain to any part of the Derivative Works, in at least one
111 | of the following places: within a NOTICE text file distributed
112 | as part of the Derivative Works; within the Source form or
113 | documentation, if provided along with the Derivative Works; or,
114 | within a display generated by the Derivative Works, if and
115 | wherever such third-party notices normally appear. The contents
116 | of the NOTICE file are for informational purposes only and
117 | do not modify the License. You may add Your own attribution
118 | notices within Derivative Works that You distribute, alongside
119 | or as an addendum to the NOTICE text from the Work, provided
120 | that such additional attribution notices cannot be construed
121 | as modifying the License.
122 |
123 | You may add Your own copyright statement to Your modifications and
124 | may provide additional or different license terms and conditions
125 | for use, reproduction, or distribution of Your modifications, or
126 | for any such Derivative Works as a whole, provided Your use,
127 | reproduction, and distribution of the Work otherwise complies with
128 | the conditions stated in this License.
129 |
130 | 5. Submission of Contributions. Unless You explicitly state otherwise,
131 | any Contribution intentionally submitted for inclusion in the Work
132 | by You to the Licensor shall be under the terms and conditions of
133 | this License, without any additional terms or conditions.
134 | Notwithstanding the above, nothing herein shall supersede or modify
135 | the terms of any separate license agreement you may have executed
136 | with Licensor regarding such Contributions.
137 |
138 | 6. Trademarks. This License does not grant permission to use the trade
139 | names, trademarks, service marks, or product names of the Licensor,
140 | except as required for reasonable and customary use in describing the
141 | origin of the Work and reproducing the content of the NOTICE file.
142 |
143 | 7. Disclaimer of Warranty. Unless required by applicable law or
144 | agreed to in writing, Licensor provides the Work (and each
145 | Contributor provides its Contributions) on an "AS IS" BASIS,
146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 | implied, including, without limitation, any warranties or conditions
148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 | PARTICULAR PURPOSE. You are solely responsible for determining the
150 | appropriateness of using or redistributing the Work and assume any
151 | risks associated with Your exercise of permissions under this License.
152 |
153 | 8. Limitation of Liability. In no event and under no legal theory,
154 | whether in tort (including negligence), contract, or otherwise,
155 | unless required by applicable law (such as deliberate and grossly
156 | negligent acts) or agreed to in writing, shall any Contributor be
157 | liable to You for damages, including any direct, indirect, special,
158 | incidental, or consequential damages of any character arising as a
159 | result of this License or out of the use or inability to use the
160 | Work (including but not limited to damages for loss of goodwill,
161 | work stoppage, computer failure or malfunction, or any and all
162 | other commercial damages or losses), even if such Contributor
163 | has been advised of the possibility of such damages.
164 |
165 | 9. Accepting Warranty or Additional Liability. While redistributing
166 | the Work or Derivative Works thereof, You may choose to offer,
167 | and charge a fee for, acceptance of support, warranty, indemnity,
168 | or other liability obligations and/or rights consistent with this
169 | License. However, in accepting such obligations, You may act only
170 | on Your own behalf and on Your sole responsibility, not on behalf
171 | of any other Contributor, and only if You agree to indemnify,
172 | defend, and hold each Contributor harmless for any liability
173 | incurred by, or claims asserted against, such Contributor by reason
174 | of your accepting any such warranty or additional liability.
175 |
176 | END OF TERMS AND CONDITIONS
177 |
178 | APPENDIX: How to apply the Apache License to your work.
179 |
180 | To apply the Apache License to your work, attach the following
181 | boilerplate notice, with the fields enclosed by brackets "[]"
182 | replaced with your own identifying information. (Don't include
183 | the brackets!) The text should be enclosed in the appropriate
184 | comment syntax for the file format. We also recommend that a
185 | file or class name and description of purpose be included on the
186 | same "printed page" as the copyright notice for easier
187 | identification within third-party archives.
188 |
189 | Copyright 2023 BroncBotz3481
190 |
191 | Licensed under the Apache License, Version 2.0 (the "License");
192 | you may not use this file except in compliance with the License.
193 | You may obtain a copy of the License at
194 |
195 | http://www.apache.org/licenses/LICENSE-2.0
196 |
197 | Unless required by applicable law or agreed to in writing, software
198 | distributed under the License is distributed on an "AS IS" BASIS,
199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 | See the License for the specific language governing permissions and
201 | limitations under the License.
202 |
--------------------------------------------------------------------------------
54 |
--------------------------------------------------------------------------------
/swervelib/parser/PIDFConfig.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser;
2 |
3 | import edu.wpi.first.math.controller.PIDController;
4 | import swervelib.parser.deserializer.PIDFRange;
5 |
6 | /**
7 | * Hold the PIDF and Integral Zone values for a PID.
8 | */
9 | public class PIDFConfig
10 | {
11 |
12 | /**
13 | * Proportional Gain for PID.
14 | */
15 | public double p;
16 | /**
17 | * Integral Gain for PID.
18 | */
19 | public double i;
20 | /**
21 | * Derivative Gain for PID.
22 | */
23 | public double d;
24 | /**
25 | * Feedforward value for PID.
26 | */
27 | public double f;
28 | /**
29 | * Integral zone of the PID.
30 | */
31 | public double iz;
32 |
33 | /**
34 | * The PIDF output range.
35 | */
36 | public PIDFRange output = new PIDFRange();
37 |
38 | /**
39 | * Used when parsing PIDF values from JSON.
40 | */
41 | public PIDFConfig()
42 | {
43 | }
44 |
45 | /**
46 | * PIDF Config constructor to contain the values.
47 | *
48 | * @param p P gain.
49 | * @param i I gain.
50 | * @param d D gain.
51 | * @param f F gain.
52 | * @param iz Intergral zone.
53 | */
54 | public PIDFConfig(double p, double i, double d, double f, double iz)
55 | {
56 | this.p = p;
57 | this.i = i;
58 | this.d = d;
59 | this.f = f;
60 | this.iz = iz;
61 | }
62 |
63 | /**
64 | * PIDF Config constructor to contain the values.
65 | *
66 | * @param p P gain.
67 | * @param i I gain.
68 | * @param d D gain.
69 | * @param f F gain.
70 | */
71 | public PIDFConfig(double p, double i, double d, double f)
72 | {
73 | this(p, i, d, f, 0);
74 | }
75 |
76 | /**
77 | * PIDF Config constructor to contain the values.
78 | *
79 | * @param p P gain.
80 | * @param i I gain.
81 | * @param d D gain.
82 | */
83 | public PIDFConfig(double p, double i, double d)
84 | {
85 | this(p, i, d, 0, 0);
86 | }
87 |
88 | /**
89 | * PIDF Config constructor to contain the values.
90 | *
91 | * @param p P gain.
92 | * @param d D gain.
93 | */
94 | public PIDFConfig(double p, double d)
95 | {
96 | this(p, 0, d, 0, 0);
97 | }
98 |
99 | /**
100 | * Create a PIDController from the PID values.
101 | *
102 | * @return PIDController.
103 | */
104 | public PIDController createPIDController()
105 | {
106 | PIDController pidController = new PIDController(p, i, d);
107 | if (iz != 0) {
108 | pidController.setIZone(iz);
109 | }
110 | return pidController;
111 | }
112 | }
113 |
--------------------------------------------------------------------------------
/swervelib/parser/Cache.java:
--------------------------------------------------------------------------------
1 | package swervelib.parser;
2 |
3 | import edu.wpi.first.wpilibj.RobotBase;
4 | import edu.wpi.first.wpilibj.RobotController;
5 | import java.util.function.Supplier;
6 |
7 | /**
8 | * Cache for frequently requested data.
9 | */
10 | public class Cache