From 934d49114fd9944a9939fde6dc028da24f19e7df Mon Sep 17 00:00:00 2001 From: Jacob Bublitz Date: Fri, 6 Dec 2019 14:18:49 -0800 Subject: [PATCH 1/3] Make getDriveCurrent and getCurrentVelocity thread-safe --- .../common/robot/drivers/Mk1SwerveModule.java | 12 +- .../common/robot/drivers/Mk2SwerveModule.java | 23 +-- .../drivers/OnboardPid2017SwerveModule.java | 37 ---- .../robot/drivers/OnboardPidSwerveModule.java | 159 ------------------ .../common/drivers/SwerveModule.java | 40 +++-- .../common/drivers/MockSwerveModule.java | 20 +++ 6 files changed, 67 insertions(+), 224 deletions(-) delete mode 100644 robot/src/main/java/org/frcteam2910/common/robot/drivers/OnboardPid2017SwerveModule.java delete mode 100644 robot/src/main/java/org/frcteam2910/common/robot/drivers/OnboardPidSwerveModule.java diff --git a/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java b/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java index b436ec0b..a1169812 100644 --- a/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java +++ b/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java @@ -4,8 +4,8 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import org.frcteam2910.common.math.Vector2; import org.frcteam2910.common.drivers.SwerveModule; +import org.frcteam2910.common.math.Vector2; import static org.frcteam2910.common.robot.Constants.CAN_TIMEOUT_MS; @@ -118,6 +118,16 @@ public double readDistance() { return driveMotor.getSelectedSensorPosition() / driveTicksPerUnit; } + @Override + protected double readDriveCurrent() { + return driveMotor.getOutputCurrent(); + } + + @Override + protected double readVelocity() { + return driveMotor.getSelectedSensorVelocity() / driveTicksPerUnit * (1000.0 / 100.0); + } + @Override protected void setTargetAngle(double angle) { angleMotor.set(ControlMode.Position, angle * ANGLE_TICKS_PER_RADIAN); diff --git a/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModule.java b/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModule.java index 12620cac..3585a693 100644 --- a/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModule.java +++ b/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModule.java @@ -107,29 +107,18 @@ protected double readDistance() { } } - protected double readVelocity() { - synchronized (canLock) { - return driveVelocity; - } - } - + @Override protected double readDriveCurrent() { - double localDriveCurrent; synchronized (canLock) { - localDriveCurrent = driveCurrent; + return driveCurrent; } - - return localDriveCurrent; } @Override - public double getCurrentVelocity() { - return readVelocity(); - } - - @Override - public double getDriveCurrent() { - return readDriveCurrent(); + protected double readVelocity() { + synchronized (canLock) { + return driveVelocity; + } } @Override diff --git a/robot/src/main/java/org/frcteam2910/common/robot/drivers/OnboardPid2017SwerveModule.java b/robot/src/main/java/org/frcteam2910/common/robot/drivers/OnboardPid2017SwerveModule.java deleted file mode 100644 index 4b6111c2..00000000 --- a/robot/src/main/java/org/frcteam2910/common/robot/drivers/OnboardPid2017SwerveModule.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.frcteam2910.common.robot.drivers; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import org.frcteam2910.common.control.PidConstants; -import org.frcteam2910.common.math.Vector2; -import org.frcteam2910.common.robot.Constants; - -/** - * A driver for the 2017 swerve module hardware but using onboard pid. - */ -public class OnboardPid2017SwerveModule extends OnboardPidSwerveModule { - private static final PidConstants ANGLE_PID_CONSTANTS = new PidConstants(0.5, 0.0, 0.0); - private static final double DRIVE_TICKS_PER_INCH = 36.65; - - public OnboardPid2017SwerveModule(Vector2 positionOffset, double angleOffset, - WPI_TalonSRX angleMotor, WPI_TalonSRX driveMotor) { - super(positionOffset, angleOffset, - angleMotor, driveMotor, - () -> angleMotor.getSelectedSensorPosition(0) * ((2.0 * Math.PI) / 1024.0), - () -> driveMotor.getSelectedSensorPosition(0) / DRIVE_TICKS_PER_INCH, - ANGLE_PID_CONSTANTS); - - angleMotor.setInverted(true); - angleMotor.setSensorPhase(true); - angleMotor.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0, Constants.CAN_TIMEOUT_MS); - angleMotor.setNeutralMode(NeutralMode.Brake); - - driveMotor.setSensorPhase(true); - driveMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.CAN_TIMEOUT_MS); - angleMotor.setNeutralMode(NeutralMode.Coast); - driveMotor.configContinuousCurrentLimit(30, Constants.CAN_TIMEOUT_MS); - driveMotor.configPeakCurrentLimit(0, Constants.CAN_TIMEOUT_MS); // Disable peak current limiting - driveMotor.enableCurrentLimit(true); - } -} diff --git a/robot/src/main/java/org/frcteam2910/common/robot/drivers/OnboardPidSwerveModule.java b/robot/src/main/java/org/frcteam2910/common/robot/drivers/OnboardPidSwerveModule.java deleted file mode 100644 index c8ef073d..00000000 --- a/robot/src/main/java/org/frcteam2910/common/robot/drivers/OnboardPidSwerveModule.java +++ /dev/null @@ -1,159 +0,0 @@ -package org.frcteam2910.common.robot.drivers; - -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.SpeedController; -import edu.wpi.first.wpilibj.Timer; -import org.frcteam2910.common.control.PidConstants; -import org.frcteam2910.common.control.PidController; -import org.frcteam2910.common.drivers.SwerveModule; -import org.frcteam2910.common.math.Rotation2; -import org.frcteam2910.common.math.Vector2; - -import java.util.function.DoubleSupplier; - -public class OnboardPidSwerveModule extends SwerveModule { - /** - * The default hardware update rate in hertz - */ - public static final double DEFAULT_HARDWARE_UPDATE_RATE = 200.0; - - private final double angleOffset; - - private final SpeedController angleMotor; - private final SpeedController driveMotor; - - private final DoubleSupplier angleEncoder; - private final DoubleSupplier driveEncoder; - - private final PidController angleController; - - private final Notifier hardwareUpdateThread; - - private volatile boolean driveInverted = false; - - private volatile double angleReading; - private volatile double driveReading; - - private volatile double angleOutput; - private volatile double driveOutput; - - private volatile double currentHardwareUpdateRate; - - /** - * @param positionOffset the modules offset from the robot's center of rotation in inches - * @param angleOffset the angle offset of the module in radians - * @param angleMotor the angle motor speed controller - * @param driveMotor the drive motor speed controller - * @param angleEncoder a supplier that will return the module angle encoder value in radians - * @param driveEncoder a supplier that will return the module drive encoder value in inches - * @param anglePidConstants the pid constants used for the the angle pid controller - */ - public OnboardPidSwerveModule(Vector2 positionOffset, double angleOffset, - SpeedController angleMotor, SpeedController driveMotor, - DoubleSupplier angleEncoder, DoubleSupplier driveEncoder, - PidConstants anglePidConstants) { - super(positionOffset); - this.angleOffset = angleOffset; - - this.angleMotor = angleMotor; - this.driveMotor = driveMotor; - this.angleEncoder = angleEncoder; - this.driveEncoder = driveEncoder; - - this.angleController = new PidController(anglePidConstants); - angleController.setContinuous(true); - angleController.setInputRange(0.0, 2.0 * Math.PI); - - this.hardwareUpdateThread = new Notifier(new HardwareUpdater()); - hardwareUpdateThread.startPeriodic(1.0 / DEFAULT_HARDWARE_UPDATE_RATE); - } - - /** - * Sets the hardware update thread rate. The default rate is {@value #DEFAULT_HARDWARE_UPDATE_RATE}. - * - * @param updateRate the rate in hertz that the hardware update thread should run. - */ - public void setHardwareUpdateRate(double updateRate) { - hardwareUpdateThread.startPeriodic(1.0 / updateRate); - } - - /** - * Gets the current rate that the hardware update thread is running at. This is not the rate specified by - * {@link #setHardwareUpdateRate(double)} but instead is determined by how fast the hardware thread is currently - * running. - * - * @return the rate the hardware update thread is running in hertz - */ - public double getCurrentHardwareUpdateRate() { - return currentHardwareUpdateRate; - } - - /** - * Gets the PID controller that controls the module angle. - *

- * This should only be called by the constructor. - * - * @return the angle controller - */ - protected PidController getAngleController() { - return angleController; - } - - @Override - protected double readAngle() { - double angle = angleReading + angleOffset % 2.0 * Math.PI; - if (angle < 0.0) { - angle += 2.0 * Math.PI; - } - - return angle; - } - - @Override - protected void setTargetAngle(double angle) { - synchronized (angleController) { - angleController.setSetpoint(angle); - } - } - - @Override - public void setDriveOutput(double output) { - driveOutput = output; - } - - @Override - public double readDistance() { - return driveReading; - } - - @Override - public void updateState(double dt) { - super.updateState(dt); - - double currentAngle = getCurrentAngle(); - - synchronized (angleController) { - angleOutput = angleController.calculate(currentAngle, dt); - } - } - - private class HardwareUpdater implements Runnable { - private double lastUpdateTime = 0.0; - - @Override - public void run() { - double now = Timer.getFPGATimestamp(); - double dt = now - lastUpdateTime; - lastUpdateTime = now; - currentHardwareUpdateRate = 1.0 / dt; - - // Read the encoder values - angleReading = angleEncoder.getAsDouble() / (2.0 * Math.PI); - driveReading = driveEncoder.getAsDouble(); - - // Write motor values - angleMotor.set(angleOutput); - driveMotor.set(driveOutput * (driveInverted ? -1.0 : 1.0)); - } - } -} diff --git a/src/main/java/org/frcteam2910/common/drivers/SwerveModule.java b/src/main/java/org/frcteam2910/common/drivers/SwerveModule.java index 1144abc1..10c0eae8 100644 --- a/src/main/java/org/frcteam2910/common/drivers/SwerveModule.java +++ b/src/main/java/org/frcteam2910/common/drivers/SwerveModule.java @@ -9,6 +9,8 @@ public abstract class SwerveModule { private final Object sensorMutex = new Object(); private double currentAngle = 0.0; private double currentDistance = 0.0; + private double currentDriveCurrent = 0.0; + private double currentVelocity = 0.0; private final Object stateMutex = new Object(); private double targetSpeed = 0.0; @@ -58,6 +60,20 @@ public String getName() { */ protected abstract double readDistance(); + /** + * Reads the current draw of the drive motor in amps. + * + * @return the current draw of the drive motor in amps + */ + protected abstract double readDriveCurrent(); + + /** + * Reads the drive encoder to get the velocity of the module wheel in inches. + * + * @return the velocity of the module in inches per second + */ + protected abstract double readVelocity(); + /** * Sets the target angle. * @@ -104,23 +120,25 @@ public final double getCurrentDistance() { } /** - * Gets the current velocity of the wheel. + * Gets the amount of current being drawn by the drive motor. * - * TODO: Allow implementors to specify the current velocity without overriding this method. - * @return the velocity of the module. + * @return the amount of current being drawn by the drive motor. */ - public double getCurrentVelocity() { - return 0; + public final double getDriveCurrent() { + synchronized (sensorMutex) { + return currentDriveCurrent; + } } /** - * Gets the amount of current being drawn by the drive motor. + * Gets the current velocity of the wheel. * - * TODO: Allow implementors to specify current draw without overriding this method. - * @return the amount of current being drawn by the drive motor. + * @return the velocity of the module. */ - public double getDriveCurrent() { - return 0; + public final double getCurrentVelocity() { + synchronized (sensorMutex) { + return currentVelocity; + } } public Vector2 getTargetVelocity() { @@ -201,6 +219,8 @@ public void updateSensors() { synchronized (sensorMutex) { currentAngle = readAngle(); currentDistance = readDistance(); + currentDriveCurrent = readDriveCurrent(); + currentVelocity = readVelocity(); } } diff --git a/src/test/java/org/frcteam2910/common/drivers/MockSwerveModule.java b/src/test/java/org/frcteam2910/common/drivers/MockSwerveModule.java index 59fe1af1..bb60f9f5 100644 --- a/src/test/java/org/frcteam2910/common/drivers/MockSwerveModule.java +++ b/src/test/java/org/frcteam2910/common/drivers/MockSwerveModule.java @@ -8,6 +8,8 @@ public final class MockSwerveModule extends SwerveModule { private double angle; private double distance; + private double driveCurrent; + private double velocity; private double targetAngle; private double driveOutput; @@ -33,6 +35,24 @@ public void writeDistance(double distance) { this.distance = distance; } + @Override + public double readDriveCurrent() { + return driveCurrent; + } + + public void writeDriveCurrent(double driveCurrent) { + this.driveCurrent = driveCurrent; + } + + @Override + public double readVelocity() { + return velocity; + } + + public void writeVelocity(double velocity) { + this.velocity = velocity; + } + @Override protected void setTargetAngle(double angle) { this.targetAngle = angle; From cc190d444d6947f83582c3c111b472eea2773abf Mon Sep 17 00:00:00 2001 From: Jacob Bublitz Date: Mon, 13 Jan 2020 17:14:49 -0800 Subject: [PATCH 2/3] Fix builder after rebase --- .../robot/drivers/Mk2SwerveModuleBuilder.java | 35 ++----------------- 1 file changed, 3 insertions(+), 32 deletions(-) diff --git a/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModuleBuilder.java b/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModuleBuilder.java index 4618726b..34073c6f 100644 --- a/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModuleBuilder.java +++ b/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk2SwerveModuleBuilder.java @@ -370,10 +370,6 @@ public SwerveModule build() { } private final class SwerveModuleImpl extends SwerveModule { - private final Object sensorLock = new Object(); - private double currentDraw = 0.0; - private double velocity = 0.0; - public SwerveModuleImpl() { super(modulePosition); @@ -387,7 +383,8 @@ protected double readAngle() { return angleSupplier.getAsDouble(); } - protected double readCurrentDraw() { + @Override + protected double readDriveCurrent() { if (currentDrawSupplier == null) { return Double.NaN; } @@ -404,6 +401,7 @@ protected double readDistance() { return distanceSupplier.getAsDouble(); } + @Override protected double readVelocity() { if (velocitySupplier == null) { return Double.NaN; @@ -412,20 +410,6 @@ protected double readVelocity() { return velocitySupplier.getAsDouble(); } - @Override - public double getCurrentVelocity() { - synchronized (sensorLock) { - return velocity; - } - } - - @Override - public double getDriveCurrent() { - synchronized (sensorLock) { - return currentDraw; - } - } - @Override protected void setTargetAngle(double angle) { targetAngleConsumer.accept(angle); @@ -436,19 +420,6 @@ protected void setDriveOutput(double output) { driveOutputConsumer.accept(output); } - @Override - public void updateSensors() { - super.updateSensors(); - - double newCurrentDraw = readCurrentDraw(); - double newVelocity = readVelocity(); - - synchronized (sensorLock) { - currentDraw = newCurrentDraw; - velocity = newVelocity; - } - } - @Override public void updateState(double dt) { super.updateState(dt); From af1647a89e663d8b32984634ed9bd25da3c473bd Mon Sep 17 00:00:00 2001 From: Jacob Bublitz Date: Mon, 13 Jan 2020 17:15:02 -0800 Subject: [PATCH 3/3] Stop using deprecated API --- .../org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java b/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java index a1169812..3f683f54 100644 --- a/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java +++ b/robot/src/main/java/org/frcteam2910/common/robot/drivers/Mk1SwerveModule.java @@ -120,7 +120,7 @@ public double readDistance() { @Override protected double readDriveCurrent() { - return driveMotor.getOutputCurrent(); + return driveMotor.getSupplyCurrent(); } @Override