Yet Another Generic Swerve Library (YAGSL) Beta

nstrike · January 30, 2023, 11:13pm

Hello World!

YAGSL is a Generic Swerve Library (AN ACTUAL LIBRARY!) that aims to simplify the process of creating a comprehensive and complete SwerveDrive’s with best practices from MANY SwerveDrive examples.

The documentation wiki is here.
An example project is here.
Library is here.
Javadoc are here.

Features:

Second Order Kinematics with limited state-space estimation.
Uses closed-loop PID exclusively.
Dashboard for each swerve module.
Support for both Falcon’s and Neo’s without changing any of the code base.
An actual library as opposed to template code.
Documentation, yes even javadocs.
Varying Encoder support.
Does not require a CommandBased project and could be implemented in a TimedRobot.
Implements WPILib’s classes RobotDriveBase and MotorSafety.
If a module is inaccessible/broken the Swerve Drive will still work.
Advanced tuning for motor controller output limits and supply amps.
Simulation support.
Low CPU utilization.

The easiest way to create a SwerveDrive is through a JSON configuration tree/directory which allows for a single small line to create the entire SwerveDrive.
SwerveDrive drive = SwerveParser.fromJSONDirectory(new File(Filesystem.getDeployDirectory(), "swerve"));
There are other ways to create the SwerveDrive but that is the easiest.

The library uses WPILib classess to ensure MotorSafety and compliance with the same parent class used for DifferentialDrive’s.

The library creates it’s own dashboard (which can be optionnaly updated in a periodic function with varying degrees of verbosity) that helps with tuning and debugging.

There is support for both Falcon’s and Neo’s WITH THE SAME CODE BASE alongside support for multiple different encoders both integrated, on the CAN bus with or without a CANivore, or directly from the roboRIO.

This implementation utilizes SecondOrderKinematics in an attempt to provide a better control system for the Swerve Drive. In addition to some state-space modeling for position estimation.

There is complete support for simulation with NEO’s however falcons require more testing/development. The configuration should be very easy for any team to use and robust enough to be applicable to most Swerve Drive’s.

I am looking for teams willing to test out YAGSL and give me feedback on what needs to be improved or configurable.

This thread will serve as another way to report issue’s if you do not submit one via github.

I hope everyone enjoys this!
Thank you!

nstrike · January 30, 2023, 11:18pm

The only configuration I have been able to test is a SDS MK4 L2 with all Neo’s, CANCoders, and a Pigeon2. Currently the only supported gyroscope is a Pigeon2. I may add NavX in the future however. I need more teams to test on their configuration!

Fletch1373 · January 31, 2023, 12:00am

Whoa, I didn’t actually think you’d go with my name suggestion. Schweet!

nstrike · January 31, 2023, 12:19am

I liked it. Thanks for the recommendation!

nstrike · January 31, 2023, 3:27am

Updated features and linked directly to swervelib.

nstrike · January 31, 2023, 4:26am

Updated with github pages javadocs Overview

GoldMonster · January 31, 2023, 3:32pm

Are there plans to support adding vision measurements to the pose estimator, or a way to expose that?

nstrike · January 31, 2023, 3:49pm

Absolutely, I just forgot to make the PoseEstimator public!

github.com

