Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Updated to 2024

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-15 14:37:13 -06:00
parent 3fd8493ccb
commit a71d7a0b4e
185 changed files with 6910 additions and 10987 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
swervelib/encoders/CANCoderSwerve.java
View File

@@ -1,12 +1,14 @@
package swervelib.encoders;
import com.ctre.phoenix.ErrorCode;
import com.ctre.phoenix.sensors.AbsoluteSensorRange;
import com.ctre.phoenix.sensors.CANCoderConfiguration;
import com.ctre.phoenix.sensors.MagnetFieldStrength;
import com.ctre.phoenix.sensors.SensorInitializationStrategy;
import com.ctre.phoenix.sensors.SensorTimeBase;
import com.ctre.phoenix.sensors.WPI_CANCoder;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.CANcoderConfigurator;
import com.ctre.phoenix6.configs.MagnetSensorConfigs;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
import com.ctre.phoenix6.signals.MagnetHealthValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import edu.wpi.first.wpilibj.DriverStation;
/**
@@ -18,7 +20,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
/**
* CANCoder with WPILib sendable and support.
*/
public WPI_CANCoder encoder;
public CANcoder encoder;
/**
* Initialize the CANCoder on the standard CANBus.
@@ -27,7 +29,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
*/
public CANCoderSwerve(int id)
{
encoder = new WPI_CANCoder(id);
encoder = new CANcoder(id);
}
/**
@@ -38,7 +40,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
*/
public CANCoderSwerve(int id, String canbus)
{
encoder = new WPI_CANCoder(id, canbus);
encoder = new CANcoder(id, canbus);
}
/**
@@ -47,7 +49,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public void factoryDefault()
{
encoder.configFactoryDefault();
encoder.getConfigurator().apply(new CANcoderConfiguration());
}
/**
@@ -67,13 +69,13 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public void configure(boolean inverted)
{
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
canCoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360;
canCoderConfiguration.sensorDirection = inverted;
canCoderConfiguration.initializationStrategy =
SensorInitializationStrategy.BootToAbsolutePosition;
canCoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond;
encoder.configAllSettings(canCoderConfiguration);
CANcoderConfigurator cfg = encoder.getConfigurator();
MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs();
cfg.refresh(magnetSensorConfiguration);
cfg.apply(magnetSensorConfiguration
.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
.withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive));
}
/**
@@ -85,46 +87,38 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
public double getAbsolutePosition()
{
readingError = false;
MagnetFieldStrength strength = encoder.getMagnetFieldStrength();
MagnetHealthValue strength = encoder.getMagnetHealth().getValue();
if (strength != MagnetFieldStrength.Good_GreenLED)
if (strength != MagnetHealthValue.Magnet_Green)
{
DriverStation.reportWarning(
"CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false);
}
if (strength == MagnetFieldStrength.Invalid_Unknown || strength == MagnetFieldStrength.BadRange_RedLED)
if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red)
{
readingError = true;
DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false);
return 0;
}
double angle = encoder.getAbsolutePosition();
StatusSignal<Double> angle = encoder.getAbsolutePosition().refresh();
// Taken from democat's library.
// Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74
ErrorCode code = encoder.getLastError();
for (int i = 0; i < maximumRetries; i++)
{
if (code == ErrorCode.OK)
if (angle.getStatus() == StatusCode.OK)
{
break;
}
try
{
Thread.sleep(10);
} catch (InterruptedException e)
{
}
angle = encoder.getAbsolutePosition();
code = encoder.getLastError();
angle = angle.waitForUpdate(0.01);
}
if (code != ErrorCode.OK)
if (angle.getStatus() != StatusCode.OK)
{
readingError = true;
DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false);
}
return angle;
return angle.getValue() * 360;
}
/**
@@ -139,16 +133,23 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
}
/**
* Sets the Absolute Encoder Offset inside of the CANcoder's Memory.
* Sets the Absolute Encoder Offset within the CANcoder's Memory.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @param offset the offset the Absolute Encoder uses as the zero point in degrees.
* @return if setting Absolute Encoder Offset was successful or not.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
ErrorCode error = encoder.configMagnetOffset(offset);
if (error == ErrorCode.OK)
CANcoderConfigurator cfg = encoder.getConfigurator();
MagnetSensorConfigs magCfg = new MagnetSensorConfigs();
StatusCode error = cfg.refresh(magCfg);
if (error != StatusCode.OK)
{
return false;
}
error = cfg.apply(magCfg.withMagnetOffset(offset / 360));
if (error == StatusCode.OK)
{
return true;
}
@@ -165,6 +166,6 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public double getVelocity()
{
return encoder.getVelocity();
return encoder.getVelocity().getValue() * 360;
}
}