Skip to content

Commit

Permalink
Motor comments
Browse files Browse the repository at this point in the history
General cleanup
  • Loading branch information
Mason-Lam committed Nov 20, 2023
1 parent a9d6427 commit be3be64
Show file tree
Hide file tree
Showing 11 changed files with 273 additions and 153 deletions.
5 changes: 5 additions & 0 deletions src/main/java/common/core/NAR_Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -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<Callback> {
Expand Down
14 changes: 14 additions & 0 deletions src/main/java/common/core/controllers/Controller.java
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ public Controller(double kP, double kI, double kD, double period) {
kG_Function = () -> 1;
consumers = new LinkedList<DoubleConsumer>();
}

/**
* Create a new object to control PID logic for a subsystem.
* <p>Sets kP, kI, kD values.
Expand All @@ -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.
Expand All @@ -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.
Expand All @@ -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
Expand All @@ -95,69 +99,79 @@ public double useOutput() {
}
return output;
}

/**
* Sets the kG_Function to a new DoubleSupplier.
* @param kG_Function DoubleSupplier with specified logic.
*/
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.
*/
public void setkS(double kS) {
setkS(()-> kS);
}

/**
* Sets the kS value.
* @param kS The constant power required to overcome static friction as a DoubleSupplier.
*/
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.
*/
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.
*/
public void setkV(DoubleSupplier kV) {
this.kV = kV;
}

/**
* Sets the kG value.
* @param kG The constant power required to overcome gravity as a double.
*/
public void setkG(double kG) {
this.kG = ()-> kG;
}

/**
* Sets the kG value.
* @param kG The constant power required to overcome gravity as a DoubleSupplier.
*/
public void setkG(DoubleSupplier kG) {
this.kG = kG;
}

/**
* Returns kS.
* @return returns kS as a double.
*/
public double getkS() {
return kS.getAsDouble();
}

/**
* Returns kV.
* @return returns kV as a double.
*/
public double getkV() {
return kV.getAsDouble();
}

/**
* Returns kG.
* @return returns kG as a double.
Expand Down
101 changes: 51 additions & 50 deletions src/main/java/common/core/controllers/PController.java
Original file line number Diff line number Diff line change
Expand Up @@ -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.
* <p>Sets kP, kI, kD, period values.
* <p>Sets kS, kG values.
* <p>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.
* <p>Sets kP, kI, kD, period values.
* <p>Sets kS, kG values.
* <p>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.
* <p>Sets kP, kI, kD values.
* <p>Sets kS, kG values.
* <p>Defaults kG_Function to 1.
* <p>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.
* <p>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.
* <p>Sets kP, kI, kD values.
* <p>Sets kS, kG values.
* <p>Defaults kG_Function to 1.
* <p>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.
* <p>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;
}

}
96 changes: 49 additions & 47 deletions src/main/java/common/core/controllers/VController.java
Original file line number Diff line number Diff line change
Expand Up @@ -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.
* <p>Sets kP, kI, kD, period values.
* <p>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.
* <p>Sets kP, kI, kD values.
* <p>Sets kS, kV values.
* <p>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.
* <p>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.
* <p>Sets kP, kI, kD, period values.
* <p>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.
* <p>Sets kP, kI, kD values.
* <p>Sets kS, kV values.
* <p>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.
* <p>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;
}

}
2 changes: 1 addition & 1 deletion src/main/java/common/hardware/camera/Camera.java
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/common/hardware/input/NAR_ButtonBoard.java
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
22 changes: 15 additions & 7 deletions src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand Down
Loading

0 comments on commit be3be64

Please sign in to comment.