mirror of
https://github.com/BroncBotz3481/YAGSL
synced 2026-06-19 06:21:40 +00:00
Added overrides for canandcoder, falcon/talonfx for encoder pulse per rotation configuration option.
This commit is contained in:
@@ -3,7 +3,6 @@ package swervelib.parser;
|
||||
import static swervelib.math.SwerveMath.calculateDegreesPerSteeringRotation;
|
||||
import static swervelib.math.SwerveMath.calculateMaxAcceleration;
|
||||
import static swervelib.math.SwerveMath.calculateMetersPerRotation;
|
||||
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import swervelib.encoders.SwerveAbsoluteEncoder;
|
||||
@@ -200,7 +199,7 @@ public class SwerveModuleConfiguration
|
||||
? calculateMetersPerRotation(
|
||||
physicalCharacteristics.wheelDiameter,
|
||||
physicalCharacteristics.driveGearRatio,
|
||||
angleMotorEncoderPulsePerRevolution)
|
||||
physicalCharacteristics.driveEncoderPulsePerRotation)
|
||||
: calculateDegreesPerSteeringRotation(
|
||||
physicalCharacteristics.angleGearRatio,
|
||||
angleMotorEncoderPulsePerRevolution);
|
||||
|
||||
@@ -169,4 +169,24 @@ public class DeviceJson
|
||||
throw new RuntimeException(
|
||||
"Could not create absolute encoder from data port of " + type + " id " + id);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the encoder pulse per rotation based off of the encoder type.
|
||||
*
|
||||
* @param angleEncoderPulsePerRotation The configured pulse per rotation.
|
||||
* @return The correct pulse per rotation based off of the encoder type.
|
||||
*/
|
||||
public int getPulsePerRotation(int angleEncoderPulsePerRotation)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case "canandcoder":
|
||||
return 360;
|
||||
case "falcon":
|
||||
case "talonfx":
|
||||
return 2048;
|
||||
default:
|
||||
return angleEncoderPulsePerRotation;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -68,6 +68,14 @@ public class ModuleJson
|
||||
SwerveMotor angleMotor = angle.createMotor(false);
|
||||
SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor);
|
||||
|
||||
// Override angle encoder pulse per rotation based on the encoder and motor type.
|
||||
int encoderPulseOverride = encoder.getPulsePerRotation(physicalCharacteristics.angleEncoderPulsePerRotation);
|
||||
int motorPulseOverride = angle.getPulsePerRotation(physicalCharacteristics.angleEncoderPulsePerRotation);
|
||||
|
||||
int angleEncoderPulsePerRotation =
|
||||
motorPulseOverride != physicalCharacteristics.angleEncoderPulsePerRotation ? motorPulseOverride
|
||||
: encoderPulseOverride;
|
||||
|
||||
// If the absolute encoder is attached.
|
||||
if (absEncoder == null)
|
||||
{
|
||||
@@ -89,7 +97,7 @@ public class ModuleJson
|
||||
absoluteEncoderInverted,
|
||||
inverted.drive,
|
||||
inverted.angle,
|
||||
angleEncoderPulsePerRevolution == 0 ? physicalCharacteristics.angleEncoderPulsePerRotation
|
||||
angleEncoderPulsePerRevolution == 0 ? angleEncoderPulsePerRotation
|
||||
: angleEncoderPulsePerRevolution,
|
||||
name.replaceAll("\\.json", ""));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user