Merge teleop into main

build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2022.4.1"
-    id "com.github.spotbugs" version "5.0.8"
+    id "com.github.spotbugs" version "5.0.13"
     id "net.ltgt.errorprone" version "2.0.2"
     id "pmd"
     id 'checkstyle'

src/main/java/frc/robot/Constants.java

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import org.frc4607.common.swerve.SwerveDriverConfig;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
  * numerical or boolean
@@ -16,4 +18,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+    public static void getFrontLeftConfig() {
+        SwerveDriverConfig c = new SwerveDriverConfig();
+    }
 }

src/main/java/frc/robot/commands/TeleOp.java

@@ -0,0 +1,17 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.SwerveDriveSubsystem;
+
+/**
+ * Default command for the drivetrain that updates it from the controller.
+ */
+public class TeleOp extends CommandBase {
+    private final SwerveDriveSubsystem m_subsystem;
+
+    public TeleOp(SwerveDriveSubsystem subsystem) {
+        m_subsystem = subsystem;
+        addRequirements(m_subsystem);
+    }
+
+}

src/main/java/frc/robot/drivers/NeoDriver.java

@@ -0,0 +1,101 @@
+package frc.robot.drivers;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.SparkMaxPIDController.ArbFFUnits;
+import org.frc4607.common.swerve.SwerveDriverConfig;
+import org.frc4607.common.swerve.SwerveMotorBase;
+
+/**
+ * Extends the {@link org.frc4607.common.swerve.SwerveMotorBase} for REV Neo brushless motors.
+ */
+public class NeoDriver extends SwerveMotorBase {
+    private final CANSparkMax m_motor;
+    private final RelativeEncoder m_neoEncoder;
+    private final SparkMaxPIDController m_pidController;
+
+    /**
+     * Constructs a new instance of this class.
+     *
+     * @param config The settings for this module.
+     */
+    public NeoDriver(SwerveDriverConfig config) {
+        super(config);
+
+        m_motor = new CANSparkMax(m_config.m_id, MotorType.kBrushless);
+        m_motor.clearFaults();
+
+        m_motor.setInverted(m_config.m_invertMotor);
+
+        m_neoEncoder = m_motor.getEncoder();
+        m_neoEncoder.setPositionConversionFactor(config.m_positionCoefficient);
+        m_neoEncoder.setVelocityConversionFactor(config.m_velocityCoefficient);
+
+        m_pidController = m_motor.getPIDController();
+        m_pidController.setP(m_config.m_kp, 0);
+        m_pidController.setI(m_config.m_ki, 0);
+        m_pidController.setD(m_config.m_kd, 0);
+        m_pidController.setIMaxAccum(m_config.m_maxI, 0);
+        m_pidController.setIZone(m_config.m_kiZone, 0);
+    }
+
+    @Override
+    public boolean isConnected() {
+        return m_motor.getFaults() == 0 && m_motor.getStickyFaults() == 0;
+    }
+
+    /** 
+     * Gets the position of the motor.
+     *
+     * @return The distance in meters if this is a drive motor or the rotation in CCW
+     postive degrees if this is a turning motor.
+     */
+    @Override
+    public double getEncoderPosition() {
+        if (m_config.m_motorType == SwerveDriverConfig.MotorType.TURNING) {
+            return m_quadEncoder.getDistance() + m_offset;
+        } else {
+            return m_neoEncoder.getPosition();
+        }
+    }
+
+    /** 
+     * Gets the velocity of the motor.
+     *
+     * @return The velocity in meters per second if this is a drive motor or the velocity in CCW
+     postive degrees per second if this is a turning motor.
+     */
+    @Override
+    public double getEncoderVelocity() {
+        if (m_config.m_motorType == SwerveDriverConfig.MotorType.TURNING) {
+            return m_quadEncoder.getRate();
+        } else {
+            return m_neoEncoder.getVelocity();
+        }
+    }
+
+    @Override
+    public void setEncoder(double target) {
+        m_neoEncoder.setPosition(target);
+    }
+
+    /**
+     * Sets the target of the motor to a setpoint.
+     *
+     * @param target The target to set the motor to. Will be in meters per second for the drive
+     motor and degrees or the turning motor.
+     */
+    @Override
+    public void setTarget(double target, double ffVolts) {
+        if (m_config.m_motorType == SwerveDriverConfig.MotorType.TURNING) {
+            m_pidController.setReference(target,
+                ControlType.kPosition, 0, ffVolts, ArbFFUnits.kVoltage);
+        } else {
+            m_pidController.setReference(target,
+                ControlType.kVelocity, 0, ffVolts, ArbFFUnits.kVoltage);
+        }
+    }
+}

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -4,16 +4,38 @@
 
 package frc.robot.subsystems;
 
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.threads.SwerveDriveThread;
+import org.frc4607.common.swerve.SwerveDrive;
 
 /**
- * Manages the drivetrain of the robot.
+ * Manages the drivetrain of the robot by talking to the thread containing the swerve drive handler.
  */
 public class SwerveDriveSubsystem extends SubsystemBase {
 
-    @Override
-    public void periodic() {
-        // This method will be called once per scheduler run
+    private final SwerveDriveThread m_thread;
+    private final Notifier m_notifier;
+
+    /**
+     * Creates a new object and sets up the thread it uses.
+
+     * @param swerveDrive The {@link org.frc4607.common.swerve.SwerveDrive} to operate on.
+     */
+    public SwerveDriveSubsystem(SwerveDrive swerveDrive) {
+        m_thread = new SwerveDriveThread(swerveDrive);
+        m_notifier = new Notifier(m_thread);
+        m_notifier.startPeriodic(0.001);
+    }
+
+    /**
+     * Sets the target {@link edu.wpi.first.math.kinematics.ChassisSpeeds} of the robot.
+
+     * @param target The speeds for the drivetrain to target.
+     */
+    public void setSpeeds(ChassisSpeeds target) {
+        m_thread.setSpeeds(target);
     }
 
     @Override

src/main/java/frc/robot/threads/SwerveDriveThread.java

@@ -0,0 +1,43 @@
+package frc.robot.threads;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import org.frc4607.common.swerve.SwerveDrive;
+
+/**
+ * A class representing a separate thread running the swerve drive controller at a higher update
+ * rate than the main loop. Methods in this class should be thread safe.
+ */
+public class SwerveDriveThread implements Runnable {
+    private final SwerveDrive m_drivetrain;
+
+    private volatile ChassisSpeeds m_lastSpeed = new ChassisSpeeds(0, 0, 0);
+    private final Object m_lastSpeedLock = new Object();
+
+    public SwerveDriveThread(SwerveDrive drive) {
+        m_drivetrain = drive;
+    }
+
+    private void periodic() {
+        synchronized (m_lastSpeedLock) {
+            m_drivetrain.update(m_lastSpeed);
+        }
+    }
+
+    /**
+     * Sets the target {@link edu.wpi.first.math.kinematics.ChassisSpeeds} of the robot.
+
+     * @param target The speeds for the drivetrain to target.
+     */
+    public void setSpeeds(ChassisSpeeds target) {
+        synchronized (m_lastSpeedLock) {
+            m_lastSpeed = target;
+        }
+    }
+
+    /**
+     * Runs the periodic loop of the object.
+     */
+    public void run() {
+        periodic();
+    }
+}