diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index f08e74cf..b8d2fec0 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -127,6 +127,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig Constants.MAX_SPEED, new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), Rotation2d.fromDegrees(0))); + } /** diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 5740a560..72732d5b 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -11,6 +11,7 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -58,12 +59,14 @@ import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; import swervelib.encoders.CANCoderSwerve; import swervelib.imu.Pigeon2Swerve; +import swervelib.imu.PigeonViaTalonSRXSwerve; import swervelib.imu.SwerveIMU; import swervelib.math.SwerveMath; import swervelib.motors.TalonFXSwerve; import swervelib.parser.Cache; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; +import swervelib.parser.SwerveParser; import swervelib.simulation.SwerveIMUSimulation; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -185,6 +188,10 @@ public class SwerveDrive * Simulation of the swerve drive. */ private SwerveIMUSimulation simIMU; + /** + * Pigeon attached to Talon class + */ + public final PigeonViaTalonSRXSwerve pigeonViaTalonSRXSwerve = new PigeonViaTalonSRXSwerve(SwerveParser.swerveDriveJson.imu.id); /** * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ @@ -944,7 +951,6 @@ public SwerveModulePosition[] getModulePositions() } return positions; } - /** * Getter for the {@link SwerveIMU}. * @@ -1053,6 +1059,16 @@ public Optional getAccel() } } + + /** + * Gets the {@link TalonSRX} that the WPI_PigeonIMU is attached to. + * + * @return the {@link TalonSRX} created for the WPI_PigeonIMU. + */ + public TalonSRX getPigeonAttachedTalonSRX() { + return pigeonViaTalonSRXSwerve.getTalonSRX(); + } + /** * Sets the drive motors to brake/coast mode. * diff --git a/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java new file mode 100644 index 00000000..b6257f13 --- /dev/null +++ b/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java @@ -0,0 +1,170 @@ +package swervelib.encoders; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SwerveMotor; + +import java.util.function.Supplier; + +/** + * SparkMax absolute encoder, attached through the data port. + */ +public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. + */ + public SparkAbsoluteEncoder encoder; + /** + * An {@link Alert} for if there is a failure configuring the encoder. + */ + private Alert failureConfiguring; + /** + * An {@link Alert} for if there is a failure configuring the encoder offset. + */ + private Alert offsetFailure; + /** + * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance. + */ + private SwerveMotor sparkFlex; + + /** + * Create the {@link SparkFlexEncoderSwerve} object as a duty cycle from the {@link SparkFlex} + * motor. + * + * @param motor Motor to create the encoder from. + * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. + */ + public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor) + { + failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Absolute Encoder", + AlertType.kWarning); + offsetFailure = new Alert( + "Encoders", + "Failure to set Absolute Encoder Offset", + AlertType.kWarning); + if (motor.getMotor() instanceof SparkFlex) + { + sparkFlex = motor; + encoder = ((SparkFlex) motor.getMotor()).getAbsoluteEncoder(); + motor.setAbsoluteEncoder(this); + motor.configureIntegratedEncoder(conversionFactor); + } else + { + throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + } + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) + { + for (int i = 0; i < maximumRetries; i++) + { + if (config.get() == REVLibError.kOk) + { + return; + } + } + failureConfiguring.set(true); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + // Do nothing + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + // Do nothing + } + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + if (sparkFlex instanceof SparkMaxSwerve) + { + SparkMaxConfig cfg = ((SparkMaxSwerve) sparkFlex).getConfig(); + cfg.absoluteEncoder.inverted(inverted); + ((SparkMaxSwerve) sparkFlex).updateConfig(cfg); + } + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return encoder.getPosition(); + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + if (sparkFlex instanceof SparkMaxSwerve) + { + SparkMaxConfig cfg = ((SparkMaxSwerve) sparkFlex).getConfig(); + cfg.absoluteEncoder.zeroOffset(offset); + ((SparkMaxSwerve) sparkFlex).updateConfig(cfg); + return true; + } + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + return encoder.getVelocity(); + } +} diff --git a/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java new file mode 100644 index 00000000..d1fdcab6 --- /dev/null +++ b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java @@ -0,0 +1,156 @@ +package swervelib.imu; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.util.Optional; + +import static edu.wpi.first.units.Units.DegreesPerSecond; + +/** + * SwerveIMU interface for the {@link WPI_PigeonIMU}. + */ +public class PigeonViaTalonSRXSwerve extends SwerveIMU +{ + + + /** + * {@link TalonSRX} TalonSRX the IMU is attached to. + */ + private final TalonSRX talon; + + /** + * {@link WPI_PigeonIMU} IMU device. + */ + private final WPI_PigeonIMU imu; + /** + * Mutable {@link AngularVelocity} for readings. + */ + private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); + /** + * Offset for the {@link WPI_PigeonIMU}. + */ + private Rotation3d offset = new Rotation3d(); + /** + * Inversion for the gyro + */ + private boolean invertedIMU = false; + + /** + * Generate the SwerveIMU for {@link WPI_PigeonIMU} attached to a {@link TalonSRX}. + * + * @param canid CAN ID for the {@link TalonSRX} the {@link WPI_PigeonIMU} is attached to, does not support CANBus. + */ + public PigeonViaTalonSRXSwerve(int canid) + { + talon = new TalonSRX(canid); + imu = new WPI_PigeonIMU(talon); + offset = new Rotation3d(); + SmartDashboard.putData(imu); + } + + public TalonSRX getTalonSRX() { + return talon; + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + imu.configFactoryDefault(); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + imu.clearStickyFaults(); + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Set the gyro to invert its default direction + * + * @param invertIMU invert gyro direction + */ + public void setInverted(boolean invertIMU) + { + invertedIMU = invertIMU; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() + { + double[] wxyz = new double[4]; + imu.get6dQuaternion(wxyz); + Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); + return invertedIMU ? reading.unaryMinus() : reading; + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + short[] initial = new short[3]; + imu.getBiasedAccelerometer(initial); + return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0)); + } + + @Override + public MutAngularVelocity getYawAngularVelocity() + { + return yawVel.mut_setMagnitude(imu.getRate()); + } + + /** + * Get the instantiated {@link WPI_PigeonIMU} IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } +} diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 8cc33213..65f4b5eb 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -9,24 +9,8 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DriverStation; -import swervelib.encoders.AnalogAbsoluteEncoderSwerve; -import swervelib.encoders.CANCoderSwerve; -import swervelib.encoders.CanAndMagSwerve; -import swervelib.encoders.PWMDutyCycleEncoderSwerve; -import swervelib.encoders.SparkMaxAnalogEncoderSwerve; -import swervelib.encoders.SparkMaxEncoderSwerve; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.encoders.TalonSRXEncoderSwerve; -import swervelib.encoders.ThriftyNovaEncoderSwerve; -import swervelib.imu.ADIS16448Swerve; -import swervelib.imu.ADIS16470Swerve; -import swervelib.imu.ADXRS450Swerve; -import swervelib.imu.AnalogGyroSwerve; -import swervelib.imu.CanandgyroSwerve; -import swervelib.imu.NavXSwerve; -import swervelib.imu.Pigeon2Swerve; -import swervelib.imu.PigeonSwerve; -import swervelib.imu.SwerveIMU; +import swervelib.encoders.*; +import swervelib.imu.*; import swervelib.motors.SparkFlexSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve.Type; @@ -81,6 +65,11 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) return new SparkMaxAnalogEncoderSwerve(motor, 3.3); case "sparkmax_analog5v": return new SparkMaxAnalogEncoderSwerve(motor, 5); + case "sparkflex_integrated": + case "sparkflex_attached": + case "sparkflex_canandmag": + case "sparkflex_canandcoder": + return new SparkFlexEncoderSwerve(motor, 360); case "canandcoder_can": case "canandmag_can": return new CanAndMagSwerve(id); @@ -154,6 +143,8 @@ public SwerveIMU createIMU() return new NavXSwerve(NavXComType.kMXP_UART); case "pigeon": return new PigeonSwerve(id); + case "pigeon_via_talonsrx" : + return new PigeonViaTalonSRXSwerve(id); case "pigeon2": return new Pigeon2Swerve(id, canbus != null ? canbus : ""); default: