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 YAGSL to support CANandEncoders.

Browse Source
This commit is contained in:
thenetworkgrinch
2023-08-09 13:15:02 -05:00
parent b18491fa9c
commit d356dec4d0
10 changed files with 277 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
swervelib/SwerveDrive.java
View File

@@ -18,6 +18,7 @@ import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -78,28 +79,28 @@ public class SwerveDrive
/**
* Invert odometry readings of drive motor positions, used as a patch for debugging currently.
*/
public boolean invertOdometry = false;
public boolean invertOdometry = false;
/**
* Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
* correction.
*/
public boolean chassisVelocityCorrection = true;
public boolean chassisVelocityCorrection = true;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
private SwerveIMU imu;
private SwerveIMU imu;
/**
* Simulation of the swerve drive.
*/
private SwerveIMUSimulation simIMU;
private SwerveIMUSimulation simIMU;
/**
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
private int moduleSynchronizationCounter = 0;
/**
* The last heading set in radians.
*/
private double lastHeadingRadians = 0;
private double lastHeadingRadians = 0;
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -312,9 +313,6 @@ public class SwerveDrive
swerveDriveConfiguration.maxSpeed,
swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond,
swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond);
} else
{
SwerveKinematics2.desaturateWheelSpeeds(desiredStates, swerveDriveConfiguration.maxSpeed);
}
// Sets states
@@ -411,7 +409,8 @@ public class SwerveDrive
*/
public void resetOdometry(Pose2d pose)
{
swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose);
kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation()));
}
/**
@@ -766,9 +765,28 @@ public class SwerveDrive
}
}
/**
* Set the gyro scope offset to a desired known rotation. Unlike {@link SwerveDrive#setGyro(Rotation3d)} it DOES NOT
* take the current rotation into account.
*
* @param offset {@link Rotation3d} known offset of the robot for gyroscope to use.
*/
public void setGyroOffset(Rotation3d offset)
{
if (SwerveDriveTelemetry.isSimulation)
{
simIMU.setAngle(offset.getZ());
} else
{
imu.setOffset(offset);
}
}
/**
* Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
* the given timestamp of the vision measurement.
* the given timestamp of the vision measurement. <br /> <b>IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET
* AFTER USING THIS FUNCTION!</b> <br /> To update your gyroscope readings directly use
* {@link SwerveDrive#setGyroOffset(Rotation3d)}.
*
* @param robotPose Robot {@link Pose2d} as measured by vision.
* @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
@@ -779,7 +797,7 @@ public class SwerveDrive
* {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry#resetPosition(Rotation2d,
* SwerveModulePosition[], Pose2d)}.
* @param trustWorthiness Trust level of vision reading when using a soft measurement, used to multiply the standard
* deviation. Set to 1 for full trust.
* deviation. Set to 1 for full trust. Higher = Less Weight.
*/
public void addVisionMeasurement(Pose2d robotPose, double timestamp, boolean soft, double trustWorthiness)
{
@@ -792,6 +810,10 @@ public class SwerveDrive
swerveDrivePoseEstimator.resetPosition(
robotPose.getRotation(), getModulePositions(), robotPose);
}
setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
resetOdometry(
new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), robotPose.getRotation()));
}
/**
@@ -818,14 +840,20 @@ public class SwerveDrive
/**
* Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new
* {@link Rotation3d}.
* Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d}
* subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}.
*
* @param gyro expected gyroscope angle.
* @param gyro expected gyroscope angle as {@link Rotation3d}.
*/
public void setGyro(Rotation3d gyro)
{
imu.setOffset(imu.getRawRotation3d().minus(gyro));
if (SwerveDriveTelemetry.isSimulation)
{
setGyroOffset(simIMU.getGyroRotation3d().minus(gyro));
} else
{
setGyroOffset(imu.getRawRotation3d().minus(gyro));
}
}
/**
@@ -865,6 +893,19 @@ public class SwerveDrive
}
}
/**
* Configure whether the {@link SwerveModuleState2} will be optimized to prevent spinning more than 90deg.
*
* @param optimizationEnabled Whether the module optimization is enabled.
*/
public void setModuleStateOptimization(boolean optimizationEnabled)
{
for (SwerveModule module : swerveModules)
{
module.moduleStateOptimization = optimizationEnabled;
}
}
/**
* Enable second order kinematics for simulation and modifying the feedforward. Second order kinematics could increase
* accuracy in odometry.
@@ -879,6 +920,25 @@ public class SwerveDrive
}
}
/**
* Enable second order kinematics with calculated values for the feedforward and return the value used.
*
* @param motorFreeSpeedRPM Steering motor free speed RPM.
* @return The feedforward value used for the last swerve module.
*/
public double enableSecondOrderKinematics(int motorFreeSpeedRPM)
{
double feedforwardValue = 0;
for (SwerveModule module : swerveModules)
{
feedforwardValue = module.configuration.moduleSteerFFCL = RobotController.getBatteryVoltage() /
(2 * Math.PI * (((double) motorFreeSpeedRPM) /
module.configuration.physicalCharacteristics.angleGearRatio) /
60);
}
return feedforwardValue;
}
/**
* Enable second order kinematics for tracking purposes but completely untuned.
*/

26
swervelib/SwerveModule.java
View File

@@ -38,23 +38,28 @@ public class SwerveModule
/**
* Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
*/
public int moduleNumber;
public int moduleNumber;
/**
* Feedforward for drive motor during closed loop control.
*/
public SimpleMotorFeedforward feedforward;
public SimpleMotorFeedforward feedforward;
/**
* Last swerve module state applied.
*/
public SwerveModuleState2 lastState;
public SwerveModuleState2 lastState;
/**
* Enable {@link SwerveModuleState2} optimizations so the angle is reversed and speed is reversed to ensure the module
* never turns more than 90deg.
*/
public boolean moduleStateOptimization = true;
/**
* Simulated swerve module.
*/
private SwerveModuleSimulation simModule;
private SwerveModuleSimulation simModule;
/**
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
private boolean synchronizeEncoderQueued = false;
/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
@@ -144,10 +149,13 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState2 desiredState, boolean isOpenLoop, boolean force)
{
desiredState = SwerveModuleState2.optimize(desiredState,
Rotation2d.fromDegrees(getAbsolutePosition()),
lastState,
configuration.moduleSteerFFCL);
if (moduleStateOptimization)
{
desiredState = SwerveModuleState2.optimize(desiredState,
Rotation2d.fromDegrees(getAbsolutePosition()),
lastState,
configuration.moduleSteerFFCL);
}
if (isOpenLoop)
{

80
swervelib/encoders/CanAndCoderSwerve.java Normal file
View File

@@ -0,0 +1,80 @@
package swervelib.encoders;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
import swervelib.motors.SwerveMotor;
/**
* SparkMax absolute encoder, attached through the data port.
*/
public class CanAndCoderSwerve extends SwerveAbsoluteEncoder {
/**
* The {@link AbsoluteEncoder} representing the duty cycle encoder attached to
* the SparkMax.
*/
public AbsoluteEncoder encoder;
/**
* Create the {@link AbsoluteEncoder} object as a duty cycle. from the
* {@link CANSparkMax} motor.
*
* @param motor Motor to create the encoder from.
*/
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");
}
}
/**
* 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) {
encoder.setInverted(inverted);
}
/**
* 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;
}
}

2
swervelib/math/SwerveMath.java
View File

@@ -89,7 +89,7 @@ public class SwerveMath
public static double calculateMaxAngularVelocity(
double maxSpeed, double furthestModuleX, double furthestModuleY)
{
return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians();
}
/**

88
swervelib/math/SwerveModuleState2.java
View File

@@ -2,7 +2,6 @@ package swervelib.math;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
/**
* Second order kinematics swerve module state.
@@ -63,36 +62,87 @@ public class SwerveModuleState2 extends SwerveModuleState
public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotation2d currentAngle,
SwerveModuleState2 lastState, double moduleSteerFeedForwardClosedLoop)
{
SwerveModuleState2 optimized = new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
desiredState.omegaRadPerSecond);
if (desiredState.angle.equals(currentAngle) || desiredState.angle.equals(
optimized.angle.rotateBy(Rotation2d.fromDegrees(180))) || moduleSteerFeedForwardClosedLoop == 0)
{
optimized.omegaRadPerSecond = 0;
}
if (desiredState.angle.equals(optimized.angle.rotateBy(Rotation2d.fromDegrees(180))))
{
desiredState.omegaRadPerSecond = 0;
return desiredState;
}
return optimized;
/*
// NEVER optimize if it's the same angle, it just doesn't make sense...
if (currentAngle.equals(desiredState.angle.rotateBy(Rotation2d.fromDegrees(180))))
{
desiredState.invert();
desiredState.omegaRadPerSecond = 0;
return desiredState;
} else if (currentAngle.equals(desiredState.angle))
{
desiredState.omegaRadPerSecond = 0;
return desiredState;
}
SwerveModuleState2 optimized;
if (moduleSteerFeedForwardClosedLoop == 0)
{
// desiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(
// lastState.omegaRadPerSecond * moduleSteerFeedForwardClosedLoop * 0.065));
// return new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
// desiredState.omegaRadPerSecond);
return new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle), 0);
optimized = new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle), 0);
// return desiredState;
} else
{
double targetAngle = SwerveMath.placeInAppropriate0To360Scope(currentAngle.getDegrees(),
desiredState.angle.getDegrees() +
Units.radiansToDegrees(lastState.omegaRadPerSecond *
moduleSteerFeedForwardClosedLoop *
0.065));
double targetSpeed = desiredState.speedMetersPerSecond;
double delta = targetAngle - currentAngle.getDegrees();
if (Math.abs(delta) > 90)
Rotation2d delta = desiredState.angle.plus(Rotation2d.fromRadians(lastState.omegaRadPerSecond *
moduleSteerFeedForwardClosedLoop *
0.065))
.minus(currentAngle);
if (Double.compare(Math.abs(delta.getDegrees()), 90.0) < 0)
{
targetSpeed = -targetSpeed;
if (delta > 90)
{
targetAngle -= 180;
} else
{
targetAngle += 180;
}
optimized = desiredState.invert();
optimized.angle = optimized.angle.plus(Rotation2d.fromRadians(lastState.omegaRadPerSecond *
moduleSteerFeedForwardClosedLoop *
0.065));
} else
{
optimized = desiredState;
}
return new SwerveModuleState2(targetSpeed, Rotation2d.fromDegrees(targetAngle), desiredState.omegaRadPerSecond);
}
if (Double.compare(Math.abs(currentAngle.minus(optimized.angle).getDegrees()),
Math.abs(currentAngle.minus(desiredState.angle)
.getDegrees())) > 0)
{
desiredState.omegaRadPerSecond = 0;
return desiredState;
}
// Maybe the omega rad per second will always be off when optimized?
// optimized.omegaRadPerSecond = 0;
return optimized;
*/
}
/**
* Invert the swerve module state.
*
* @return Current inverted {@link SwerveModuleState2}
*/
public SwerveModuleState2 invert()
{
// omegaRadPerSecond *= -1;
// speedMetersPerSecond *= -1;
// angle = angle.rotateBy(Rotation2d.fromDegrees(180));
// return this;
return new SwerveModuleState2(-speedMetersPerSecond,
angle.rotateBy(Rotation2d.fromDegrees(180)),
-omegaRadPerSecond);
}
/**

7
swervelib/math/SwervePoseEstimator2.java
View File

@@ -224,6 +224,13 @@ public class SwervePoseEstimator2 // extends SwerveDrivePoseEstimator
sample.get().modulePositions,
sample.get().poseMeters.exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the same timestamp
// m_poseBuffer.addSample(
// timestampSeconds,
// new InterpolationRecord(
// getEstimatedPosition(), sample.get().gyroAngle, sample.get().gyroPitch, sample.get().gyroRoll,
// sample.get().modulePositions));
// Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry<Double, InterpolationRecord> entry :

6
swervelib/motors/SparkMaxSwerve.java
View File

@@ -269,8 +269,12 @@ public class SparkMaxSwerve extends SwerveMotor
* Save the configurations from flash to EEPROM.
*/
@Override
public void burnFlash()
public void burnFlash()
{
try {
Thread.sleep(200);
} catch (Exception e) {
}
motor.burnFlash();
}

42
swervelib/parser/json/DeviceJson.java
View File

@@ -4,6 +4,7 @@ import com.revrobotics.SparkMaxRelativeEncoder.Type;
import edu.wpi.first.wpilibj.SerialPort.Port;
import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
import swervelib.encoders.CANCoderSwerve;
import swervelib.encoders.CanAndCoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.imu.ADIS16448Swerve;
@@ -42,16 +43,33 @@ public class DeviceJson
/**
* Create a {@link SwerveAbsoluteEncoder} from the current configuration.
*
* @param motor {@link SwerveMotor} of which attached encoders will be created from, only used when the type is
* "attached" or "canandencoder".
* @return {@link SwerveAbsoluteEncoder} given.
*/
public SwerveAbsoluteEncoder createEncoder()
public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor)
{
SwerveAbsoluteEncoder attachedChoice = null;
switch (type)
{
case "none":
case "integrated":
case "attached":
return null;
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;
case "canandcoder":
return new CanAndCoderSwerve(motor);
case "thrifty":
case "throughbore":
case "dutycycle":
@@ -142,24 +160,4 @@ 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);
case "falcon":
case "talonfx":
return null;
}
throw new RuntimeException(
"Could not create absolute encoder from data port of " + type + " id " + id);
}
}

9
swervelib/parser/json/ModuleJson.java
View File

@@ -66,14 +66,7 @@ public class ModuleJson
String name)
{
SwerveMotor angleMotor = angle.createMotor(false);
SwerveAbsoluteEncoder absEncoder = encoder.createEncoder();
// If the absolute encoder is attached.
if (absEncoder == null)
{
absEncoder = angle.createIntegratedEncoder(angleMotor);
angleMotor.setAbsoluteEncoder(absEncoder);
}
SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor);
return new SwerveModuleConfiguration(
drive.createMotor(true),

2
swervelib/parser/json/PhysicalPropertiesJson.java
View File

@@ -39,7 +39,7 @@ public class PhysicalPropertiesJson
* your drive train does not drift towards the direction you are rotating while you translate. Default value is 0. If
* robot arcs while translating and rotating negate this.
*/
public double moduleFeedForwardClosedLoop = SwerveDriveTelemetry.isSimulation ? -0.33 : 0;
public double moduleFeedForwardClosedLoop = SwerveDriveTelemetry.isSimulation ? 0.33 : 0;
/**
* DEPRECATED: No longer needed, tune {@link PhysicalPropertiesJson#moduleFeedForwardClosedLoop} instead.
*/