BroncBotz3481/YAGSL/blob/main/swervelib/SwerveDrive.java#L72


      
           * Maximum speed in meters per second.
           */
          public final double                m_driverMaxSpeedMPS, m_driverMaxAngularVelocity, m_physicalMaxSpeedMPS;
          /**
           * Swerve drive kinematics.
           */
          private final SwerveDriveKinematics2   m_swerveKinematics;
          /**
           * Swerve drive pose estimator for attempting to figure out our current position.
           */
          private final SwerveDrivePoseEstimator m_swervePoseEstimator;
          /**
           * Pigeon 2.0 centered on the robot.
           */
          private final WPI_Pigeon2              m_pigeonIMU;
          /**
           * Field2d displayed on shuffleboard with current position.
           */
          private final Field2d                  m_field = new Field2d();
          /**
           * The slew rate limiters to make control smooth.

GoldMonster · January 31, 2023, 4:26pm

Thanks, additionally, would we be able to set voltages to individual motors for each swerve module? We use a custom characterization command so we need voltage control over motors.

nstrike · January 31, 2023, 4:28pm

I don’t think i have one to set voltages directly but I do have one to set percentage from -1 to 1.

GoldMonster · January 31, 2023, 4:28pm

Is voltage compensation enabled or controllable to make this useful?

nstrike · January 31, 2023, 4:30pm

It is! You can control it via the setVoltageCompensation function

github.com

BroncBotz3481/YAGSL/blob/main/swervelib/SwerveMotor.java#L149


      
           *
           * @return Current value of the encoder.
           */
          public abstract double getPosition();
          
          
/**
           * Set the voltage compensation for the swerve module motor.
           *
           * @param nominalVoltage Nominal voltage for operation to output to.
           */
          public abstract void setVoltageCompensation(double nominalVoltage);
          
          
/**
           * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
           * voltage compensation. This is useful to protect the motor from current spikes.
           *
           * @param currentLimit Current limit in AMPS at free speed.
           */
          public abstract void setCurrentLimit(int currentLimit);
          
          
/**

nstrike · January 31, 2023, 4:42pm

Just added that now

GoldMonster · January 31, 2023, 5:01pm

Great to hear. This also doesn’t appear to support drivetrain characterization values like kS, kV, and kA. Perhaps I missed that in the docs.

nstrike · January 31, 2023, 5:14pm

I am honestly not sure how to implement that? Is there any reason it couldn’t be implemented via a SwerveSubsystem or equivalent?

github.com

BroncBotz3481/YAGSL-Example/blob/main/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

package frc.robot.subsystems.swervedrive;

import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.subsystems.swervedrive.swerve.SwerveDrive;
import frc.robot.subsystems.swervedrive.swerve.SwerveModule.Verbosity;
import frc.robot.subsystems.swervedrive.swerve.SwerveParser;
import java.io.File;

/**
 * SwerveDrive Subsystem.
 */
public class SwerveSubsystem extends SubsystemBase
{

