diff --git a/src/main/java/frc/robot/util/Neo.java b/src/main/java/frc/robot/util/Neo.java index 5fc541bd..99be5a39 100644 --- a/src/main/java/frc/robot/util/Neo.java +++ b/src/main/java/frc/robot/util/Neo.java @@ -1,11 +1,18 @@ -// Developed by Reza from Team Spyder 1622 +// Developed in tandem with Reza from Team Spyder (1622) package frc.robot.util; -import com.revrobotics.*; +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import com.revrobotics.REVPhysicsSim; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.NeoMotorConstants; @@ -16,7 +23,6 @@ * Some of this is adapted from 3005's 2022 Code * Original source published at https://github.com/FRC3005/Rapid-React-2022-Public/tree/d499655448ed592c85f9cfbbd78336d8841f46e2 */ - public class Neo extends CANSparkMax { public final RelativeEncoder encoder; public final SparkPIDController pidController; @@ -57,8 +63,8 @@ public Neo(int id, boolean reversed, CANSparkBase.IdleMode mode) { super(id, CANSparkLowLevel.MotorType.kBrushless); this.reversedMultiplier = reversed ? -1 : 1; - // restoreFactoryDefaults(); - // Timer.delay(0.050); + restoreFactoryDefaults(); + Timer.delay(0.050); // If a parameter set fails, this will add more time to alleviate any bus traffic // default is 20ms @@ -79,50 +85,101 @@ public Neo(int id, boolean reversed) { } /** - * Sets the target position for the NEO. - * @param position Position to set the Neo to in rotations. - * @param arbitraryFeedForward Arbitrary feed forward to add to the motor output. + * Sets the target position for the Neo motor + * with an optional arbitrary feedforward value. + * + * @param position The target position for the motor. + * @param arbitraryFeedForward An optional arbitrary feedforward value. */ - public void setTargetPosition(double position, double arbitraryFeedForward, int slot) { - position *= reversedMultiplier; - if (!FieldConstants.IS_SIMULATION) { - pidController.setReference(position, ControlType.kPosition, slot, arbitraryFeedForward, SparkPIDController.ArbFFUnits.kVoltage); - } - targetPosition = position; - controlType = ControlLoopType.POSITION; - } - public void setTargetPosition(double position, double arbitraryFeedForward) { setTargetPosition(position, arbitraryFeedForward, 0); } + /** + * Sets the target position for the Neo motor + * with an optional arbitrary feedforward value. + * + * @param position The target position for the motor. + * @param slot The PID slot for the reference + */ + public void setTargetPosition(double position, int slot) { + setTargetPosition(position, 0, slot); + } + + /** + * Sets the target position for the Neo motor. + * + * @param position the desired target position + */ public void setTargetPosition(double position) { setTargetPosition(position, 0, 0); } + + /** + * Sets the target position for the Neo motor controller. + * + * @param position The desired position in encoder units. + * @param arbitraryFeedForward The arbitrary feedforward value to be applied. + * @param slot The PID slot to use for control. + */ + public void setTargetPosition(double position, double arbitraryFeedForward, int slot) { + position *= reversedMultiplier; + if (!FieldConstants.IS_SIMULATION) { + pidController.setReference(position, ControlType.kPosition, slot, arbitraryFeedForward, ArbFFUnits.kVoltage); + } + targetPosition = position; + controlType = ControlLoopType.POSITION; + } - // Creates method to set the target percent of the speed output for the NEO. + /** + * Sets the target percent for the Neo motor. + * + * @param percent the target percent to set + */ public void setTargetPercent(double percent) { setTargetPercent(percent, 0); } /** - * Sets the target percent of speed output for the NEO. - * @param percent Percent of NEO output speed. + * Sets the target percent of speed output for the Neo. + * + * @param percent Percent of Neo output speed. * @param slot The PID slot. */ public void setTargetPercent(double percent, int slot) { + setTargetPercent(percent, 0, slot); + } + + /** + * Sets the target percent of speed output for the Neo. + * + * @param percent Percent of Neo output speed. + * @param arbitraryFeedForward The FF for the reference + */ + public void setTargetPercent(double percent, double arbitraryFeedForward) { + setTargetPercent(percent, arbitraryFeedForward, 0); + } + + /** + * Sets the target percent output for the Neo motor controller. + * + * @param percent The desired percent output (-1.0 to 1.0). + * @param arbitraryFeedForward The arbitrary feedforward value. + * @param slot The PID slot to use. + */ + public void setTargetPercent(double percent, double arbitraryFeedForward, int slot) { percent *= reversedMultiplier; if (percent == 0) { setVoltage(0); } else { - pidController.setReference(percent, ControlType.kDutyCycle); + pidController.setReference(percent, ControlType.kDutyCycle, slot, arbitraryFeedForward, ArbFFUnits.kPercentOut); } targetPercent = percent; controlType = ControlLoopType.PERCENT; } /** - * Sets the target velocity for the NEO. + * Sets the target velocity for the Neo. * @param velocity Velocity to set the Neo to in rotations per minute. */ public void setTargetVelocity(double velocity) { @@ -130,31 +187,68 @@ public void setTargetVelocity(double velocity) { } /** - * Sets the target velocity for the NEO. - * @param velocity Velocity to set the Neo to in rotations per minute. - * @param arbitraryFeedForward Arbitrary feed forward to add to the motor output. + * Sets the target velocity for the Neo. + * + * @param velocity Velocity to set the Neo to in rotations per + * minute. + * @param arbitraryFeedForward The FF for the reference. + */ + public void setTargetVelocity(double velocity, double arbitraryFeedForward) { + setTargetVelocity(velocity, arbitraryFeedForward, 0); + } + + /** + * Sets the target velocity for the Neo motor. + * + * @param velocity the target velocity to set + * @param slot the slot to set the target velocity for */ public void setTargetVelocity(double velocity, int slot) { + setTargetVelocity(velocity, 0, slot); + } + + /** + * Sets the target velocity for the Neo. + * + * @param velocity Velocity to set the Neo to in rotations per + * minute. + * @param arbitraryFeedForward Arbitrary feed forward to add to the motor + * output. + */ + public void setTargetVelocity(double velocity, double arbitraryFeedForward, int slot) { velocity *= reversedMultiplier; if (velocity == 0) { setVoltage(0); } else { - pidController.setReference(velocity, ControlType.kVelocity); + pidController.setReference(velocity, ControlType.kVelocity, slot, arbitraryFeedForward, ArbFFUnits.kVoltage); } targetVelocity = velocity; controlType = ControlLoopType.VELOCITY; } + + /** + * Sets the output of the Neo motor controller based on a percentage value. + * Positive values are counter clockwise if there is no reversedMultiplier + * + * @param percent The percentage value to set the motor output to. + */ public void set(double percent) { percent *= reversedMultiplier; - setVoltage(percent * RobotController.getBatteryVoltage()); + super.set(percent); controlType = ControlLoopType.PERCENT; } private boolean shouldCache = false; private double position = 0; private double velo = 0; - + + /** + * Updates the position and velocity of the Neo motor. + * If caching is enabled, it caches the position and velocity values. + * If the simulation is active and the control type is position, + * it simulates movement on the motor + */ public void tick() { if (shouldCache) { position = encoder.getPosition(); @@ -162,16 +256,21 @@ public void tick() { } if ((FieldConstants.IS_SIMULATION) && controlType == ControlLoopType.POSITION) { - setVoltage(pidController.getP() * (targetPosition - getPosition())); } } + + /** + * Registers the Neo motor instance. + * Adds the motor to the list of registered motors + * and also adds the motor to the REVPhysicsSim instance + * if in simulation. + */ public void register() { NeoMotorConstants.motors.add(this); if (FieldConstants.IS_SIMULATION) - REVPhysicsSim.getInstance().addSparkMax(this, DCMotor.getNEO(1)); - + REVPhysicsSim.getInstance().addSparkMax(this, DCMotor.getNEO(1)); } /** @@ -179,12 +278,8 @@ public void register() { * @return The position of the Neo in rotations relative to the last 0 position. */ public double getPosition() { - double pos; - if (shouldCache) { - pos = position; - } else { - pos = encoder.getPosition(); - } + + double pos = shouldCache ? position : encoder.getPosition(); if ((FieldConstants.IS_SIMULATION) && controlType == ControlLoopType.VELOCITY) { pos /= encoder.getVelocityConversionFactor(); @@ -198,16 +293,16 @@ public double getPosition() { * @return The instantaneous velocity of the Neo in rotations per minute. */ public double getVelocity() { - if (shouldCache) { - return velo; - } else { - return encoder.getVelocity(); - } + return shouldCache ? velo : encoder.getVelocity(); } + /** + * Sets the position of the Neo encoder. + * + * @param position the desired position to set + */ public void setPosition(double position) { - position *= reversedMultiplier; - encoder.setPosition(position); + encoder.setPosition(position * reversedMultiplier); } /** @@ -226,19 +321,57 @@ public double getTargetVelocity() { return targetVelocity; } + /** + * Gets the target percentage of the Neo in... well, % + * @return The target percent speed of the Neo + */ + public double getTargetPercent() { + return targetPercent; + } + + /** + * Adds a follower to the Neo motor. + * + * @param follower the Neo motor to be added as a follower + */ public void addFollower(Neo follower) { addFollower(follower, false); } + /** + * Adds a follower to the Neo motor. + * + * @param follower The Neo motor to be added as a follower. + * @param invert Specifies whether the follower should be inverted or not. + */ public void addFollower(Neo follower, boolean invert) { followers.add(follower); - follower.follow(this); + follower.follow(this, invert); } + /** + * Sets the PID values and output limits for the Neo motor controller. + * + * @param P the proportional gain + * @param I the integral gain + * @param D the derivative gain + * @param minOutput the minimum output value + * @param maxOutput the maximum output value + */ public void setPID(double P, double I, double D, double minOutput, double maxOutput) { setPID(P, I, D, minOutput, maxOutput, 0); } + /** + * Sets the PID values and output range for the PID controller. + * + * @param P the proportional gain + * @param I the integral gain + * @param D the derivative gain + * @param minOutput the minimum output value + * @param maxOutput the maximum output value + * @param slotID the slot ID of the PID controller + */ public void setPID(double P, double I, double D, double minOutput, double maxOutput, int slotID) { pidController.setP(P, slotID); pidController.setI(I, slotID); @@ -247,66 +380,107 @@ public void setPID(double P, double I, double D, double minOutput, double maxOut pidController.setOutputRange(minOutput, maxOutput, slotID); } + /** + * Sets the position conversion factor for the encoder. + * This is generally a gear ratio + * + * @param factor the conversion factor to set + */ public void setPositionConversionFactor(double factor) { encoder.setPositionConversionFactor(factor); } + /** + * Sets the velocity conversion factor for the encoder. + * This is generally a gear ratio + * + * @param factor the conversion factor to set + */ public void setVelocityConversionFactor(double factor) { encoder.setVelocityConversionFactor(factor); } /** - * Gets the proportional gain constant for PIDFF controller. - * @return The proportional gain constant for PIDFF controller. + * Gets the proportional gain constant for PID controller. + * @return The proportional gain constant for PID controller. */ public double getP() { return pidController.getP(); } /** - * Gets the integral gain constant for PIDFF controller. - * @return The integral gain constant for PIDFF controller. + * Gets the integral gain constant for PID controller. + * @return The integral gain constant for PID controller. */ public double getI() { return pidController.getI(); } /** - * Gets the derivative gain constant for PIDFF controller. - * @return The derivative gain constant for PIDFF controller. + * Gets the derivative gain constant for PID controller. + * @return The derivative gain constant for PID controller. */ public double getD() { return pidController.getD(); } /** - * Gets the I-Zone constant for PIDFF controller. - * @return The I-Zone constant for PIDFF control. + * Gets the I-Zone constant for PID controller. + * The I-Zone is the zone at which the integral + * will be applied and "enabled" + * + * Example: + * an I-Zone of 0.25 + * will make the integral apply + * for when the absolute value of + * | desired - current | is 0.25 or less. + * + * Think of this like the final stretch of PID + * + * @return The I-Zone constant for PID control. */ public double getIZ() { return pidController.getIZone(); } /** - * Gets the feedforward gain constant for PIDFF controller. - * @return The feedforward gain constant for PIDFF controller. + * Gets the feedforward gain constant for PID controller. + * @return The feedforward gain constant for PID controller. */ public double getFF() { return pidController.getFF(); } - // Documentation: https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames + /** + * Represents an error that can occur while using the REVLib library. + * Rev docs: + * https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames + * + * @param frame The status frame to reset + * @param period The update period for the status frame. + * @return error + */ public REVLibError changeStatusFrame(StatusFrame frame, int period) { REVLibError error = setPeriodicFramePeriod(frame.getFrame(), period); return error; } + /** + * Resets the status frame of the NEO motor controller to its default period. + * Rev docs: https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames + * + * @param frame the status frame to reset + * @return the REVLibError indicating the result of the operation + */ public REVLibError resetStatusFrame(StatusFrame frame) { - return changeStatusFrame(frame, frame.getDefaultPeriod()); + return changeStatusFrame(frame, frame.getDefaultPeriodms()); } - // Documentation: https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames + /** + * Represents the status frames for the Neo class. + * Rev docs: https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames + */ public enum StatusFrame { APPLIED_FAULTS_FOLLOWER(PeriodicFrame.kStatus0, 10), VELO_TEMP_VOLTAGE_CURRENT(PeriodicFrame.kStatus1, 20), @@ -317,31 +491,60 @@ public enum StatusFrame { ABSOLUTE_ENCODER_VELO(PeriodicFrame.kStatus6, 200); private final PeriodicFrame frame; - private final int defaultPeriod; // ms + private final int defaultPeriodms; + + /** + * Constructs a StatusFrame with the specified frame and default period. + * + * @param frame The periodic frame. + * @param defaultPeriod The default period in milliseconds. + */ StatusFrame(PeriodicFrame frame, int defaultPeriod) { this.frame = frame; - this.defaultPeriod = defaultPeriod; + this.defaultPeriodms = defaultPeriod; } + /** + * Gets the periodic frame associated with this StatusFrame. + * + * @return The periodic frame. + */ public PeriodicFrame getFrame() { return frame; } - public int getDefaultPeriod() { - return defaultPeriod; + /** + * Gets the default period in milliseconds for this StatusFrame. + * + * @return The default period in milliseconds. + */ + public int getDefaultPeriodms() { + return defaultPeriodms; } } + /** + * Enum representing the control loop types for the Neo motor. + * The control loop types include POSITION, VELOCITY, and PERCENT. + */ public enum ControlLoopType { POSITION, VELOCITY, PERCENT; } + /** + * Sets the motor to brake mode. + * This will make it try to stop when not power. + */ public void setBrakeMode() { this.setIdleMode(CANSparkBase.IdleMode.kCoast); } + /** + * Sets the motor to coast mode. + * This will make it spin freely when not powered. + */ public void setCoastMode() { this.setIdleMode(CANSparkBase.IdleMode.kBrake); }