From f88fdfb6cf5128bed882ce563ce8a2363a7b0cee Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Wed, 9 Aug 2023 15:05:33 -0500 Subject: [PATCH] Added support for CANandCoder --- swervelib/encoders/CanAndCoderSwerve.java | 60 +++++++++---------- swervelib/encoders/SparkMaxEncoderSwerve.java | 7 ++- swervelib/parser/json/DeviceJson.java | 39 +++++++----- swervelib/parser/json/ModuleJson.java | 7 +++ 4 files changed, 66 insertions(+), 47 deletions(-) diff --git a/swervelib/encoders/CanAndCoderSwerve.java b/swervelib/encoders/CanAndCoderSwerve.java index e1489be..71d3a61 100644 --- a/swervelib/encoders/CanAndCoderSwerve.java +++ b/swervelib/encoders/CanAndCoderSwerve.java @@ -1,61 +1,59 @@ package swervelib.encoders; -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkMaxAbsoluteEncoder.Type; -import swervelib.motors.SwerveMotor; +import com.reduxrobotics.sensors.canandcoder.CANandcoder; /** - * SparkMax absolute encoder, attached through the data port. + * HELIUM {@link CANandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus. */ -public class CanAndCoderSwerve extends SwerveAbsoluteEncoder { +public class CanAndCoderSwerve extends SwerveAbsoluteEncoder +{ /** - * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to - * the SparkMax. + * The {@link CANandcoder} representing the CANandCoder on the CAN bus. */ - public AbsoluteEncoder encoder; + public CANandcoder encoder; + /** + * Inversion state of the encoder. + */ + private boolean inverted = false; /** - * Create the {@link AbsoluteEncoder} object as a duty cycle. from the - * {@link CANSparkMax} motor. + * Create the {@link CANandcoder} * - * @param motor Motor to create the encoder from. + * @param canid The CAN ID whenever the CANandCoder is operating on the CANBus. */ - public CanAndCoderSwerve(SwerveMotor motor) { - if (motor.getMotor() instanceof CANSparkMax) { - encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle); - encoder.setPositionConversionFactor(360); - encoder.setVelocityConversionFactor(360); - } else { - throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); - } + public CanAndCoderSwerve(int canid) + { + encoder = new CANandcoder(canid); } /** * Reset the encoder to factory defaults. */ @Override - public void factoryDefault() { - // Do nothing + public void factoryDefault() + { + encoder.resetFactoryDefaults(false); } /** * Clear sticky faults on the encoder. */ @Override - public void clearStickyFaults() { - // Do nothing + public void clearStickyFaults() + { + encoder.clearStickyFaults(); } /** - * Configure the absolute encoder to read from [0, 360) per second. + * Configure the CANandCoder to read from [0, 360) per second. * * @param inverted Whether the encoder is inverted. */ @Override - public void configure(boolean inverted) { - encoder.setInverted(inverted); + public void configure(boolean inverted) + { + this.inverted = inverted; } /** @@ -64,8 +62,9 @@ public class CanAndCoderSwerve extends SwerveAbsoluteEncoder { * @return Absolute position in degrees from [0, 360). */ @Override - public double getAbsolutePosition() { - return encoder.getPosition(); + public double getAbsolutePosition() + { + return (inverted ? -1.0 : 1.0) * encoder.getPosition() * 360; } /** @@ -74,7 +73,8 @@ public class CanAndCoderSwerve extends SwerveAbsoluteEncoder { * @return Absolute encoder object. */ @Override - public Object getAbsoluteEncoder() { + public Object getAbsoluteEncoder() + { return encoder; } } diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java index cc2aa24..0e95953 100644 --- a/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -19,13 +19,16 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder /** * Create the {@link AbsoluteEncoder} object as a duty cycle. from the {@link CANSparkMax} motor. * - * @param motor Motor to create the encoder from. + * @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 SparkMaxEncoderSwerve(SwerveMotor motor) + public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) { if (motor.getMotor() instanceof CANSparkMax) { encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle); + encoder.setVelocityConversionFactor(conversionFactor); + encoder.setPositionConversionFactor(conversionFactor); } else { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java index 2ff1b18..b27b6da 100644 --- a/swervelib/parser/json/DeviceJson.java +++ b/swervelib/parser/json/DeviceJson.java @@ -49,27 +49,16 @@ public class DeviceJson */ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) { - SwerveAbsoluteEncoder attachedChoice = null; switch (type) { case "none": case "integrated": case "attached": - if (motor instanceof SparkMaxBrushedMotorSwerve || motor instanceof SparkMaxSwerve) - { - attachedChoice = new SparkMaxEncoderSwerve(motor); - motor.setAbsoluteEncoder(attachedChoice); - } else if (motor instanceof TalonFXSwerve || motor instanceof TalonSRXSwerve) - { - motor.setAbsoluteEncoder(null); - } else - { - throw new RuntimeException( - "Could not create absolute encoder from data port of " + type + " id " + id); - } - return attachedChoice; + return null; case "canandcoder": - return new CanAndCoderSwerve(motor); + return new SparkMaxEncoderSwerve(motor, 360); + case "canandcoder_can": + return new CanAndCoderSwerve(id); case "thrifty": case "throughbore": case "dutycycle": @@ -160,4 +149,24 @@ public class DeviceJson throw new RuntimeException(type + " is not a recognized absolute encoder type."); } } + + /** + * Create a {@link SwerveAbsoluteEncoder} from the data port on the motor controller. + * + * @param motor The motor to create the absolute encoder from. + * @return {@link SwerveAbsoluteEncoder} from the motor controller. + */ + public SwerveAbsoluteEncoder createIntegratedEncoder(SwerveMotor motor) + { + switch (type) + { + case "sparkmax": + return new SparkMaxEncoderSwerve(motor, 1); + case "falcon": + case "talonfx": + return null; + } + throw new RuntimeException( + "Could not create absolute encoder from data port of " + type + " id " + id); + } } diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java index 34951d0..208a34a 100644 --- a/swervelib/parser/json/ModuleJson.java +++ b/swervelib/parser/json/ModuleJson.java @@ -68,6 +68,13 @@ public class ModuleJson SwerveMotor angleMotor = angle.createMotor(false); SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); + // If the absolute encoder is attached. + if (absEncoder == null) + { + absEncoder = angle.createIntegratedEncoder(angleMotor); + angleMotor.setAbsoluteEncoder(absEncoder); + } + return new SwerveModuleConfiguration( drive.createMotor(true), angleMotor,