  private final Timer       syncTimer = new Timer();
  /**
   * Swerve Drive object.

This file has been truncated. show original

GoldMonster · January 31, 2023, 6:25pm

The drivetrain characterization values allow you to dynamically calculate feedforward for each module. I believe 364’s template has an implementation.

nstrike · January 31, 2023, 6:44pm

Like this?

github.com

BroncBotz3481/YAGSL-Example/blob/main/src/main/java/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.java#L91


      
                m_secondaryPidSlot = REV_slotIdx.Position.ordinal();
          
          
//      setPIDF(0.1, 0, 0, 0, 1);
                setPIDF(0.01, 0, 0.005, 0, 0);
          
          
      setConversionFactor(((Math.PI * wheelDiameter) / gearRatio) / 60);
          
          
      setCurrentLimit(50);
              } else
              {
                m_moduleRadkV = (12 * 60) / (freeSpeedRPM * Math.toRadians(360 / (m_integratedAbsEncoder ? 1 : gearRatio)));
          
          
      if (m_integratedAbsEncoder)
                {
                  m_encoderPosRet = m_encoderRet = ((AbsoluteEncoder) m_encoder)::getPosition;
                } else
                {
                  m_encoderPosRet = m_encoderRet = ((RelativeEncoder) m_encoder)::getPosition;
                }
          
          
      m_feedforwardUnits = ArbFFUnits.kVoltage;

github.com

BroncBotz3481/YAGSL-Example/blob/main/src/main/java/frc/robot/subsystems/swervedrive/swerve/motors/REVSwerveMotor.java#L216


      
          /**
           * Set the target for the PID loop.
           *
           * @param target      The PID target to aim for.
           * @param feedforward feedForward value.
           */
          @Override
          public void setTarget(double target, double feedforward)
          {
            this.target = target;
            m_pid.setReference(target, m_pidControlType, m_mainPidSlot, feedforward * m_moduleRadkV, m_feedforwardUnits);
          }
          
          
/**
           * Stop the motor by sending 0 volts to it.
           */
          @Override
          public void stop()
          {
            m_motor.set(0);
          }

or this?

github.com

BroncBotz3481/YAGSL-Example/blob/main/src/main/java/frc/robot/subsystems/swervedrive/swerve/SwerveModule.java#L210


      
          
          
// Set the maximum speed for each swerve module for use when trying to optimize movements.
          // Drive feedforward gains
          //        public static final double KS = 0;
          //        public static final double KV = 12 / MAX_SPEED; // Volt-seconds per meter (max voltage divided by max
          //        speed)
          //        public static final double KA = 12 / MAX_ACCELERATION; // Volt-seconds^2 per meter (max voltage divided
          //        by max accel)
          maxDriveSpeedMPS = maxSpeedMPS;
          driveFeedforward = new SimpleMotorFeedforward(0, 12 / maxDriveSpeedMPS, 12 / maxDriveAcceleration);
          steeringKV = (12 * 60) / (steeringMotorFreeSpeedRPM * Math.toRadians(360 / steerGearRatio));
          
          
driveMotor.setInverted(drivingInverted);
          invertedDrive = drivingInverted;
          turningMotor.setInverted(steeringInverted);
          invertedTurn = steeringInverted;
          
          
absoluteEncoder.configure();
          
          
setAngleOffset(steeringOffsetDegrees);
          resetEncoders();

github.com

BroncBotz3481/YAGSL-Example/blob/main/src/main/java/frc/robot/subsystems/swervedrive/swerve/SwerveModule.java#L462


      
          //                                                                                        state.angle), currentAngle);
              SwerveModuleState2 optimizedState = SwerveModuleState2.optimize(state, currentAngle);
              double             angle          = optimizedState.angle.getDegrees(); // getDegrees returns in the range of -180 to 180 we want 0 to
              // 360.
              double velocity = (Math.abs(optimizedState.speedMetersPerSecond) <= (maxDriveSpeedMPS * 0.01)) ? 0
                                                                                                             :
                                optimizedState.speedMetersPerSecond;
              // turn motor code
              // Prevent rotating module if speed is less then 1%. Prevents Jittering.
              angle = (Math.abs(optimizedState.speedMetersPerSecond) <= (maxDriveSpeedMPS * 0.01)) ? 0 : angle;
              setAngle(angle, optimizedState.angularVelocityRadPerSecond * steeringKV);
              setVelocity(velocity);
              targetAngle = angle;
              targetVelocity = velocity;
              targetAngularVelocityRPS = optimizedState.angularVelocityRadPerSecond;
            }
          
          
  /**
             * Get the swerve module position based off the sensors.
             *
             * @return Swerve Module position, with the angle as what the angle is supposed to be, not what it actually is.

It appears I did it twice

GoldMonster · January 31, 2023, 6:54pm

Yeah, typically with a SimpleMotorFeedforward, but we do have measured kS, kV, and kA instead of just calculated values.

Oblarg · January 31, 2023, 7:19pm

I really like the idea behind this; I think you’ll find that the architecture you’ve chosen is quite difficult to maintain in the long-term, however, due to the the tight coupling between the SwerveMotor abstract class and the particular shape of the REV API. I’d look at ways you could generalize that. You may also want to distinguish steering motors from drive motors.

nstrike · January 31, 2023, 8:24pm

I can make the feedforward public and able to be redefined. That would match BaseFalconSwerve. Or would it be better to have it configurable via JSON too?