Skip to content

Commit

Permalink
Refactor code
Browse files Browse the repository at this point in the history
  • Loading branch information
@viggy96
viggy96 committed Dec 28, 2023
1 parent 88f1a44 commit 68b40af
Show file tree
Hide file tree
Showing 5 changed files with 129 additions and 84 deletions.
2 changes: 2 additions & 0 deletions src/main/java/org/lasarobotics/drive/MAXSwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -274,8 +274,10 @@ public void set(SwerveModuleState state) {
// Auto lock modules if auto lock enabled, speed not requested, and time has elapsed
if (m_autoLock && state.speedMetersPerSecond < EPSILON) {
state.speedMetersPerSecond = 0.0;
// Time's up, lock now...
if (Duration.between(m_autoLockTimer, Instant.now()).toMillis() > m_autoLockTime)
state.angle = LOCK_POSITION.minus(m_location.offset);
// Waiting to lock...
else state.angle = m_previousRotatePosition.minus(m_location.offset);
} else {
// Not locking this loop, restart timer...
Expand Down
10 changes: 5 additions & 5 deletions ...lasarobotics/drive/TurnPIDController.java → ...sarobotics/drive/RotatePIDController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;

/** Turn PID controller */
public class TurnPIDController extends PIDController {
/** Rotate PID controller */
public class RotatePIDController extends PIDController {
private final double MIN_DEADBAND = 0.001;
private final double MAX_DEADBAND = 0.2;
private final double FILTER_FACTOR = 1.0 / 3.0;
Expand All @@ -27,14 +27,14 @@ public class TurnPIDController extends PIDController {
private boolean m_isTurning;

/**
* Create an instance of TurnPIDController
* @param turnInputCurve Turn input curve
* Create an instance of RotatePIDController
* @param turnInputCurve Rotate input curve
* @param pidf PID constants
* @param turnScalar Value to turn input by (degrees)
* @param deadband Controller deadband
* @param lookAhead Number of loops to look ahead by
*/
public TurnPIDController(PolynomialSplineFunction turnInputCurve, PIDConstants pidf, double turnScalar, double deadband, double lookAhead) {
public RotatePIDController(PolynomialSplineFunction turnInputCurve, PIDConstants pidf, double turnScalar, double deadband, double lookAhead) {
super(pidf.kP, 0.0, pidf.kD, pidf.period);
this.m_turnScalar = turnScalar;
this.m_deadband = MathUtil.clamp(deadband, MIN_DEADBAND, MAX_DEADBAND);
Expand Down
112 changes: 110 additions & 2 deletions src/main/java/org/lasarobotics/hardware/revrobotics/SparkMax.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -437,8 +437,39 @@ public void initializeSparkPID(SparkPIDConfig config, FeedbackSensor feedbackSen
break;
}

m_config.initializeSparkPID(this, selectedSensor, forwardLimitSwitch, reverseLimitSwitch);
burnFlash();
// Configure feedback sensor and set sensor phase
try {
m_spark.getPIDController().setFeedbackDevice(selectedSensor);
selectedSensor.setInverted(m_config.getSensorPhase());
} catch (IllegalArgumentException e) {}

// Configure forward and reverse soft limits
if (config.isSoftLimitEnabled()) {
setForwardSoftLimit(config.getUpperLimit());
enableForwardSoftLimit();
setReverseSoftLimit(config.getLowerLimit());
enableReverseSoftLimit();
}

// Configure forward and reverse limit switches if required, and disable soft limit
if (forwardLimitSwitch) {
enableForwardLimitSwitch();
disableForwardSoftLimit();
}
if (reverseLimitSwitch) {
enableReverseLimitSwitch();
disableReverseSoftLimit();
}

// Invert motor if required
setInverted(config.getInverted());

// Configure PID values
setP(config.getP());
setI(config.getI());
setD(config.getD());
setF(config.getF());
setIzone(config.getI() != 0.0 ? config.getTolerance() * 2 : 0.0);
}

/**
Expand Down Expand Up @@ -594,6 +625,83 @@ public REVLibError setVelocityConversionFactor(FeedbackSensor sensor, double fac
return status;
}

/**
* Set proportional gain for PIDF controller on Spark Max
* @param value Value to set
* @return {@link REVLibError#kOk} if successful
*/
public REVLibError setP(double value) {
REVLibError status;
status = applyParameter(
() -> m_spark.getPIDController().setP(value),
() -> m_spark.getPIDController().getP() == value,
"Set kP failure!"
);
return status;
}

/**
* Set integral gain for PIDF controller on Spark Max
* @param value Value to set
* @return {@link REVLibError#kOk} if successful
*/
public REVLibError setI(double value) {
REVLibError status;
status = applyParameter(
() -> m_spark.getPIDController().setI(value),
() -> m_spark.getPIDController().getI() == value,
"Set kI failure!"
);
return status;
}

/**
* Set derivative gain for PIDF controller on Spark Max
* @param value Value to set
* @return {@link REVLibError#kOk} if successful
*/
public REVLibError setD(double value) {
REVLibError status;
status = applyParameter(
() -> m_spark.getPIDController().setD(value),
() -> m_spark.getPIDController().getD() == value,
"Set kD failure!"
);
return status;
}

/**
* Set feed-forward gain for PIDF controller on Spark Max
* @param value Value to set
* @return {@link REVLibError#kOk} if successful
*/
public REVLibError setF(double value) {
REVLibError status;
status = applyParameter(
() -> m_spark.getPIDController().setFF(value),
() -> m_spark.getPIDController().getFF() == value,
"Set kF failure!"
);
return status;
}

/**
* Set integral zone range for PIDF controller on Spark Max
* <p>
* This value specifies the range the |error| must be within for the integral constant to take effect
* @param value Value to set
* @return {@link REVLibError#kOk} if successful
*/
public REVLibError setIzone(double value) {
REVLibError status;
status = applyParameter(
() -> m_spark.getPIDController().setIZone(value),
() -> m_spark.getPIDController().getIZone() == value,
"Set Izone failure!"
);
return status;
}

/**
* Execute a smooth motion to desired position
* @param value The target value for the motor
Expand Down
83 changes: 9 additions & 74 deletions src/main/java/org/lasarobotics/hardware/revrobotics/SparkPIDConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,6 @@

import org.lasarobotics.utils.PIDConstants;

import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.SparkMaxPIDController;

/**
* Automates the configuration of Spark PID
*/
Expand Down Expand Up @@ -71,72 +68,6 @@ public SparkPIDConfig(PIDConstants pidf, boolean sensorPhase, boolean invertMoto
this.m_enableSoftLimits = enableSoftLimits;
}

/**
* Initializes Spark Max PID
* <p>
* Must call {@link SparkMax#burnFlash()} after configuring all settings
* @param spark Spark motor controller to apply settings to
* @param feedbackSensor Feedback device to use for Spark PID
* @param forwardLimitSwitch Enable forward limit switch
* @param reverseLimitSwitch Enable reverse limit switch
*/
public void initializeSparkPID(SparkMax spark, MotorFeedbackSensor feedbackSensor,
boolean forwardLimitSwitch, boolean reverseLimitSwitch) {
// Get PID controller
SparkMaxPIDController pidController = spark.getMotorController().getPIDController();

// Configure feedback sensor and set sensor phase
try {
pidController.setFeedbackDevice(feedbackSensor);
feedbackSensor.setInverted(m_sensorPhase);
} catch (IllegalArgumentException e) {}

// Configure forward and reverse soft limits
if (m_enableSoftLimits) {
spark.setForwardSoftLimit(m_upperLimit);
spark.enableForwardSoftLimit();
spark.setReverseSoftLimit(m_lowerLimit);
spark.enableReverseSoftLimit();
}

// Configure forward and reverse limit switches if required, and disable soft limit
if (forwardLimitSwitch) {
spark.enableForwardLimitSwitch();
spark.disableForwardSoftLimit();
}
if (reverseLimitSwitch) {
spark.enableReverseLimitSwitch();
spark.disableReverseSoftLimit();
}

// Invert motor if required
spark.setInverted(m_invertMotor);

// Configure PID values
spark.applyParameter(() -> pidController.setP(m_kP), () -> pidController.getP() == m_kP, "Set kP failure!");
spark.applyParameter(() -> pidController.setI(m_kI), () -> pidController.getI() == m_kI, "Set kI failure!");
spark.applyParameter(() -> pidController.setD(m_kD), () -> pidController.getD() == m_kD, "Set kD failure!");
spark.applyParameter(() -> pidController.setFF(m_kF), () -> pidController.getFF() == m_kF, "Set kF failure!");
spark.applyParameter(
() -> pidController.setIZone(m_kI != 0.0 ? m_tolerance * 2 : 0.0),
() -> pidController.getIZone() == (m_kI != 0.0 ? m_tolerance * 2 : 0.0),
"Set Izone failure!"
);
}

/**
* Initializes Spark PID
* <p>
* Calls {@link SparkPIDConfig#initializeSparkPID(SparkMax, MotorFeedbackSensor, boolean, boolean)} with no limit switches
* <p>
* Must call {@link SparkMax#burnFlash()} after configuring all settings
* @param spark Spark motor controller to apply settings to
* @param feedbackSensor Feedback device to use for Spark PID
*/
public void initializeSparkPID(SparkMax spark, MotorFeedbackSensor feedbackSensor) {
initializeSparkPID(spark, feedbackSensor, false, false);
}

/**
* @return Sensor phase
*/
Expand All @@ -147,35 +78,35 @@ public boolean getSensorPhase() {
/**
* @return Whether motor is inverted or not
*/
public boolean getInvertMotor() {
public boolean getInverted() {
return m_invertMotor;
}

/**
* @return Proportional gain
*/
public double getkP() {
public double getP() {
return m_kP;
}

/**
* @return Integral gain
*/
public double getkI() {
public double getI() {
return m_kI;
}

/**
* @return Derivative gain
*/
public double getkD() {
public double getD() {
return m_kD;
}

/**
* @return Feed-forward gain
*/
public double getkF() {
public double getF() {
return m_kF;
}

Expand All @@ -186,6 +117,10 @@ public double getTolerance() {
return m_tolerance;
}

public boolean isSoftLimitEnabled() {
return m_enableSoftLimits;
}

/**
* @return Lower limit of mechanism
*/
Expand Down
6 changes: 3 additions & 3 deletions ...robotics/drive/TurnPIDControllerTest.java → ...botics/drive/RotatePIDControllerTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
import org.lasarobotics.utils.PIDConstants;

@TestMethodOrder(MethodOrderer.OrderAnnotation.class)
public class TurnPIDControllerTest {
public class RotatePIDControllerTest {
private final double DELTA = 1e-5;

private static final double CONTROLLER_DEADBAND = 0.10;
Expand All @@ -30,11 +30,11 @@ public class TurnPIDControllerTest {
private static final double DRIVE_TURN_SCALAR = 30.0;
private static final double DRIVE_LOOKAHEAD = 3;

private TurnPIDController m_turnPIDController;
private RotatePIDController m_turnPIDController;

@BeforeEach
public void setup() {
m_turnPIDController = new TurnPIDController(DRIVE_TURN_INPUT_CURVE, DRIVE_TURN_PID, DRIVE_TURN_SCALAR, CONTROLLER_DEADBAND, DRIVE_LOOKAHEAD);
m_turnPIDController = new RotatePIDController(DRIVE_TURN_INPUT_CURVE, DRIVE_TURN_PID, DRIVE_TURN_SCALAR, CONTROLLER_DEADBAND, DRIVE_LOOKAHEAD);
}

@Test
Expand Down

0 comments on commit 68b40af

Please sign in to comment.