diff --git a/build.gradle b/build.gradle index 616c6af..fa3d961 100644 --- a/build.gradle +++ b/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' diff --git a/common b/common index 8c5893d..9cdd4e8 160000 --- a/common +++ b/common @@ -1 +1 @@ -Subproject commit 8c5893df5c6779e3803e6e458ff7f3051f74be71 +Subproject commit 9cdd4e8288657d97792ede16f7f98ada38272737 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4bf01fc..7c83499 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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(); + } } diff --git a/src/main/java/frc/robot/commands/TeleOp.java b/src/main/java/frc/robot/commands/TeleOp.java new file mode 100644 index 0000000..e580797 --- /dev/null +++ b/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); + } + +} diff --git a/src/main/java/frc/robot/drivers/NeoDriver.java b/src/main/java/frc/robot/drivers/NeoDriver.java new file mode 100644 index 0000000..9d6c508 --- /dev/null +++ b/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); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 3bfa21f..8aa41e1 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/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 diff --git a/src/main/java/frc/robot/threads/SwerveDriveThread.java b/src/main/java/frc/robot/threads/SwerveDriveThread.java new file mode 100644 index 0000000..2639e07 --- /dev/null +++ b/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(); + } +}