diff --git a/src/main/java/common/core/NAR_Robot.java b/src/main/java/common/core/NAR_Robot.java index 29c902c..01b999f 100644 --- a/src/main/java/common/core/NAR_Robot.java +++ b/src/main/java/common/core/NAR_Robot.java @@ -13,6 +13,11 @@ import edu.wpi.first.wpilibj.IterativeRobotBase; import edu.wpi.first.wpilibj.Timer; +/** + * Team 3128's Robot class that includes advantageScope and addPeriodic + * @since 2023 CHARGED UP + * @author Mason Lam + */ public class NAR_Robot extends IterativeRobotBase { @SuppressWarnings("MemberName") static class Callback implements Comparable { diff --git a/src/main/java/common/core/controllers/Controller.java b/src/main/java/common/core/controllers/Controller.java index 9eb98a6..6169d32 100644 --- a/src/main/java/common/core/controllers/Controller.java +++ b/src/main/java/common/core/controllers/Controller.java @@ -42,6 +42,7 @@ public Controller(double kP, double kI, double kD, double period) { kG_Function = () -> 1; consumers = new LinkedList(); } + /** * Create a new object to control PID logic for a subsystem. *

Sets kP, kI, kD values. @@ -66,6 +67,7 @@ public Controller(double kP, double kI, double kD) { public void setMeasurementSource(DoubleSupplier measurement) { this.measurement = measurement; } + /** * Adds a motor set method to consumers to be later used during PID logic. * @see LinkedList consumers: includes void functions that accept double values. @@ -74,6 +76,7 @@ public void setMeasurementSource(DoubleSupplier measurement) { public void addMotor(NAR_Motor motor) { consumers.add(motor::set); } + /** * Adds a useOutput method to consumers to be later used during PID logic. * @see LinkedList consumers: includes void functions that accept double values. @@ -82,6 +85,7 @@ public void addMotor(NAR_Motor motor) { public void addOutput(DoubleConsumer output) { consumers.add(output); } + /** * Calculates the output based on calculations from the measurement, then gives each DoubleConsumer in consumers the output as an argument. * @return calculated output based on calculations from the measurement @@ -95,6 +99,7 @@ public double useOutput() { } return output; } + /** * Sets the kG_Function to a new DoubleSupplier. * @param kG_Function DoubleSupplier with specified logic. @@ -102,6 +107,7 @@ public double useOutput() { public void setkG_Function(DoubleSupplier kG_Function) { this.kG_Function = kG_Function; } + /** * Sets the kS value. * @param kS The constant power required to overcome static friction as a double. @@ -109,6 +115,7 @@ public void setkG_Function(DoubleSupplier kG_Function) { public void setkS(double kS) { setkS(()-> kS); } + /** * Sets the kS value. * @param kS The constant power required to overcome static friction as a DoubleSupplier. @@ -116,6 +123,7 @@ public void setkS(double kS) { public void setkS(DoubleSupplier kS) { this.kS = kS; } + /** * Sets the kV value. * @param kV The constant power required to maintain a set velocity as a double. @@ -123,6 +131,7 @@ public void setkS(DoubleSupplier kS) { public void setkV(double kV) { setkV(()-> kV); } + /** * Sets the kV value. * @param kV The constant power required to maintain a set velocity as a DoubleSupplier. @@ -130,6 +139,7 @@ public void setkV(double kV) { public void setkV(DoubleSupplier kV) { this.kV = kV; } + /** * Sets the kG value. * @param kG The constant power required to overcome gravity as a double. @@ -137,6 +147,7 @@ public void setkV(DoubleSupplier kV) { public void setkG(double kG) { this.kG = ()-> kG; } + /** * Sets the kG value. * @param kG The constant power required to overcome gravity as a DoubleSupplier. @@ -144,6 +155,7 @@ public void setkG(double kG) { public void setkG(DoubleSupplier kG) { this.kG = kG; } + /** * Returns kS. * @return returns kS as a double. @@ -151,6 +163,7 @@ public void setkG(DoubleSupplier kG) { public double getkS() { return kS.getAsDouble(); } + /** * Returns kV. * @return returns kV as a double. @@ -158,6 +171,7 @@ public double getkS() { public double getkV() { return kV.getAsDouble(); } + /** * Returns kG. * @return returns kG as a double. diff --git a/src/main/java/common/core/controllers/PController.java b/src/main/java/common/core/controllers/PController.java index 60701e5..8d392ab 100644 --- a/src/main/java/common/core/controllers/PController.java +++ b/src/main/java/common/core/controllers/PController.java @@ -8,55 +8,56 @@ * @author Mason Lam, William Yuan */ public class PController extends Controller { + /** + * Create a new object to control position PID logic for a subsystem. + *

Sets kP, kI, kD, period values. + *

Sets kS, kG values. + *

Defaults kG_Function to 1. + * + * @param kS The constant power required to overcome static friction. + * @param kG The constant power required to overcome gravity. + * @param kP The proportional coefficient of the on board PIDController. + * @param kI The integral coefficient of the on board PIDController. + * @param kD The derivative coefficient of the on board PIDController. + * @param period The PID periodic update rate in seconds. + */ + public PController(double kS, double kG, double kP, double kI, double kD, double period) { + super(kP, kI, kD, period); + setkS(()-> kS); + setkG(()-> kG); + setkG_Function(() -> 1); + } - /** - * Create a new object to control position PID logic for a subsystem. - *

Sets kP, kI, kD, period values. - *

Sets kS, kG values. - *

Defaults kG_Function to 1. - * - * @param kS The constant power required to overcome static friction. - * @param kG The constant power required to overcome gravity. - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - * @param period The PID periodic update rate in seconds. - */ - public PController(double kS, double kG, double kP, double kI, double kD, double period) { - super(kP, kI, kD, period); - setkS(()-> kS); - setkG(()-> kG); - setkG_Function(() -> 1); - } - /** - * Create a new object to control position PID logic for a subsystem. - *

Sets kP, kI, kD values. - *

Sets kS, kG values. - *

Defaults kG_Function to 1. - *

Defaults period to 0.02. - * - * @param kS The constant power required to overcome static friction. - * @param kG The constant power required to overcome gravity. - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - */ - public PController(double kS, double kG, double kP, double kI, double kD) { - this(kS, kG, kP, kI, kD, 0.02); - } - /** - * Overrides the calculate method in {@link PIDController} to use PID and FF logic. - *

Uses kG instead of kV. - * - * @param measurement The desired setpoint for the subsystem. - * - * @return the output from PID and FF. - */ - @Override - public double calculate(double measurement) { - final double pid = super.calculate(measurement); - final double ff = !atSetpoint() ? Math.copySign(getkS(), pid) : 0 + getkG() * kG_Function.getAsDouble(); - return pid + ff; - } - + /** + * Create a new object to control position PID logic for a subsystem. + *

Sets kP, kI, kD values. + *

Sets kS, kG values. + *

Defaults kG_Function to 1. + *

Defaults period to 0.02. + * + * @param kS The constant power required to overcome static friction. + * @param kG The constant power required to overcome gravity. + * @param kP The proportional coefficient of the on board PIDController. + * @param kI The integral coefficient of the on board PIDController. + * @param kD The derivative coefficient of the on board PIDController. + */ + public PController(double kS, double kG, double kP, double kI, double kD) { + this(kS, kG, kP, kI, kD, 0.02); } + + /** + * Overrides the calculate method in {@link PIDController} to use PID and FF logic. + *

Uses kG instead of kV. + * + * @param measurement The desired setpoint for the subsystem. + * + * @return the output from PID and FF. + */ + @Override + public double calculate(double measurement) { + final double pid = super.calculate(measurement); + final double ff = !atSetpoint() ? Math.copySign(getkS(), pid) : 0 + getkG() * kG_Function.getAsDouble(); + return pid + ff; + } + +} diff --git a/src/main/java/common/core/controllers/VController.java b/src/main/java/common/core/controllers/VController.java index 6b751bc..7f96084 100644 --- a/src/main/java/common/core/controllers/VController.java +++ b/src/main/java/common/core/controllers/VController.java @@ -9,51 +9,53 @@ * @author Mason Lam, William Yuan */ public class VController extends Controller { - /** - * Create a new object to control velocity PID logic for a subsystem. - *

Sets kP, kI, kD, period values. - *

Sets kS, kV values. - * - * @param kS The constant power required to overcome static friction. - * @param kV The constant power required to maintain a set velocity. - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - * @param period The PID periodic update rate in seconds. - */ - public VController(double kS, double kV, double kP, double kI, double kD, double period) { - super(kP, kI, kD, period); - setkS(()-> kS); - setkV(()-> kV); - } - /** - * Create a new object to control velocity PID logic for a subsystem. - *

Sets kP, kI, kD values. - *

Sets kS, kV values. - *

Defaults period to 0.02. - * - * @param kS The constant power required to overcome static friction. - * @param kV The constant power required to maintain a set velocity. - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - */ - public VController(double kS, double kV, double kP, double kI, double kD) { - this(kS, kV, kP, kI, kD, 0.02); - } - /** - * Overrides the calculate method in {@link PIDController} to use PID and FF logic. - *

Uses kV instead of kG. - * - * @param measurement The desired setpoint for the subsystem. - * - * @return the output from PID and FF. - */ - @Override - public double calculate(double measurement) { - final double pid = super.calculate(measurement); - final double ff = !atSetpoint() ? Math.copySign(getkS(), pid) : 0 + getkV() * getSetpoint(); - return pid + ff; - } - + /** + * Create a new object to control velocity PID logic for a subsystem. + *

Sets kP, kI, kD, period values. + *

Sets kS, kV values. + * + * @param kS The constant power required to overcome static friction. + * @param kV The constant power required to maintain a set velocity. + * @param kP The proportional coefficient of the on board PIDController. + * @param kI The integral coefficient of the on board PIDController. + * @param kD The derivative coefficient of the on board PIDController. + * @param period The PID periodic update rate in seconds. + */ + public VController(double kS, double kV, double kP, double kI, double kD, double period) { + super(kP, kI, kD, period); + setkS(()-> kS); + setkV(()-> kV); + } + + /** + * Create a new object to control velocity PID logic for a subsystem. + *

Sets kP, kI, kD values. + *

Sets kS, kV values. + *

Defaults period to 0.02. + * + * @param kS The constant power required to overcome static friction. + * @param kV The constant power required to maintain a set velocity. + * @param kP The proportional coefficient of the on board PIDController. + * @param kI The integral coefficient of the on board PIDController. + * @param kD The derivative coefficient of the on board PIDController. + */ + public VController(double kS, double kV, double kP, double kI, double kD) { + this(kS, kV, kP, kI, kD, 0.02); } + + /** + * Overrides the calculate method in {@link PIDController} to use PID and FF logic. + *

Uses kV instead of kG. + * + * @param measurement The desired setpoint for the subsystem. + * + * @return the output from PID and FF. + */ + @Override + public double calculate(double measurement) { + final double pid = super.calculate(measurement); + final double ff = !atSetpoint() ? Math.copySign(getkS(), pid) : 0 + getkV() * getSetpoint(); + return pid + ff; + } + +} diff --git a/src/main/java/common/hardware/camera/Camera.java b/src/main/java/common/hardware/camera/Camera.java index 75a1db1..9c7f9cf 100644 --- a/src/main/java/common/hardware/camera/Camera.java +++ b/src/main/java/common/hardware/camera/Camera.java @@ -5,7 +5,7 @@ /** * Represents and holds the specs of a camera. * - * @since 2023 Charged Up + * @since 2022 RAPID REACT * @author Mason Lam, William Yuan */ public class Camera { diff --git a/src/main/java/common/hardware/input/NAR_ButtonBoard.java b/src/main/java/common/hardware/input/NAR_ButtonBoard.java index 1ae1776..0b6f384 100644 --- a/src/main/java/common/hardware/input/NAR_ButtonBoard.java +++ b/src/main/java/common/hardware/input/NAR_ButtonBoard.java @@ -5,7 +5,7 @@ /** * Wrapper for the WPILib Joystick class. Works with device that can be recognized as joystick as driverstation. - * @since 2023 Charged Up + * @since 2023 Charged UP * @author Peter Ma, Arav Chadha */ public class NAR_ButtonBoard { diff --git a/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java b/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java index cd4fbd2..779bd0b 100644 --- a/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java +++ b/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java @@ -24,6 +24,9 @@ * @author Mason Lam */ public class NAR_CANSparkMax extends NAR_Motor { + /** + * Team 3128's status frames + */ public enum SparkMaxConfig { DEFAULT(MAX_PRIORITY, HIGH_PRIORITY, HIGH_PRIORITY, NO_PRIORITY, NO_PRIORITY, NO_PRIORITY, NO_PRIORITY), FOLLOWER(MEDIUM_PRIORITY, NO_PRIORITY, NO_PRIORITY, NO_PRIORITY, NO_PRIORITY, NO_PRIORITY, NO_PRIORITY), @@ -44,6 +47,9 @@ private SparkMaxConfig(int status0, int status1, int status2, int status3, int s } } + /** + * Type of encoder used + */ public enum EncoderType { Relative, Absolute @@ -250,8 +256,15 @@ public void enableContinuousInput(double minInput, double maxInput) { */ public void enableContinuousInput(double minInput, double maxInput, double factor) { controller.setPositionPIDWrappingEnabled(true); - controller.setPositionPIDWrappingMinInput(minInput * factor); - controller.setPositionPIDWrappingMaxInput(maxInput * factor); + controller.setPositionPIDWrappingMinInput(minInput / factor); + controller.setPositionPIDWrappingMaxInput(maxInput / factor); + } + + /** + * Burns all settings to flash; stores settings between power cycles + */ + public void burnFlash() { + motor.burnFlash(); } @Override @@ -264,11 +277,6 @@ protected void setPercentOutput(double speed) { controller.setReference(speed, ControlType.kDutyCycle); } - @Override - protected void setVoltage(double volts) { - controller.setReference(volts, ControlType.kVoltage); - } - @Override protected void setVelocity(double rpm, double feedForward) { controller.setReference(rpm, ControlType.kVelocity, 0, feedForward); diff --git a/src/main/java/common/hardware/motorcontroller/NAR_Motor.java b/src/main/java/common/hardware/motorcontroller/NAR_Motor.java index 19dce5e..d20450d 100644 --- a/src/main/java/common/hardware/motorcontroller/NAR_Motor.java +++ b/src/main/java/common/hardware/motorcontroller/NAR_Motor.java @@ -7,6 +7,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.motorcontrol.MotorController; +/** + * Team 3128's motor class wrapper replacement for {@link MotorController} + * @since CHARGED UP 2023 + * @author Mason Lam + */ public abstract class NAR_Motor { private static final HashSet leaders = new HashSet(); @@ -21,13 +26,18 @@ public abstract class NAR_Motor { }, 0.1); } + /** + * Motor control modes + */ public enum Control { PercentOutput, - Voltage, Velocity, Position; } + /** + * Motor states when no voltage is applied + */ public enum Neutral { BRAKE, COAST @@ -40,37 +50,29 @@ public enum Neutral { protected double unitConversionFactor = 1; protected double timeConversionFactor = 1; - public void follow(NAR_Motor leader) { - leader.followers.add(this); - leaders.add(leader); - } - - public abstract void enableVoltageCompensation(double volts); - - public void setNeutralMode(Neutral mode) { - switch(mode) { - case BRAKE: - setBrakeMode(); - break; - case COAST: - setCoastMode(); - break; - } - } - - protected abstract void setBrakeMode(); - - protected abstract void setCoastMode(); - - + /** + * Sets motor output power + * @param output The speed of the motor from -1 to 1 + */ public void set(double output) { set(output, Control.PercentOutput); } + /** + * Sets the motor state + * @param value Output of motor dependent on control mode + * @param mode Type of control mode + */ public void set(double value, Control mode) { set(value, mode, 0); } + /** + * Sets the motor state with feedforward + * @param value Output of motor dependent on control mode + * @param mode Type of control mode + * @param feedForward Feedforward of motor measured in volts + */ public void set(double value, Control mode, double feedForward) { if (value == prevValue && mode == prevMode && feedForward == prevFeedForward) return; prevValue = value; @@ -80,55 +82,148 @@ public void set(double value, Control mode, double feedForward) { case PercentOutput: setPercentOutput(MathUtil.clamp(value, -1, 1)); break; - case Voltage: - setVoltage(MathUtil.clamp(value, -12, 12)); - break; case Velocity: - setVelocity(value, feedForward); + setVelocity(value / unitConversionFactor * timeConversionFactor, feedForward); break; case Position: - setPosition(value, feedForward); + setPosition(value / unitConversionFactor, feedForward); break; } } + /** + * Changes the units the motor measures position in, ie. rotations -> degrees + * @param conversionFactor Conversion factor to change position units + */ public void setUnitConversionFactor(double conversionFactor) { this.unitConversionFactor = conversionFactor; } + /** + * Changes the units the motor measures time/velocity in, ie. rotations per minute -> rotations per second + * @param conversionFactor Conversion factor to change time units + */ public void setTimeConversionFactor(double conversionFactor) { this.timeConversionFactor = conversionFactor; } - public void resetPosition(double units) { - resetPosition(units * unitConversionFactor); + /** + * Resets the motor position + * @param position Motor position, default units - rotations + */ + public void resetPosition(double position) { + resetRawPosition(position / unitConversionFactor); } + /** + * Sets the motor inverted + * @param inverted Inverts the motor + */ public abstract void setInverted(boolean inverted); + /** + * Sets motor output power + * @param speed The speed of the motor from -1 to 1 + */ protected abstract void setPercentOutput(double speed); - protected abstract void setVoltage(double volts); - + /** + * Sets the motor output based on its velocity + * @param rpm Velocity of the motor in RPM + * @param feedForward Feedforward component measured in volts + */ protected abstract void setVelocity(double rpm, double feedForward); + /** + * Sets motor output based on its position + * @param rotations Position of the motor in rotations + * @param feedForward Feedforward component measured in volts + */ protected abstract void setPosition(double rotations, double feedForward); + /** + * Resets the motor position + * @param rotations Number of rotations + */ protected abstract void resetRawPosition(double rotations); + /** + * Gets the motors current output + * @return Double measuring percent output from -1 to 1 + */ public abstract double getAppliedOutput(); + /** + * Returns the current motor position, default unit - rotations + * @return Double measuring motor position + */ public double getPosition() { - return getRawPosition() / unitConversionFactor; + return getRawPosition() * unitConversionFactor; } + /** + * Returns the current motor velocity, default unit - RPM + * @return Double measuring motor velocity + */ public double getVelocity() { - return getRawVelocity() / unitConversionFactor / timeConversionFactor; + return getRawVelocity() * unitConversionFactor / timeConversionFactor; } + /** + * Returns the motor position in rotations + * @return Double measuring motor position + */ protected abstract double getRawPosition(); + /** + * Returns the current motor velocity in RPM + * @return Double measuring motor velocity + */ protected abstract double getRawVelocity(); + /** + * Sets a motor's output based on anothers + * @param leader The motor to follow + */ + public void follow(NAR_Motor leader) { + leader.followers.add(this); + leaders.add(leader); + } + + /** + * Sets the motor's idle mode, its behavior when no voltage is applied + * @param mode Type of idle mode + */ + public void setNeutralMode(Neutral mode) { + switch(mode) { + case BRAKE: + setBrakeMode(); + break; + case COAST: + setCoastMode(); + break; + } + } + + /** + * Sets the motor in brake mode + */ + protected abstract void setBrakeMode(); + + /** + * Sets the motor in coast mode + */ + protected abstract void setCoastMode(); + + /** + * Sets voltage compensation, keeps output consistent when battery is above x volts + * @param volts The max volts the motor goes too + */ + public abstract void enableVoltageCompensation(double volts); + + /** + * Returns the motor object controlling the motion + * @return The motor ie. SparkMax, TalonFX + */ public abstract MotorController getMotor(); } diff --git a/src/main/java/common/hardware/motorcontroller/NAR_TalonFX.java b/src/main/java/common/hardware/motorcontroller/NAR_TalonFX.java index a75d443..2abc3bd 100644 --- a/src/main/java/common/hardware/motorcontroller/NAR_TalonFX.java +++ b/src/main/java/common/hardware/motorcontroller/NAR_TalonFX.java @@ -158,11 +158,6 @@ protected void setPercentOutput(double speed) { motor.set(speed); } - @Override - protected void setVoltage(double volts) { - motor.set(volts / 12.0); - } - @Override protected void setVelocity(double rpm, double feedForward) { motor.set(ControlMode.Velocity, rpm * RPM_TO_FALCON, DemandType.ArbitraryFeedForward, feedForward / 12.0); diff --git a/src/main/java/common/hardware/motorcontroller/NAR_TalonSRX.java b/src/main/java/common/hardware/motorcontroller/NAR_TalonSRX.java index 0fe544a..c36359b 100644 --- a/src/main/java/common/hardware/motorcontroller/NAR_TalonSRX.java +++ b/src/main/java/common/hardware/motorcontroller/NAR_TalonSRX.java @@ -6,6 +6,11 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +/** + * Team 3128's streamlined {@link WPI_TalonSRX} class. + * @since 2023 CHARGED UP + * @author Mason Lam + */ public class NAR_TalonSRX extends NAR_Motor { protected WPI_TalonSRX motor; @@ -43,11 +48,6 @@ protected void setPercentOutput(double speed) { motor.set(speed); } - @Override - protected void setVoltage(double volts) { - motor.set(volts / 12.0); - } - @Override protected void setVelocity(double rpm, double feedForward) { motor.set(ControlMode.Velocity, rpm * RPM_TO_TALONSRX, DemandType.ArbitraryFeedForward, feedForward / 12.0); diff --git a/src/main/java/common/narwhaldashboard/NarwhalDashboard.java b/src/main/java/common/narwhaldashboard/NarwhalDashboard.java index 5a5cb83..12efd22 100644 --- a/src/main/java/common/narwhaldashboard/NarwhalDashboard.java +++ b/src/main/java/common/narwhaldashboard/NarwhalDashboard.java @@ -120,7 +120,7 @@ public void addAutos(String... names) { /** * Called after connecting to the web server - * @param conn The WebSocket instance this event is occuring on. + * @param conn The WebSocket instance this event is occuring on. * @param handshake The handshake of the websocket instance */ @SuppressWarnings("unchecked") @@ -197,7 +197,7 @@ public void addButton(String key, BooleanConsumer button) { /** * Called when a message from the web server is sent - * @param conn The WebSocket instance this event is occurring on. + * @param conn The WebSocket instance this event is occurring on. * @param message The UTF-8 decoded message that was received. */ @Override