From 02ddcd2c298294b29ebe37aa874fd682796132a8 Mon Sep 17 00:00:00 2001 From: Randall Hauch Date: Sun, 15 Nov 2015 20:57:31 -0600 Subject: [PATCH 1/6] Added a TalonController to work with hardware-based PID control on Talon SRX --- .../src/org/strongback/mock/Mock.java | 23 +- .../org/strongback/mock/MockController.java | 102 ++++++ .../strongback/mock/MockPIDController.java | 100 ++++++ .../src/org/strongback/mock/MockTalonSRX.java | 227 +++++++++++- .../org/strongback/components/TalonSRX.java | 227 +++++++++++- .../org/strongback/control/Controller.java | 115 +++--- .../org/strongback/control/PIDController.java | 12 +- .../control/SoftwarePIDController.java | 31 +- .../strongback/control/TalonController.java | 313 ++++++++++++++++ .../src/org/strongback/hardware/Hardware.java | 33 ++ .../hardware/HardwarePneumaticsModule.java | 4 +- .../hardware/HardwareTalonController.java | 334 ++++++++++++++++++ .../strongback/hardware/HardwareTalonSRX.java | 197 ++++++++++- 13 files changed, 1625 insertions(+), 93 deletions(-) create mode 100644 strongback-testing/src/org/strongback/mock/MockController.java create mode 100644 strongback-testing/src/org/strongback/mock/MockPIDController.java create mode 100644 strongback/src/org/strongback/control/TalonController.java create mode 100644 strongback/src/org/strongback/hardware/HardwareTalonController.java diff --git a/strongback-testing/src/org/strongback/mock/Mock.java b/strongback-testing/src/org/strongback/mock/Mock.java index b582e1e..49d6649 100644 --- a/strongback-testing/src/org/strongback/mock/Mock.java +++ b/strongback-testing/src/org/strongback/mock/Mock.java @@ -17,6 +17,9 @@ package org.strongback.mock; import org.strongback.components.Fuse; +import org.strongback.components.TalonSRX; +import org.strongback.control.Controller; +import org.strongback.control.PIDController; /** * Factory for mock components. @@ -230,7 +233,7 @@ public static MockMotor runningMotor(double speed) { } /** - * Create a stopped mock Talon SRX motor. + * Create a stopped mock {@link TalonSRX} motor. * * @return the mock TalonSRX motor; never null */ @@ -239,7 +242,7 @@ public static MockTalonSRX stoppedTalonSRX() { } /** - * Create a running mock Talon SRX motor. + * Create a running mock {@link TalonSRX} motor. * * @param speed the initial speed * @return the mock TalonSRX motor; never null @@ -247,4 +250,20 @@ public static MockTalonSRX stoppedTalonSRX() { public static MockTalonSRX runningTalonSRX(double speed) { return new MockTalonSRX(speed); } + + /** + * Create a mock {@link Controller}. + * @return the mock controller; never null + */ + public static MockController controller() { + return new MockController(); + } + + /** + * Create a mock {@link PIDController}. + * @return the mock controller; never null + */ + public static MockPIDController pidController() { + return new MockPIDController(); + } } diff --git a/strongback-testing/src/org/strongback/mock/MockController.java b/strongback-testing/src/org/strongback/mock/MockController.java new file mode 100644 index 0000000..7c54343 --- /dev/null +++ b/strongback-testing/src/org/strongback/mock/MockController.java @@ -0,0 +1,102 @@ +/* + * Strongback + * Copyright 2015, Strongback and individual contributors by the @authors tag. + * See the COPYRIGHT.txt in the distribution for a full listing of individual + * contributors. + * + * Licensed under the MIT License; you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * http://opensource.org/licenses/MIT + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.strongback.mock; + +import org.strongback.Executable; +import org.strongback.control.Controller; + +/** + * A {@link Controller} implementation that tests can use to manually {@link #setValue(double) set the value}. + */ +public class MockController implements Controller { + + private boolean enabled = true; + private double setpoint = 0.0d; + private double tolerance = 0.0d; + private double value = 0.0d; + private final Executable executable = (time)->computeOutput(); + + @Override + public boolean isEnabled() { + return enabled; + } + + @Override + public Controller enable() { + enabled = true; + return this; + } + + @Override + public Controller disable() { + enabled = false; + return this; + } + + @Override + public double getTarget() { + return setpoint; + } + + @Override + public MockController withTarget(double target) { + this.setpoint = target; + return this; + } + + @Override + public double getTolerance() { + return tolerance; + } + + @Override + public MockController withTolerance(double tolerance) { + this.tolerance = tolerance; + return this; + } + + @Override + public boolean computeOutput() { + return isWithinTolerance(); + } + + @Override + public MockController reset() { + return this; + } + + @Override + public boolean hasExecutable() { + return true; + } + + @Override + public Executable executable() { + return executable; + } + + @Override + public double getValue() { + return value; + } + + public MockController setValue( double value ) { + this.value = value; + return this; + } + +} diff --git a/strongback-testing/src/org/strongback/mock/MockPIDController.java b/strongback-testing/src/org/strongback/mock/MockPIDController.java new file mode 100644 index 0000000..389d231 --- /dev/null +++ b/strongback-testing/src/org/strongback/mock/MockPIDController.java @@ -0,0 +1,100 @@ +/* + * Strongback + * Copyright 2015, Strongback and individual contributors by the @authors tag. + * See the COPYRIGHT.txt in the distribution for a full listing of individual + * contributors. + * + * Licensed under the MIT License; you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * http://opensource.org/licenses/MIT + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.strongback.mock; + +import java.util.Collections; +import java.util.Map; +import java.util.Set; +import java.util.concurrent.ConcurrentHashMap; + +import org.strongback.control.Controller; +import org.strongback.control.PIDController; + +/** + * A {@link Controller} implementation that tests can use to manually {@link #setValue(double) set the value}. + */ +public class MockPIDController extends MockController implements PIDController { + + private volatile int currentProfile = PIDController.DEFAULT_PROFILE; + private final Map profiles = new ConcurrentHashMap<>(); + + public MockPIDController() { + withGains(0.0,0.0,0.0); + } + + @Override + public Set getProfiles() { + return Collections.unmodifiableSet(profiles.keySet()); + } + + @Override + public synchronized MockPIDController withProfile(int profile, double p, double i, double d, double feedForward) { + profiles.put(profile, new Gains(p,i,d,feedForward)); + return this; + } + + @Override + public synchronized int getCurrentProfile() { + return currentProfile; + } + + @Override + public synchronized Gains getGainsForCurrentProfile() { + return profiles.get(currentProfile); + } + + @Override + public synchronized MockPIDController useProfile(int profile) { + if ( !profiles.containsKey(profile) ) throw new IllegalArgumentException("Invalid profile"); + currentProfile = profile; + return this; + } + + protected final class Gains implements PIDController.Gains { + protected final double p; + protected final double i; + protected final double d; + protected final double feedForward; + + protected Gains(double p, double i, double d, double feedForward) { + this.p = p; + this.i = i; + this.d = d; + this.feedForward = feedForward; + }; + + @Override + public double getP() { + return p; + } + + @Override + public double getI() { + return i; + } + + @Override + public double getD() { + return d; + } + + @Override + public double getFeedForward() { + return feedForward; + } + } +} diff --git a/strongback-testing/src/org/strongback/mock/MockTalonSRX.java b/strongback-testing/src/org/strongback/mock/MockTalonSRX.java index 1b51a14..1753c90 100644 --- a/strongback-testing/src/org/strongback/mock/MockTalonSRX.java +++ b/strongback-testing/src/org/strongback/mock/MockTalonSRX.java @@ -16,18 +16,26 @@ package org.strongback.mock; +import org.strongback.components.Fuse; +import org.strongback.components.Switch; import org.strongback.components.TalonSRX; +import org.strongback.components.TemperatureSensor; +import org.strongback.components.VoltageSensor; -/** - * @author Randall Hauch - * - */ public class MockTalonSRX extends MockMotor implements TalonSRX { private final MockAngleSensor encoder = new MockAngleSensor(); private final MockCurrentSensor current = new MockCurrentSensor(); + private final MockVoltageSensor voltage = new MockVoltageSensor(); + private final MockVoltageSensor busVoltage = new MockVoltageSensor(); + private final MockTemperatureSensor temperature = new MockTemperatureSensor(); private final MockSwitch forwardLimitSwitch = new MockSwitch(); private final MockSwitch reverseLimitSwitch = new MockSwitch(); + private final StickyFaults stickyFaults = new StickyFaults(); + private final MockFaults instantaneousFaults = new MockFaults(stickyFaults); + private boolean safetyEnabled = true; + private double expiration = 0.1d; + private boolean alive = true; protected MockTalonSRX(double speed) { super(speed); @@ -49,6 +57,21 @@ public MockCurrentSensor getCurrentSensor() { return current; } + @Override + public VoltageSensor getVoltageSensor() { + return voltage; + } + + @Override + public VoltageSensor getBusVoltageSensor() { + return busVoltage; + } + + @Override + public TemperatureSensor getTemperatureSensor() { + return temperature; + } + @Override public MockSwitch getForwardLimitSwitch() { return forwardLimitSwitch; @@ -59,4 +82,200 @@ public MockSwitch getReverseLimitSwitch() { return reverseLimitSwitch; } + @Override + public TalonSRX setForwardSoftLimit(int forwardLimit) { + return this; + } + + @Override + public TalonSRX enableForwardSoftLimit(boolean enable) { + return this; + } + + @Override + public TalonSRX setReverseSoftLimit(int reverseLimit) { + return this; + } + + @Override + public TalonSRX enableReverseSoftLimit(boolean enable) { + return this; + } + + @Override + public TalonSRX enableLimitSwitch(boolean forward, boolean reverse) { + return this; + } + + @Override + public TalonSRX setForwardLimitSwitchNormallyOpen(boolean normallyOpen) { + return this; + } + + @Override + public TalonSRX setReverseLimitSwitchNormallyOpen(boolean normallyOpen) { + return this; + } + + @Override + public TalonSRX enableBrakeMode(boolean brake) { + return this; + } + + @Override + public MockFaults faults() { + return instantaneousFaults; + } + + @Override + public StickyFaults stickyFaults() { + return stickyFaults; + } + + @Override + public MockTalonSRX clearStickyFaults() { + stickyFaults.reset(); + return this; + } + + @Override + public long getFirmwareVersion() { + return 0; + } + + @Override + public boolean isSafetyEnabled() { + return safetyEnabled; + } + + @Override + public TalonSRX setSafetyEnabled(boolean enabled) { + safetyEnabled = enabled; + return this; + } + + @Override + public TalonSRX setExpiration(double timeout) { + expiration = timeout; + return this; + } + + @Override + public double getExpiration() { + return expiration; + } + + @Override + public boolean isAlive() { + return alive; + } + + public MockTalonSRX setAlive( boolean alive ) { + this.alive = alive; + if ( !alive ) stop(); + return this; + } + + private void triggerFault( MockSwitch stickySwitch ) { + // Any fault should always stop the motor ... + stickySwitch.setTriggered(); + stop(); + } + + public class MockFaults implements Faults { + private final Fuse overTemperature; + private final Fuse underVoltage; + private final Fuse forwardSoftLimit; + private final Fuse reverseSoftLimit; + private final Fuse forwardLimitSwitch; + private final Fuse reverseLimitSwitch; + private final Fuse hardwareFailure; + protected MockFaults( StickyFaults sticky ) { + // These should trip the sticky faults and then immediately reset ... + overTemperature = Fuse.instantaneous(()->triggerFault(sticky.overTemperature)); + underVoltage = Fuse.instantaneous(()->triggerFault(sticky.underVoltage)); + forwardSoftLimit = Fuse.instantaneous(()->triggerFault(sticky.forwardSoftLimit)); + reverseSoftLimit = Fuse.instantaneous(()->triggerFault(sticky.reverseSoftLimit)); + forwardLimitSwitch = Fuse.instantaneous(()->triggerFault(sticky.forwardLimitSwitch)); + reverseLimitSwitch = Fuse.instantaneous(()->triggerFault(sticky.reverseLimitSwitch)); + hardwareFailure = Fuse.instantaneous(()->triggerFault(sticky.hardwareFailure)); + } + @Override + public Fuse overTemperature() { + return overTemperature; + } + @Override + public Fuse underVoltage() { + return underVoltage; + } + @Override + public Fuse forwardSoftLimit() { + return forwardSoftLimit; + } + @Override + public Fuse reverseSoftLimit() { + return reverseSoftLimit; + } + @Override + public Fuse forwardLimitSwitch() { + return forwardLimitSwitch; + } + @Override + public Fuse reverseLimitSwitch() { + return reverseLimitSwitch; + } + @Override + public Fuse hardwareFailure() { + return hardwareFailure; + } + } + + protected static class StickyFaults implements Faults { + protected final MockSwitch overTemperature = new MockSwitch().setNotTriggered(); + protected final MockSwitch underVoltage = new MockSwitch().setNotTriggered(); + protected final MockSwitch forwardSoftLimit = new MockSwitch().setNotTriggered(); + protected final MockSwitch reverseSoftLimit = new MockSwitch().setNotTriggered(); + protected final MockSwitch forwardLimitSwitch = new MockSwitch().setNotTriggered(); + protected final MockSwitch reverseLimitSwitch = new MockSwitch().setNotTriggered(); + protected final MockSwitch hardwareFailure = new MockSwitch().setNotTriggered(); + + @Override + public Switch overTemperature() { + return overTemperature; + } + @Override + public Switch underVoltage() { + return underVoltage; + } + @Override + public Switch forwardSoftLimit() { + return forwardSoftLimit; + } + @Override + public Switch reverseSoftLimit() { + return reverseSoftLimit; + } + @Override + public Switch forwardLimitSwitch() { + return forwardLimitSwitch; + } + @Override + public Switch reverseLimitSwitch() { + return reverseLimitSwitch; + } + @Override + public Switch hardwareFailure() { + return hardwareFailure; + } + protected void reset() { + overTemperature.setNotTriggered(); + underVoltage.setNotTriggered(); + forwardSoftLimit.setNotTriggered(); + reverseSoftLimit.setNotTriggered(); + forwardLimitSwitch.setNotTriggered(); + reverseLimitSwitch.setNotTriggered(); + hardwareFailure.setNotTriggered(); + } + } + } diff --git a/strongback/src/org/strongback/components/TalonSRX.java b/strongback/src/org/strongback/components/TalonSRX.java index 5e47a6e..cd8ab9a 100644 --- a/strongback/src/org/strongback/components/TalonSRX.java +++ b/strongback/src/org/strongback/components/TalonSRX.java @@ -34,10 +34,233 @@ public interface TalonSRX extends LimitedMotor { public AngleSensor getAngleSensor(); /** - * Get the Talon SRX's current sensor. + * Get the Talon SRX's output current sensor. * - * @return the current sensor; never null + * @return the output current sensor; never null */ public CurrentSensor getCurrentSensor(); + /** + * Get the Talon SRX's output voltage sensor. + * + * @return the output voltage sensor; never null + */ + public VoltageSensor getVoltageSensor(); + + /** + * Get the Talon SRX's bus voltage. + * + * @return the bus voltage sensor; never null + */ + public VoltageSensor getBusVoltageSensor(); + + /** + * Get the Talon SRX's temperature sensor. + * + * @return the temperature sensor; never null + */ + public TemperatureSensor getTemperatureSensor(); + + /** + * Set the soft limit for forward motion, which will disable the motor when the sensor is out of range. + * + * @param forwardLimit the sensor limit beyond which the motor's forward direction should be halted. + * @return this object so that methods can be chained; never null + */ + public TalonSRX setForwardSoftLimit(int forwardLimit); + + /** + * Enable the forward soft limit. + * + * @param enable true if the limit is to be enabled, or false otherwise + * @return this object so that methods can be chained; never null + */ + public TalonSRX enableForwardSoftLimit(boolean enable); + + /** + * Set the soft limit for reverse motion, which will disable the motor when the sensor is out of range. + * + * @param reverseLimit the sensor limit beyond which the motor's reverse direction should be halted. + * @return this object so that methods can be chained; never null + */ + public TalonSRX setReverseSoftLimit(int reverseLimit); + + /** + * Enable the reverse soft limit. + * + * @param enable true if the limit is to be enabled, or false otherwise + * @return this object so that methods can be chained; never null + */ + public TalonSRX enableReverseSoftLimit(boolean enable); + + /** + * Enable the forward and reverse limit switches. + * + * @param forward true if the forward limit is to be enabled, or false otherwise + * @param reverse true if the reverse limit is to be enabled, or false otherwise + * @return this object so that methods can be chained; never null + */ + public TalonSRX enableLimitSwitch(boolean forward, boolean reverse); + + /** + * Configure the forward limit switch to be normally open or normally closed. Talon will disable momentarily if the Talon's + * current setting is dissimilar to the caller's requested setting. + * + * Since Talon saves setting to flash this should only affect a given Talon initially during robot install. + * + * @param normallyOpen true for normally open, or false for normally closed. + * @return this object so that methods can be chained; never null + */ + public TalonSRX setForwardLimitSwitchNormallyOpen(boolean normallyOpen); + + /** + * Configure the reverse limit switch to be normally open or normally closed. Talon will disable momentarily if the Talon's + * current setting is dissimilar to the caller's requested setting. + * + * Since Talon saves setting to flash this should only affect a given Talon initially during robot install. + * + * @param normallyOpen true for normally open, or false for normally closed. + * @return this object so that methods can be chained; never null + */ + public TalonSRX setReverseLimitSwitchNormallyOpen(boolean normallyOpen); + + /** + * Enable the brake mode. + * + * @param brake true if the brake mode is to be enabled, or false otherwise + * @return this object so that methods can be chained; never null + */ + public TalonSRX enableBrakeMode(boolean brake); + + /** + * Get the faults currently associated with the Talon controller. These state of these faults may change at any time based + * upon the state of the module, and so they cannot be manually cleared. + * + * @return the current faults; never null + * @see #stickyFaults() + */ + public Faults faults(); + + /** + * Get the sticky faults associated with the Talon controller. Once these faults are triggered, they are only reset with + * #clearStickyFaults + * + * @return the sticky faults; never null + * @see #faults() + * @see #clearStickyFaults() + */ + public Faults stickyFaults(); + + /** + * Clear all {@link #stickyFaults() sticky faults} that may have been triggered. + * + * @return this instance so that methods can be chained; never null + */ + public TalonSRX clearStickyFaults(); + + /** + * Get the firmware version. + * + * @return the version of the firmware running on the Talon + */ + public long getFirmwareVersion(); + + /** + * Determine whether this motor controller's safety mode is enabled. + * + * @return true if the motor will be automatically disabled if it is not used within the expiration time, or + * false otherwise + */ + public boolean isSafetyEnabled(); + + /** + * Set whether this motor controller's safety mode is enabled. + * + * @param enabled true if the motor will be automatically disabled if it is not used within the expiration + * time, or false otherwise + * @return this instance so that methods can be chained; never null + */ + public TalonSRX setSafetyEnabled(boolean enabled); + + /** + * Get the motor safety expiration time in milliseconds. When {@link #isSafetyEnabled() safety is enabled}, then the motor + * will be automatically disabled if it is not used within the expiration time, which is typically 100ms. This can be + * adjusted to a larger value when debugging the robot. + * + * @return the safety expiration time in milliseconds + */ + public double getExpiration(); + + /** + * Set the motor safety expiration time in milliseconds. When {@link #isSafetyEnabled() safety is enabled}, then the motor + * will be automatically disabled if it is not used within the expiration time, which is typically 100ms. This can be + * adjusted to a larger value when debugging the robot. + * + * @param timeout the safety expiration time in milliseconds + * @return this instance so that methods can be chained; never null + */ + public TalonSRX setExpiration(double timeout); + + /** + * Determine if this motor controller is alive or has been disabled because it has not been used within the + * {@link #getExpiration() expiration}. + * + * @return true if the controller is alive, or false if it has been disabled after expiring. + */ + public boolean isAlive(); + + /** + * The set of possible faults that this module can trigger. + */ + public static interface Faults { + + /** + * The switch that is {@link Switch#isTriggered() triggered} when the Talon's over-temperature fault is tripped. + * + * @return the switch; never null + */ + Switch overTemperature(); + + /** + * The switch that is {@link Switch#isTriggered() triggered} when the voltage is too low. + * + * @return the switch; never null + */ + Switch underVoltage(); + + /** + * The switch that is {@link Switch#isTriggered() triggered} when the forward limit switch fault is tripped. + * + * @return the switch; never null + */ + Switch forwardLimitSwitch(); + + /** + * The switch that is {@link Switch#isTriggered() triggered} when the reverse limit switch fault is tripped. + * + * @return the switch; never null + */ + Switch reverseLimitSwitch(); + + /** + * The switch that is {@link Switch#isTriggered() triggered} when the forward soft limit is tripped. + * + * @return the switch; never null + */ + Switch forwardSoftLimit(); + + /** + * The switch that is {@link Switch#isTriggered() triggered} when the reverse soft limit is tripped. + * + * @return the switch; never null + */ + Switch reverseSoftLimit(); + + /** + * The switch that is {@link Switch#isTriggered() triggered} when a hardware fault occurs. + * + * @return the switch; never null + */ + Switch hardwareFailure(); + } } \ No newline at end of file diff --git a/strongback/src/org/strongback/control/Controller.java b/strongback/src/org/strongback/control/Controller.java index 439db6f..2689872 100644 --- a/strongback/src/org/strongback/control/Controller.java +++ b/strongback/src/org/strongback/control/Controller.java @@ -16,7 +16,9 @@ package org.strongback.control; -import org.strongback.command.UnmanagedControllerCommand; +import org.strongback.Executable; +import org.strongback.Executor; +import org.strongback.Strongback; import org.strongback.command.Requirable; /** @@ -25,39 +27,31 @@ * * @author Randall Hauch * @see PIDController - * @see UnmanagedControllerCommand */ public interface Controller extends Requirable { /** - * Calculate the next controller output. This usually involves reading one or more inputs and computing the output based - * upon a model of the system. - * - * @return true if the execution completed because it resulted in a value that is within the tolerance of the - * setpoint, or false if this controller is not {@link #isEnabled() enabled} or has not yet reached the - * setpoint given the tolerance - */ - public boolean computeOutput(); - - /** - * Determines whether the supplied value is within the {@link #getTolerance() tolerance} of the {@link #getTarget() target}. + * Determine if this controller is {@link #enable() enabled}. * - * @param value the proposed value - * @return true if the proposed value is within the tolerance of the target, or false otherwise + * @return true if enabled, or false otherwise */ - public default boolean checkTolerance(double value) { - return Math.abs(value) <= (getTarget() - getTolerance()); - } + public boolean isEnabled(); /** - * Determines whether the input to the controller is currently within the {@link #getTolerance() tolerance} of the - * {@link #getTarget() target}. - * - * @return true if the current value is within the tolerance of the target, or false otherwise - */ - public default boolean isWithinTolerance() { - return checkTolerance(getValue()); - } + * Enable this controller so that it does read inputs, compute errors, and generate outputs when {@link #computeOutput()} is + * called. + * + * @return this object so that methods can be chained; never null + */ + public Controller enable(); + + /** + * Disable this controller to not read inputs, compute errors, and generate outputs when {@link #computeOutput()} + * is called. + * + * @return this object so that methods can be chained; never null + */ + public Controller disable(); /** * Get the current measured value for this controller. @@ -65,6 +59,13 @@ public default boolean isWithinTolerance() { */ public double getValue(); + /** + * Get the target value for this controller. + * @return the target value + * @see #withTarget(double) + */ + public double getTarget(); + /** * Sets the target value for this controller. * @@ -74,13 +75,6 @@ public default boolean isWithinTolerance() { */ public Controller withTarget(double target); - /** - * Get the target value for this controller. - * @return the target value - * @see #withTarget(double) - */ - public double getTarget(); - /** * Sets the absolute tolerance for this controller. * @@ -98,33 +92,62 @@ public default boolean isWithinTolerance() { public double getTolerance(); /** - * Reset any error values stored from previous {@link #computeOutput() executions}. + * Determines whether the supplied value is within the {@link #getTolerance() tolerance} of the {@link #getTarget() target}. * - * @return this object so that methods can be chained; never null + * @param value the proposed value + * @return true if the proposed value is within the tolerance of the target, or false otherwise */ - public Controller reset(); + public default boolean checkTolerance(double value) { + return Math.abs(value) <= (getTarget() - getTolerance()); + } /** - * Determine if this controller is {@link #enable() enabled}. + * Determines whether the input to the controller is currently within the {@link #getTolerance() tolerance} of the + * {@link #getTarget() target}. * - * @return true if enabled, or false otherwise + * @return true if the current value is within the tolerance of the target, or false otherwise */ - public boolean isEnabled(); + public default boolean isWithinTolerance() { + return checkTolerance(getValue()); + } /** - * Enable this controller so that it does read inputs, compute errors, and generate outputs when {@link #computeOutput()} is - * called. + * Calculate the next controller output. This usually involves reading one or more inputs and computing the output based + * upon a model of the system. * - * @return this object so that methods can be chained; never null + * @return true if the execution completed because it resulted in a value that is within the tolerance of the + * setpoint, or false if this controller is not {@link #isEnabled() enabled} or has not yet reached the + * setpoint given the tolerance */ - public Controller enable(); + public boolean computeOutput(); /** - * Disable this controller to not read inputs, compute errors, and generate outputs when {@link #computeOutput()} - * is called. + * Reset any error values stored from previous {@link #computeOutput() executions}. * * @return this object so that methods can be chained; never null */ - public Controller disable(); + public Controller reset(); + + /** + * Return whether the {@link Executable} instance returned by {@link #executable()} does anything useful. Software-based + * controllers may often return true, whereas many hardware-based controllers may return false. + * @return true if the {@link #executable()} does useful work and enables the controller to operate + * continually and automatically, or false if the {@link #executable()} does nothing. + */ + public default boolean hasExecutable() { + return true; + } + + /** + * Get the {@link Executable} instance that will continuously {@link #computeOutput() execute} this controller to read + * inputs from the source and generate outputs to reach the {@link #getTarget() target}. + *

+ * If this is used, then this same controller should never be used with commands. This is not checked, so robot + * programs are responsible for ensuring this does not happen. + * + * @return the {@link Executable} object that can be registered with an {@link Executor} (typically Strongback's + * {@link Strongback#executor() central executor}); never null and always the same instance for this controller + */ + public Executable executable(); } diff --git a/strongback/src/org/strongback/control/PIDController.java b/strongback/src/org/strongback/control/PIDController.java index 6a43889..59ff1e4 100644 --- a/strongback/src/org/strongback/control/PIDController.java +++ b/strongback/src/org/strongback/control/PIDController.java @@ -49,7 +49,9 @@ public interface PIDController extends Controller { * @param d the differential gain * @return this object so that methods can be chained; never null */ - public PIDController withGains(double p, double i, double d); + public default PIDController withGains(double p, double i, double d) { + return withGains(p, i, d, 0.0); + } /** * Set the control gains on this controller's current profile. @@ -60,7 +62,9 @@ public interface PIDController extends Controller { * @param feedForward the feed-forward gain * @return this object so that methods can be chained; never null */ - public PIDController withGains(double p, double i, double d, double feedForward); + public default PIDController withGains(double p, double i, double d, double feedForward) { + return withProfile(getCurrentProfile(), p, i, d, 0.0); + } /** * Get the set of profile numbers. @@ -82,7 +86,9 @@ public interface PIDController extends Controller { * @return this object so that methods can be chained; never null * @throws IllegalArgumentException if the profile name is null */ - public PIDController withProfile(int profile, double p, double i, double d); + public default PIDController withProfile(int profile, double p, double i, double d) { + return withProfile(profile,p,i,d,0.0); + } /** * Set the control gains for the specified profile, which may or may not be the current profile. Profile numbers start from diff --git a/strongback/src/org/strongback/control/SoftwarePIDController.java b/strongback/src/org/strongback/control/SoftwarePIDController.java index 69f3848..330e9a8 100644 --- a/strongback/src/org/strongback/control/SoftwarePIDController.java +++ b/strongback/src/org/strongback/control/SoftwarePIDController.java @@ -146,6 +146,7 @@ public SoftwarePIDController(DoubleSupplier source, DoubleConsumer output) { * @return the {@link Executable} object that can be registered with an {@link Executor} (typically Strongback's * {@link Strongback#executor() central executor}); never null and always the same instance for this controller */ + @Override public Executable executable() { return executable; } @@ -224,9 +225,8 @@ public SoftwarePIDController withProfile(int profile, double p, double i, double @Override public SoftwarePIDController withProfile(int profile, double p, double i, double d, double feedForward) { synchronized (this) { - Gains newGains = this.gainsByProfile.compute(profile, (key, gains) -> { - return new Gains(p, i, d, feedForward); - }); + Gains newGains = new Gains(p, i, d, feedForward); + this.gainsByProfile.put(profile, newGains); if (profile == this.currentProfile) { this.gains = newGains; onTable(table -> { @@ -524,10 +524,7 @@ protected double currentFeedForward() { return gains.feedForward; } - /** - * A set of gains. - */ - private static final class Gains implements PIDController.Gains { + protected final class Gains implements PIDController.Gains { protected final double p; protected final double i; protected final double d; @@ -540,41 +537,21 @@ protected Gains(double p, double i, double d, double feedForward) { this.feedForward = feedForward; }; - /** - * Get the proportional gain. - * - * @return the proportional gain. - */ @Override public double getP() { return p; } - /** - * Get the integral gain. - * - * @return the integral gain. - */ @Override public double getI() { return i; } - /** - * Get the differential gain. - * - * @return the differential gain. - */ @Override public double getD() { return d; } - /** - * Get the feed forward gain. - * - * @return the feed forward gain. - */ @Override public double getFeedForward() { return feedForward; diff --git a/strongback/src/org/strongback/control/TalonController.java b/strongback/src/org/strongback/control/TalonController.java new file mode 100644 index 0000000..2b75e2a --- /dev/null +++ b/strongback/src/org/strongback/control/TalonController.java @@ -0,0 +1,313 @@ +/* + * Strongback + * Copyright 2015, Strongback and individual contributors by the @authors tag. + * See the COPYRIGHT.txt in the distribution for a full listing of individual + * contributors. + * + * Licensed under the MIT License; you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * http://opensource.org/licenses/MIT + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.strongback.control; + +import org.strongback.components.TalonSRX; + +/** + * The Talon SRX Proportional Integral Differential (PID) controller with optional support for feed forward. + */ +public interface TalonController extends PIDController, TalonSRX { + + /** + * A set of gains. + */ + public static interface Gains extends PIDController.Gains { + + /** + * Get the IZone value for the current profile of this controller. When IZone is used, the controller will automatically + * clear the integral accumulated error if the closed loop error is outside the IZone. + * + * @return the IZone value + */ + public double getIzone(); + + /** + * Get the closed loop ramp rate for the current profile of this controller. + * + * @return the feed forward gain + */ + public double getCloseLoopRampRate(); + } + + /** + * The type of control input used by this Talon controller. + */ + public static enum ControlMode { + /** + * Directly control the motor by setting the speed as percent voltage, from -1.0 to 1.0 with 0.0 as stopped. + */ + PERCENT_VBUS(0), + /** + * Control the motor by directly setting the desired position (in ticks or an analog value) for the motor's encoder. + */ + POSITION(1), + /** + * Control the motor's speed in position change every 10 milliseconds. + */ + SPEED(2), + /** + * Control the motor by directly setting the current to be sent to the motor. + */ + CURRENT(3), + /** + * Control the motor by directly setting the voltage to be sent to the motor. + */ + VOLTAGE(4), + /** + * Control the motor by following another Talon SRX device. + */ + FOLLOWER(5); + + private final int value; + + public static ControlMode valueOf(int value) { + for (ControlMode mode : values()) { + if (mode.value == value) { + return mode; + } + } + return null; + } + + private ControlMode(int value) { + this.value = value; + } + + public int value() { + return this.value; + } + } + + /** + * The type of control sensor used by this Talon controller. + */ + public enum FeedbackDevice { + /** + * Use Quadrature Encoder. + */ + QUADRATURE_ENCODER(0), + /** + * Analog potentiometer, 0-3.3V + */ + ANALOG_POTENTIOMETER(2), + /** + * Analog encoder, 0-3.3V + */ + ANALOG_ENCODER(3), + /** + * Encoder that increment position per rising edge on Quadrature-A. + */ + ENCODER_RISING(4), + /** + * Encoder that increment position per falling edge on Quadrature-A. + */ + ENCODER_FALLING(5); + + public int value; + + public static FeedbackDevice valueOf(int value) { + for (FeedbackDevice mode : values()) { + if (mode.value == value) { + return mode; + } + } + return null; + } + + private FeedbackDevice(int value) { + this.value = value; + } + + public int value() { + return this.value; + } + } + + /** + * Types of status frame rates. + */ + public enum StatusFrameRate { + GENERAL(0), FEEDBACK(1), QUADRATURE_ENCODER(2), ANALOG_TEMP_VBAT(3); + public int value; + + public static StatusFrameRate valueOf(int value) { + for (StatusFrameRate mode : values()) { + if (mode.value == value) { + return mode; + } + } + return null; + } + + private StatusFrameRate(int value) { + this.value = value; + } + + public int value() { + return this.value; + } + } + + /** + * Get the CAN device ID. + * + * @return the device ID. + */ + public int getDeviceID(); + + /** + * Get this controller's current control mode. + * + * @return the control mode; never null + */ + public ControlMode getControlMode(); + + /** + * Set the control mode for this controller. + * + * @param mode the control mode; may not be null + * @return this object so that methods can be chained; never null + */ + public TalonController setControlMode(ControlMode mode); + + /** + * Set the feedback device for this controller. + * + * @param device the feedback device; may not be null + * @return this object so that methods can be chained; never null + */ + public TalonController setFeedbackDevice(FeedbackDevice device); + + /** + * Set the status frame rate for this controller. + * + * @param frameRate the status frame rate; may not be null + * @param periodMillis frame rate period in milliseconds + * @return this object so that methods can be chained; never null + */ + public TalonController setStatusFrameRate(StatusFrameRate frameRate, int periodMillis); + + /** + * Flips the sign (multiplies by negative one) the sensor values going into the talon. + * + * This only affects position and velocity closed loop control. Allows for situations where you may have a sensor flipped + * and going in the wrong direction. + * + * @param flip true if sensor input should be flipped, or false if not. + * @return this object so that methods can be chained; never null + */ + public TalonController reverseSensor(boolean flip); + + /** + * Flips the sign (multiplies by negative one) the throttle values going into the motor on the talon in closed loop modes. + * + * @param flip true if motor output should be flipped; or false if not. + * @return this object so that methods can be chained; never null + */ + public TalonController reverseOutput(boolean flip); + + @Override + public TalonController setForwardSoftLimit(int forwardLimit); + + @Override + public TalonController enableForwardSoftLimit(boolean enable); + + @Override + public TalonController setReverseSoftLimit(int reverseLimit); + + @Override + public TalonController enableReverseSoftLimit(boolean enable); + + @Override + public TalonController enableLimitSwitch(boolean forward, boolean reverse); + + @Override + public TalonController setForwardLimitSwitchNormallyOpen(boolean normallyOpen); + + @Override + public TalonController setReverseLimitSwitchNormallyOpen(boolean normallyOpen); + + @Override + public TalonController enableBrakeMode(boolean brake); + + @Override + public TalonController withGains(double p, double i, double d); + + @Override + public TalonController withGains(double p, double i, double d, double feedForward); + + /** + * Set the gains for the current profile of this controller. + * + * @param p the proportional gain + * @param i the integral gain + * @param d the differential gain + * @param feedForward the feed-forward gain + * @param izone Integration zone -- prevents accumulation of integration error with large errors. Setting this to zero will + * ignore any izone stuff. + * @param closeLoopRampRate Closed loop ramp rate. Maximum change in voltage, in volts / sec. + * @return this object so that methods can be chained; never null + * @see #useProfile(int) + */ + public TalonController withGains(double p, double i, double d, double feedForward, int izone, double closeLoopRampRate); + + @Override + public TalonController withProfile(int profile, double p, double i, double d); + + @Override + public TalonController withProfile(int profile, double p, double i, double d, double feedForward); + + /** + * Set the gains for the specified profile of this controller. + * + * @param profile which profile to set the PID constants for. You can have two profiles, with values of 0 or 1, allowing you + * to keep a second set of values on hand in the Talon. In order to switch profiles without recalling setPID, you + * must call setProfile(). + * @param p the proportional gain + * @param i the integral gain + * @param d the differential gain + * @param feedForward the feed-forward gain + * @param izone Integration zone -- prevents accumulation of integration error with large errors. Setting this to zero will + * ignore any izone stuff. + * @param closeLoopRampRate Closed loop ramp rate. Maximum change in voltage, in volts / sec. + * @return this object so that methods can be chained; never null + * @see #useProfile(int) + */ + public TalonController withProfile(int profile, double p, double i, double d, double feedForward, int izone, double closeLoopRampRate); + + @Override + public Gains getGainsForCurrentProfile(); + + /** + * Set the voltage ramp rate for the current profile of this controller. It limits the rate at which the throttle will + * change. Affects all modes. + * + * @param rampRate Maximum change in voltage, in volts / second + * @return this object so that methods can be chained; never null + */ + public TalonController setVoltageRampRate(double rampRate); + + @Override + public TalonController clearStickyFaults(); + + @Override + public TalonController setSafetyEnabled(boolean enabled); + + @Override + public TalonController setExpiration(double timeout); +} diff --git a/strongback/src/org/strongback/hardware/Hardware.java b/strongback/src/org/strongback/hardware/Hardware.java index e1ed4bb..555345d 100644 --- a/strongback/src/org/strongback/hardware/Hardware.java +++ b/strongback/src/org/strongback/hardware/Hardware.java @@ -32,6 +32,7 @@ import org.strongback.components.ui.FlightStick; import org.strongback.components.ui.Gamepad; import org.strongback.components.ui.InputDevice; +import org.strongback.control.TalonController; import org.strongback.function.DoubleToDoubleFunction; import org.strongback.util.Values; @@ -520,6 +521,19 @@ public static Motor victor(int channel, DoubleToDoubleFunction speedLimiter) { return new HardwareMotor(new Victor(channel), SPEED_LIMITER); } + /** + * Creates a {@link TalonSRX} motor controlled by a Talon SRX with built-in current sensor and position (angle) sensor. + * The {@link CANTalon} object passed into this method should be already configured by the calling code. + * + * @param deviceNumber the CAN device number; may not be null + * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft + * @return a {@link TalonSRX} motor; never null + */ + public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree) { + CANTalon talon = new CANTalon(deviceNumber); + return new HardwareTalonSRX(talon, pulsesPerDegree); + } + /** * Creates a {@link TalonSRX} motor controlled by a Talon SRX with built-in current sensor and position (angle) sensor. * The {@link CANTalon} object passed into this method should be already configured by the calling code. @@ -534,6 +548,25 @@ public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree) { } } + /** + * Factory method for hardware-based controllers. + */ + public static final class Controllers { + + /** + * Create a component that manages and uses the hardware-based PID controller on the Talon SRX with the given CAN device + * number. + * + * @param deviceNumber the CAN device number; may not be null + * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft + * @return the interface for managing and using the Talon SRX hardware-based PID controller; never null + */ + public static TalonController talonController(int deviceNumber, double pulsesPerDegree) { + CANTalon talon = new CANTalon(deviceNumber); + return new HardwareTalonController(talon, pulsesPerDegree); + } + } + /** * Factory methods for solenoids. */ diff --git a/strongback/src/org/strongback/hardware/HardwarePneumaticsModule.java b/strongback/src/org/strongback/hardware/HardwarePneumaticsModule.java index a84214c..64c98bf 100644 --- a/strongback/src/org/strongback/hardware/HardwarePneumaticsModule.java +++ b/strongback/src/org/strongback/hardware/HardwarePneumaticsModule.java @@ -16,6 +16,7 @@ package org.strongback.hardware; +import org.strongback.annotation.ThreadSafe; import org.strongback.components.CurrentSensor; import org.strongback.components.PneumaticsModule; import org.strongback.components.Relay; @@ -29,7 +30,8 @@ * * @author Randall Hauch */ -public class HardwarePneumaticsModule implements PneumaticsModule { +@ThreadSafe +class HardwarePneumaticsModule implements PneumaticsModule { private final Compressor pcm; private final Relay closedLoop; diff --git a/strongback/src/org/strongback/hardware/HardwareTalonController.java b/strongback/src/org/strongback/hardware/HardwareTalonController.java new file mode 100644 index 0000000..d809730 --- /dev/null +++ b/strongback/src/org/strongback/hardware/HardwareTalonController.java @@ -0,0 +1,334 @@ +/* + * Strongback + * Copyright 2015, Strongback and individual contributors by the @authors tag. + * See the COPYRIGHT.txt in the distribution for a full listing of individual + * contributors. + * + * Licensed under the MIT License; you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * http://opensource.org/licenses/MIT + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.strongback.hardware; + +import java.util.Collections; +import java.util.Set; +import java.util.concurrent.ConcurrentHashMap; + +import org.strongback.Executable; +import org.strongback.annotation.ThreadSafe; +import org.strongback.control.Controller; +import org.strongback.control.PIDController; +import org.strongback.control.TalonController; + +import edu.wpi.first.wpilibj.CANTalon; + +/** + * A hardware-based Talon SRX PID controller. + */ +@ThreadSafe +class HardwareTalonController extends HardwareTalonSRX implements TalonController { + + private static final Executable NO_OP = (clock) -> { + }; + + private static final class Gains implements TalonController.Gains { + private final double p; + private final double i; + private final double d; + private final double feedForward; + private final double iZone; + private final double closedLoopRampRate; + + protected Gains(double p, double i, double d, double feedForward, double iZone, double closeLoopRampRate) { + this.p = p; + this.i = i; + this.d = d; + this.feedForward = feedForward; + this.iZone = iZone; + this.closedLoopRampRate = closeLoopRampRate; + }; + + @Override + public double getP() { + return p; + } + + @Override + public double getI() { + return i; + } + + @Override + public double getD() { + return d; + } + + @Override + public double getFeedForward() { + return feedForward; + } + @Override + public double getIzone() { + return iZone; + } + @Override + public double getCloseLoopRampRate() { + return closedLoopRampRate; + } + } + + private double tolerance; + private volatile int currentProfile = 0; + private final Set profiles = Collections.newSetFromMap(new ConcurrentHashMap<>()); + + HardwareTalonController(CANTalon talon, double pulsesPerDegree) { + super(talon, pulsesPerDegree); + profiles.add(currentProfile); + } + + @Override + public int getDeviceID() { + return talon.getDeviceID(); + } + + @Override + public Controller disable() { + talon.disable(); + return this; + } + + @Override + public Controller enable() { + talon.enableControl(); + return this; + } + + @Override + public boolean isEnabled() { + return talon.isControlEnabled(); + } + + @Override + public double getValue() { + return talon.get(); + } + + @Override + public double getTarget() { + return talon.getSetpoint(); + } + + @Override + public Controller withTarget(double target) { + talon.set(target); + return this; + } + + @Override + public double getTolerance() { + return this.tolerance; + } + + @Override + public Controller withTolerance(double tolerance) { + this.tolerance = tolerance; + return this; + } + + @Override + public boolean computeOutput() { + // The Talon SRX is always computing output, so we don't have to do anything except + // return whether we're within tolerance ... + return isWithinTolerance(); + } + + @Override + public Controller reset() { + talon.ClearIaccum(); + return this; + } + + @Override + public Executable executable() { + return NO_OP; + } + + @Override + public boolean hasExecutable() { + return false; + } + + @Override + public ControlMode getControlMode() { + return ControlMode.valueOf(talon.getControlMode().value); + } + + @Override + public TalonController setControlMode(ControlMode mode) { + talon.changeControlMode(edu.wpi.first.wpilibj.CANTalon.ControlMode.valueOf(mode.value())); + return this; + } + + @Override + public TalonController setFeedbackDevice(FeedbackDevice device) { + talon.setFeedbackDevice(edu.wpi.first.wpilibj.CANTalon.FeedbackDevice.valueOf(device.value())); + return this; + } + + @Override + public TalonController setStatusFrameRate(StatusFrameRate frameRate, int periodMillis) { + talon.setStatusFrameRateMs(edu.wpi.first.wpilibj.CANTalon.StatusFrameRate.valueOf(frameRate.value()), periodMillis); + return this; + } + + @Override + public TalonController reverseOutput(boolean flip) { + talon.reverseOutput(flip); + return this; + } + + @Override + public TalonController reverseSensor(boolean flip) { + talon.reverseSensor(flip); + return this; + } + + @Override + public TalonController setForwardSoftLimit(int forwardLimit) { + super.setForwardSoftLimit(forwardLimit); + return this; + } + + @Override + public TalonController enableForwardSoftLimit(boolean enable) { + super.enableForwardSoftLimit(enable); + return this; + } + + @Override + public TalonController setReverseSoftLimit(int reverseLimit) { + super.setReverseSoftLimit(reverseLimit); + return this; + } + + @Override + public TalonController enableReverseSoftLimit(boolean enable) { + super.enableReverseSoftLimit(enable); + return this; + } + + @Override + public TalonController enableLimitSwitch(boolean forward, boolean reverse) { + super.enableLimitSwitch(forward, reverse); + return this; + } + + @Override + public TalonController enableBrakeMode(boolean brake) { + super.enableBrakeMode(brake); + return this; + } + + @Override + public TalonController setForwardLimitSwitchNormallyOpen(boolean normallyOpen) { + super.setForwardLimitSwitchNormallyOpen(normallyOpen); + return this; + } + + @Override + public TalonController setReverseLimitSwitchNormallyOpen(boolean normallyOpen) { + super.setReverseLimitSwitchNormallyOpen(normallyOpen); + return this; + } + + @Override + public TalonController withGains(double p, double i, double d) { + talon.setPID(p, i, d); + return this; + } + + @Override + public TalonController withGains(double p, double i, double d, double feedForward) { + talon.setPID(p, i, d); + talon.setF(feedForward); + return this; + } + + @Override + public TalonController withGains(double p, double i, double d, double feedForward, int izone, double closeLoopRampRate) { + talon.setPID(p, i, d); + talon.setF(feedForward); + talon.setIZone(izone); + talon.setCloseLoopRampRate(closeLoopRampRate); + return this; + } + + @Override + public TalonController withProfile(int profile, double p, double i, double d) { + return withProfile(profile,p,i,d,0.0,0,0.0); + } + + @Override + public TalonController withProfile(int profile, double p, double i, double d, double feedForward) { + return withProfile(profile,p,i,d,feedForward,0,0.0); + } + + @Override + public TalonController withProfile(int profile, double p, double i, double d, double feedForward, int izone, double closeLoopRampRate) { + talon.setPID(p, i, d, feedForward, izone, closeLoopRampRate, profile); + return this; + } + + @Override + public PIDController useProfile(int profile) { + talon.setProfile(profile); + this.currentProfile = profile; + this.profiles.add(profile); + return this; + } + + @Override + public int getCurrentProfile() { + return currentProfile; + } + + @Override + public Set getProfiles() { + return Collections.unmodifiableSet(profiles); + } + + @Override + public TalonController.Gains getGainsForCurrentProfile() { + return new Gains(talon.getP(),talon.getI(), talon.getD(), talon.getF(), talon.getIZone(), talon.getCloseLoopRampRate()); + } + + @Override + public TalonController setVoltageRampRate(double rampRate) { + talon.setVoltageRampRate(rampRate); + return this; + } + + @Override + public TalonController clearStickyFaults() { + super.clearStickyFaults(); + return this; + } + + @Override + public TalonController setSafetyEnabled(boolean enabled) { + super.setSafetyEnabled(enabled); + return this; + } + + @Override + public TalonController setExpiration(double timeout) { + super.setExpiration(timeout); + return this; + } +} diff --git a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java index fb4cdba..97e8918 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java @@ -21,6 +21,8 @@ import org.strongback.components.CurrentSensor; import org.strongback.components.Switch; import org.strongback.components.TalonSRX; +import org.strongback.components.TemperatureSensor; +import org.strongback.components.VoltageSensor; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CANTalon.ControlMode; @@ -33,20 +35,28 @@ * @see CANTalon */ @Immutable -final class HardwareTalonSRX implements TalonSRX { +class HardwareTalonSRX implements TalonSRX { - private final CANTalon talon; - private final AngleSensor encoder; - private final Switch forwardLimitSwitch; - private final Switch reverseLimitSwitch; - private final CurrentSensor current; + protected final CANTalon talon; + protected final AngleSensor encoder; + protected final Switch forwardLimitSwitch; + protected final Switch reverseLimitSwitch; + protected final CurrentSensor outputCurrent; + protected final VoltageSensor outputVoltage; + protected final VoltageSensor busVoltage; + protected final TemperatureSensor temperature; + protected final Faults instantaneousFaults; + protected final Faults stickyFaults; HardwareTalonSRX(CANTalon talon, double pulsesPerDegree) { this.talon = talon; this.forwardLimitSwitch = talon::isRevLimitSwitchClosed; this.reverseLimitSwitch = talon::isFwdLimitSwitchClosed; - this.current = talon::getOutputCurrent; + this.outputCurrent = talon::getOutputCurrent; + this.outputVoltage = talon::getOutputVoltage; + this.busVoltage = talon::getBusVoltage; + this.temperature = talon::getTemp; this.encoder = new AngleSensor() { private int zero = 0; @@ -61,6 +71,66 @@ public AngleSensor zero() { return this; } }; + instantaneousFaults = new Faults() { + @Override + public Switch forwardLimitSwitch() { + return ()->talon.getFaultForLim() != 0; + } + @Override + public Switch reverseLimitSwitch() { + return ()->talon.getFaultRevLim() != 0; + } + @Override + public Switch forwardSoftLimit() { + return ()->talon.getFaultForSoftLim() != 0; + } + @Override + public Switch reverseSoftLimit() { + return ()->talon.getFaultRevSoftLim() != 0; + } + @Override + public Switch hardwareFailure() { + return ()->talon.getFaultHardwareFailure() != 0; + } + @Override + public Switch overTemperature() { + return ()->talon.getFaultOverTemp() != 0; + } + @Override + public Switch underVoltage() { + return ()->talon.getFaultUnderVoltage() != 0; + } + }; + stickyFaults = new Faults() { + @Override + public Switch forwardLimitSwitch() { + return ()->talon.getStickyFaultForLim() != 0; + } + @Override + public Switch reverseLimitSwitch() { + return ()->talon.getStickyFaultRevLim() != 0; + } + @Override + public Switch forwardSoftLimit() { + return ()->talon.getStickyFaultForSoftLim() != 0; + } + @Override + public Switch reverseSoftLimit() { + return ()->talon.getStickyFaultRevSoftLim() != 0; + } + @Override + public Switch hardwareFailure() { + return ()->talon.getFaultHardwareFailure() != 0; // no sticky version! + } + @Override + public Switch overTemperature() { + return ()->talon.getStickyFaultOverTemp() != 0; + } + @Override + public Switch underVoltage() { + return ()->talon.getStickyFaultUnderVoltage() != 0; + } + }; } @Override @@ -99,6 +169,117 @@ public Switch getReverseLimitSwitch() { @Override public CurrentSensor getCurrentSensor() { - return current; + return outputCurrent; + } + + @Override + public VoltageSensor getVoltageSensor() { + return outputVoltage; + } + + @Override + public VoltageSensor getBusVoltageSensor() { + return busVoltage; + } + + @Override + public TemperatureSensor getTemperatureSensor() { + return temperature; + } + + @Override + public TalonSRX setForwardSoftLimit(int forwardLimit) { + talon.setForwardSoftLimit(forwardLimit); + return this; + } + + @Override + public TalonSRX enableForwardSoftLimit(boolean enable) { + talon.enableForwardSoftLimit(enable); + return this; + } + + @Override + public TalonSRX setReverseSoftLimit(int reverseLimit) { + talon.setReverseSoftLimit(reverseLimit); + return this; + } + + @Override + public TalonSRX enableReverseSoftLimit(boolean enable) { + talon.enableReverseSoftLimit(enable); + return this; + } + + @Override + public TalonSRX enableLimitSwitch(boolean forward, boolean reverse) { + talon.enableLimitSwitch(forward, reverse); + return this; + } + + @Override + public TalonSRX enableBrakeMode(boolean brake) { + talon.enableBrakeMode(brake); + return this; + } + + @Override + public TalonSRX setForwardLimitSwitchNormallyOpen(boolean normallyOpen) { + talon.ConfigFwdLimitSwitchNormallyOpen(normallyOpen); + return this; + } + + @Override + public TalonSRX setReverseLimitSwitchNormallyOpen(boolean normallyOpen) { + talon.ConfigRevLimitSwitchNormallyOpen(normallyOpen); + return this; + } + + @Override + public Faults faults() { + return instantaneousFaults; + } + + @Override + public Faults stickyFaults() { + return stickyFaults; + } + + @Override + public TalonSRX clearStickyFaults() { + talon.clearStickyFaults(); + return this; + } + + @Override + public long getFirmwareVersion() { + return talon.GetFirmwareVersion(); + } + + @Override + public boolean isSafetyEnabled() { + return talon.isSafetyEnabled(); + } + + @Override + public TalonSRX setSafetyEnabled(boolean enabled) { + talon.setSafetyEnabled(enabled); + return this; + } + + @Override + public double getExpiration() { + return talon.getExpiration(); + } + + @Override + public TalonSRX setExpiration(double timeout) { + talon.setExpiration(timeout); + return this; + } + + @Override + public boolean isAlive() { + return talon.isAlive(); } } \ No newline at end of file From 1e8d3540e6921a79426cf3565ad48f433f14e997 Mon Sep 17 00:00:00 2001 From: Randall Hauch Date: Mon, 16 Nov 2015 16:53:11 -0600 Subject: [PATCH 2/6] Pushed more TalonController methods up into TalonSRX. Also revamped how the input sensors are accessed via TalonSRX; both analog and quadrature encoder inputs are accessible via Gyroscope (since both position and velocity are available), and selecting one for feedback will be reflected in the separate selected input Gyroscope. The trick is that all 3 are available at the same time, although they operate upon different frames and at different rates. --- .../src/org/strongback/mock/Mock.java | 35 ++- .../src/org/strongback/mock/MockTalonSRX.java | 63 ++++- .../org/strongback/components/TalonSRX.java | 149 ++++++++++- .../control/SoftwarePIDController.java | 2 +- .../strongback/control/TalonController.java | 208 ++++++--------- .../src/org/strongback/hardware/Hardware.java | 109 +++++++- .../hardware/HardwareTalonController.java | 15 +- .../strongback/hardware/HardwareTalonSRX.java | 240 +++++++++++++++--- 8 files changed, 632 insertions(+), 189 deletions(-) diff --git a/strongback-testing/src/org/strongback/mock/Mock.java b/strongback-testing/src/org/strongback/mock/Mock.java index 49d6649..c9bd73a 100644 --- a/strongback-testing/src/org/strongback/mock/Mock.java +++ b/strongback-testing/src/org/strongback/mock/Mock.java @@ -16,6 +16,8 @@ package org.strongback.mock; +import java.util.concurrent.atomic.AtomicLong; + import org.strongback.components.Fuse; import org.strongback.components.TalonSRX; import org.strongback.control.Controller; @@ -28,6 +30,8 @@ */ public class Mock { + private static final AtomicLong CAN_DEVICE_ID_GENERATOR = new AtomicLong(); + /** * Create a mock power panel. * @@ -232,13 +236,34 @@ public static MockMotor runningMotor(double speed) { return new MockMotor(speed); } + /** + * Create a stopped mock {@link TalonSRX} motor. + * + * @param deviceId the CAN device ID + * @return the mock TalonSRX motor; never null + */ + public static MockTalonSRX stoppedTalonSRX(int deviceId) { + return runningTalonSRX(deviceId, 0.0); + } + + /** + * Create a running mock {@link TalonSRX} motor. + * + * @param deviceId the CAN device ID + * @param speed the initial speed + * @return the mock TalonSRX motor; never null + */ + public static MockTalonSRX runningTalonSRX(int deviceId, double speed) { + return new MockTalonSRX(deviceId, speed); + } + /** * Create a stopped mock {@link TalonSRX} motor. * * @return the mock TalonSRX motor; never null */ public static MockTalonSRX stoppedTalonSRX() { - return new MockTalonSRX(0.0); + return stoppedTalonSRX(nextDeviceId()); } /** @@ -248,11 +273,16 @@ public static MockTalonSRX stoppedTalonSRX() { * @return the mock TalonSRX motor; never null */ public static MockTalonSRX runningTalonSRX(double speed) { - return new MockTalonSRX(speed); + return runningTalonSRX(nextDeviceId(), speed); + } + + protected static int nextDeviceId() { + return (int)CAN_DEVICE_ID_GENERATOR.getAndIncrement(); } /** * Create a mock {@link Controller}. + * * @return the mock controller; never null */ public static MockController controller() { @@ -261,6 +291,7 @@ public static MockController controller() { /** * Create a mock {@link PIDController}. + * * @return the mock controller; never null */ public static MockPIDController pidController() { diff --git a/strongback-testing/src/org/strongback/mock/MockTalonSRX.java b/strongback-testing/src/org/strongback/mock/MockTalonSRX.java index 1753c90..aa8beb7 100644 --- a/strongback-testing/src/org/strongback/mock/MockTalonSRX.java +++ b/strongback-testing/src/org/strongback/mock/MockTalonSRX.java @@ -17,6 +17,7 @@ package org.strongback.mock; import org.strongback.components.Fuse; +import org.strongback.components.Gyroscope; import org.strongback.components.Switch; import org.strongback.components.TalonSRX; import org.strongback.components.TemperatureSensor; @@ -24,7 +25,12 @@ public class MockTalonSRX extends MockMotor implements TalonSRX { - private final MockAngleSensor encoder = new MockAngleSensor(); + private static final Gyroscope NO_OP_GYRO = new MockGyroscope(); + + private final int deviceId; + private final MockGyroscope encoderInput = new MockGyroscope(); + private final MockGyroscope analogInput = new MockGyroscope(); + private Gyroscope selectedInput = NO_OP_GYRO; private final MockCurrentSensor current = new MockCurrentSensor(); private final MockVoltageSensor voltage = new MockVoltageSensor(); private final MockVoltageSensor busVoltage = new MockVoltageSensor(); @@ -37,8 +43,14 @@ public class MockTalonSRX extends MockMotor implements TalonSRX { private double expiration = 0.1d; private boolean alive = true; - protected MockTalonSRX(double speed) { + protected MockTalonSRX(int deviceId, double speed) { super(speed); + this.deviceId = deviceId; + } + + @Override + public int getDeviceID() { + return deviceId; } @Override @@ -48,8 +60,51 @@ public MockTalonSRX setSpeed(double speed) { } @Override - public MockAngleSensor getAngleSensor() { - return encoder; + public MockGyroscope getAnalogInput() { + return analogInput; + } + + @Override + public MockGyroscope getEncoderInput() { + return encoderInput; + } + + @Override + public Gyroscope getSelectedSensor() { + return selectedInput; + } + + @Override + public MockTalonSRX reverseSensor(boolean flip) { + return this; + } + + @Override + public MockTalonSRX setFeedbackDevice(FeedbackDevice device) { + switch(device) { + case ANALOG_POTENTIOMETER: + case ANALOG_ENCODER: + this.selectedInput = analogInput; + break; + case QUADRATURE_ENCODER: + this.selectedInput = encoderInput; + break; + case ENCODER_FALLING: + case ENCODER_RISING: + selectedInput = NO_OP_GYRO; + break; + } + return this; + } + + @Override + public MockTalonSRX setStatusFrameRate(StatusFrameRate frameRate, int periodMillis) { + return this; + } + + @Override + public MockTalonSRX setVoltageRampRate(double rampRate) { + return this; } @Override diff --git a/strongback/src/org/strongback/components/TalonSRX.java b/strongback/src/org/strongback/components/TalonSRX.java index cd8ab9a..669313b 100644 --- a/strongback/src/org/strongback/components/TalonSRX.java +++ b/strongback/src/org/strongback/components/TalonSRX.java @@ -27,11 +27,52 @@ public interface TalonSRX extends LimitedMotor { public TalonSRX setSpeed(double speed); /** + * Get the CAN device ID. + * + * @return the device ID. + */ + public int getDeviceID(); + + /** + * Deprecated. Use {@link #getEncoderInput()} instead. + *

* Get the angle sensor (encoder) hooked up to the Talon SRX motor controller. * * @return the angle sensor; never null, but if not hooked up the sensor will always return a meaningless value + * @deprecated Use {@link #getEncoderInput()} instead. + */ + @Deprecated + public default AngleSensor getAngleSensor() { + return getEncoderInput(); + } + + /** + * Get the current encoder angle and rate, regardless of whether it is the current feedback device. + * + * @return the gyroscope that reads the encoder sensor; or null if the motor was created with no quadrature encoder input + * @see org.strongback.hardware.Hardware.Motors#talonSRX(int) + * @see org.strongback.hardware.Hardware.Motors#talonSRX(int, double) + * @see org.strongback.hardware.Hardware.Motors#talonSRX(int, double, double) + */ + public Gyroscope getEncoderInput(); + + /** + * Get the current analog angle and rate, regardless of whether it is the current feedback device. + * + * @return the gyroscope that reads the 3.3V analog sensor; or null if the motor was created with no analog input + * @see org.strongback.hardware.Hardware.Motors#talonSRX(int) + * @see org.strongback.hardware.Hardware.Motors#talonSRX(int, double) + * @see org.strongback.hardware.Hardware.Motors#talonSRX(int, double, double) + */ + public Gyroscope getAnalogInput(); + + /** + * Get the input angle and rate of the current {@link #setFeedbackDevice(FeedbackDevice) feedback device}. + * + * @return the selected input device sensor; never null, but it may return a meaningless value if a sensor is not physically + * wired as an input to the Talon device */ - public AngleSensor getAngleSensor(); + public Gyroscope getSelectedSensor(); /** * Get the Talon SRX's output current sensor. @@ -61,6 +102,36 @@ public interface TalonSRX extends LimitedMotor { */ public TemperatureSensor getTemperatureSensor(); + /** + * Set the feedback device for this controller. + * + * @param device the feedback device; may not be null + * @return this object so that methods can be chained; never null + * @see #reverseSensor(boolean) + */ + public TalonSRX setFeedbackDevice(FeedbackDevice device); + + /** + * Set the status frame rate for this controller. + * + * @param frameRate the status frame rate; may not be null + * @param periodMillis frame rate period in milliseconds + * @return this object so that methods can be chained; never null + */ + public TalonSRX setStatusFrameRate(StatusFrameRate frameRate, int periodMillis); + + /** + * Flips the sign (multiplies by negative one) the {@link #setFeedbackDevice(FeedbackDevice) feedback device} values read by + * the Talon. + *

+ * This only affects position and velocity closed loop control. Allows for situations where you may have a sensor flipped + * and going in the wrong direction. + * + * @param flip true if sensor input should be flipped, or false if not. + * @return this object so that methods can be chained; never null + */ + public TalonSRX reverseSensor(boolean flip); + /** * Set the soft limit for forward motion, which will disable the motor when the sensor is out of range. * @@ -132,6 +203,16 @@ public interface TalonSRX extends LimitedMotor { */ public TalonSRX enableBrakeMode(boolean brake); + /** + * The Talon SRX can be set to honor a ramp rate to prevent instantaneous changes in throttle. This ramp rate is in effect + * regardless of which mode is selected (throttle, slave, or closed-loop). For example, setting the ramp rate to + * 6.0 will limit the maximum voltage change within any second to be less than or equal to 6.0 volts. + * + * @param rampRate maximum change in voltage per second, in volts / second + * @return this object so that methods can be chained; never null + */ + public TalonSRX setVoltageRampRate(double rampRate); + /** * Get the faults currently associated with the Talon controller. These state of these faults may change at any time based * upon the state of the module, and so they cannot be manually cleared. @@ -209,6 +290,72 @@ public interface TalonSRX extends LimitedMotor { */ public boolean isAlive(); + /** + * The type of feedback sensor used by this Talon controller. + */ + public enum FeedbackDevice { + /** + * Use Quadrature Encoder. + */ + QUADRATURE_ENCODER(0), /** + * Analog potentiometer, 0-3.3V + */ + ANALOG_POTENTIOMETER(2), /** + * Analog encoder or any other analog device, 0-3.3V + */ + ANALOG_ENCODER(3), /** + * Encoder that increments position per rising edge (and never decrements) on Quadrature-A. + */ + ENCODER_RISING(4), /** + * Encoder that increments position per falling edge (and never decrements) on Quadrature-A. + */ + ENCODER_FALLING(5); + + public int value; + + public static FeedbackDevice valueOf(int value) { + for (FeedbackDevice mode : values()) { + if (mode.value == value) { + return mode; + } + } + return null; + } + + private FeedbackDevice(int value) { + this.value = value; + } + + public int value() { + return this.value; + } + } + + /** + * Types of status frame rates. + */ + public enum StatusFrameRate { + GENERAL(0), FEEDBACK(1), QUADRATURE_ENCODER(2), ANALOG_TEMPERATURE_BATTERY_VOLTAGE(3); + public int value; + + public static StatusFrameRate valueOf(int value) { + for (StatusFrameRate mode : values()) { + if (mode.value == value) { + return mode; + } + } + return null; + } + + private StatusFrameRate(int value) { + this.value = value; + } + + public int value() { + return this.value; + } + } + /** * The set of possible faults that this module can trigger. */ diff --git a/strongback/src/org/strongback/control/SoftwarePIDController.java b/strongback/src/org/strongback/control/SoftwarePIDController.java index 330e9a8..c5c1690 100644 --- a/strongback/src/org/strongback/control/SoftwarePIDController.java +++ b/strongback/src/org/strongback/control/SoftwarePIDController.java @@ -524,7 +524,7 @@ protected double currentFeedForward() { return gains.feedForward; } - protected final class Gains implements PIDController.Gains { + protected static final class Gains implements PIDController.Gains { protected final double p; protected final double i; protected final double d; diff --git a/strongback/src/org/strongback/control/TalonController.java b/strongback/src/org/strongback/control/TalonController.java index 2b75e2a..6f58d4d 100644 --- a/strongback/src/org/strongback/control/TalonController.java +++ b/strongback/src/org/strongback/control/TalonController.java @@ -19,7 +19,72 @@ import org.strongback.components.TalonSRX; /** - * The Talon SRX Proportional Integral Differential (PID) controller with optional support for feed forward. + * A hardware-based Proportional Integral Differential (PID) controller that runs on the Talon SRX motor controller. The + * controller's input is wired as a sensor on the Talon SRX device, and specified via the + * {@link #setFeedbackDevice(FeedbackDevice)} method. The controller's mode of control is set via the + * {@link #setControlMode(ControlMode)}. + *

Important usage notes

+ *

+ * This section outlines some useful and frequently used features of the TalonController. + *

Ramp rate

+ *

+ * The Talon SRX can be set to honor a ramp rate to prevent instantaneous changes in throttle. This ramp rate is in effect + * regardless of which mode is selected (throttle, slave, or closed-loop). + * + * 

+ * controller.setVoltageRampRate(6.0); // Allow throttle changes up to 6.0V per second
+ *
+ * + *

Feedback device

+ *

+ * The analog and quadrature signals are available all the time in all modes (throttle, slave, or closed-loop), but the Talon + * SRX requires the robot application to "pick" a particular feedback device for soft limit and closed-loop features. The Talon + * SRX defaults to the {@link org.strongback.components.TalonSRX.FeedbackDevice#QUADRATURE_ENCODER quadrature encoder}. + * + * 

+ * controller.setFeedbackDevice(FeedbackDevice.ANALOG_ENCODER);
+ *
+ * + *

+ * In order for limit switches and closed-loop features to function correctly, the sensor and motor have to be “in-phase”. This + * means that the sensor position must move in a positive direction as the motor controller drives positive + * throttle. To test this, first drive the motor manually (using a human input device), and watch the sensor position either in + * the roboRIO Web-based Configuration Self-Test, or by calling {@link #getSelectedSensor()} and printing it to console. If the + * "Sensor Position" moves in a negative direction while Talon SRX throttle is positive (blinking green), then use the + * {@link #reverseSensor(boolean)} so that the Talon negates the sensor reading. Then re-test to confirm sensed position moves + * in a positive direction with positive motor drive. + *

+ * In the special case of using the {@link org.strongback.components.TalonSRX.FeedbackDevice#ENCODER_RISING} feedback device, + * {@link #reverseSensor(boolean)} should be called with false, since a rising encoder is guaranteed to be positive + * since it increments per rising edge, and never decrements. + * + *

Soft limits

+ *

+ * Soft limits can be used to disable motor drive when the position value read from the feedback device is outside of a + * specified range. Forward throttle will be disabled when {@link #enableForwardSoftLimit(boolean)} is set to true + * and the sensor position is greater than the {@link #setForwardSoftLimit(int)} value. Reverse throttle will be disabled when + * {@link #enableReverseSoftLimit(boolean)} is set to true and the sensor position is less than the + * {@link #setReverseSoftLimit(int)}. + * + *

Control modes

+ *

+ * The closed-loop logic is the same regardless of which feedback sensor or closed-loop mode is selected. + * + * + *

Follower mode

+ *

+ * Any Talon SRX on CAN bus can be instructed to "follow" the drive output of another Talon SRX. This is done by putting a Talon + * SRX into "follower" mode and specifying the device ID of the "lead Talon". The follower Talon will then mirror the output of + * the leader, which can be in any mode: closed-loop, voltage percent (duty-cycle), or even following yet another Talon SRX. + * + * 

+ * leaderController = ...
+ * followerController.setControlMode(ControlMode.FOLLOWER);
+ * followerController.withTarget(leaderController.getDeviceID());
+ *
+ * + * The {@link #reverseOutput(boolean)} method can be used to invert the output of a follower Talon. This may be useful if a + * follower and leader Talon are wired out of phase with each other. */ public interface TalonController extends PIDController, TalonSRX { @@ -55,22 +120,18 @@ public static enum ControlMode { /** * Control the motor by directly setting the desired position (in ticks or an analog value) for the motor's encoder. */ - POSITION(1), - /** - * Control the motor's speed in position change every 10 milliseconds. - */ - SPEED(2), - /** - * Control the motor by directly setting the current to be sent to the motor. - */ - CURRENT(3), - /** - * Control the motor by directly setting the voltage to be sent to the motor. - */ - VOLTAGE(4), - /** - * Control the motor by following another Talon SRX device. - */ + POSITION(1), /** + * Control the motor's speed in position change every 10 milliseconds. + */ + SPEED(2), /** + * Control the motor by directly setting the current to be sent to the motor. + */ + CURRENT(3), /** + * Control the motor by directly setting the voltage to be sent to the motor. + */ + VOLTAGE(4), /** + * Control the motor by following another Talon SRX device. + */ FOLLOWER(5); private final int value; @@ -93,83 +154,6 @@ public int value() { } } - /** - * The type of control sensor used by this Talon controller. - */ - public enum FeedbackDevice { - /** - * Use Quadrature Encoder. - */ - QUADRATURE_ENCODER(0), - /** - * Analog potentiometer, 0-3.3V - */ - ANALOG_POTENTIOMETER(2), - /** - * Analog encoder, 0-3.3V - */ - ANALOG_ENCODER(3), - /** - * Encoder that increment position per rising edge on Quadrature-A. - */ - ENCODER_RISING(4), - /** - * Encoder that increment position per falling edge on Quadrature-A. - */ - ENCODER_FALLING(5); - - public int value; - - public static FeedbackDevice valueOf(int value) { - for (FeedbackDevice mode : values()) { - if (mode.value == value) { - return mode; - } - } - return null; - } - - private FeedbackDevice(int value) { - this.value = value; - } - - public int value() { - return this.value; - } - } - - /** - * Types of status frame rates. - */ - public enum StatusFrameRate { - GENERAL(0), FEEDBACK(1), QUADRATURE_ENCODER(2), ANALOG_TEMP_VBAT(3); - public int value; - - public static StatusFrameRate valueOf(int value) { - for (StatusFrameRate mode : values()) { - if (mode.value == value) { - return mode; - } - } - return null; - } - - private StatusFrameRate(int value) { - this.value = value; - } - - public int value() { - return this.value; - } - } - - /** - * Get the CAN device ID. - * - * @return the device ID. - */ - public int getDeviceID(); - /** * Get this controller's current control mode. * @@ -185,32 +169,13 @@ public int value() { */ public TalonController setControlMode(ControlMode mode); - /** - * Set the feedback device for this controller. - * - * @param device the feedback device; may not be null - * @return this object so that methods can be chained; never null - */ + @Override public TalonController setFeedbackDevice(FeedbackDevice device); - /** - * Set the status frame rate for this controller. - * - * @param frameRate the status frame rate; may not be null - * @param periodMillis frame rate period in milliseconds - * @return this object so that methods can be chained; never null - */ + @Override public TalonController setStatusFrameRate(StatusFrameRate frameRate, int periodMillis); - /** - * Flips the sign (multiplies by negative one) the sensor values going into the talon. - * - * This only affects position and velocity closed loop control. Allows for situations where you may have a sensor flipped - * and going in the wrong direction. - * - * @param flip true if sensor input should be flipped, or false if not. - * @return this object so that methods can be chained; never null - */ + @Override public TalonController reverseSensor(boolean flip); /** @@ -288,18 +253,13 @@ public int value() { * @return this object so that methods can be chained; never null * @see #useProfile(int) */ - public TalonController withProfile(int profile, double p, double i, double d, double feedForward, int izone, double closeLoopRampRate); + public TalonController withProfile(int profile, double p, double i, double d, double feedForward, int izone, + double closeLoopRampRate); @Override public Gains getGainsForCurrentProfile(); - /** - * Set the voltage ramp rate for the current profile of this controller. It limits the rate at which the throttle will - * change. Affects all modes. - * - * @param rampRate Maximum change in voltage, in volts / second - * @return this object so that methods can be chained; never null - */ + @Override public TalonController setVoltageRampRate(double rampRate); @Override diff --git a/strongback/src/org/strongback/hardware/Hardware.java b/strongback/src/org/strongback/hardware/Hardware.java index 555345d..9a26d0a 100644 --- a/strongback/src/org/strongback/hardware/Hardware.java +++ b/strongback/src/org/strongback/hardware/Hardware.java @@ -44,6 +44,7 @@ import edu.wpi.first.wpilibj.AnalogTrigger; import edu.wpi.first.wpilibj.BuiltInAccelerometer; import edu.wpi.first.wpilibj.CANTalon; +import edu.wpi.first.wpilibj.CANTalon.ControlMode; import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -522,29 +523,105 @@ public static Motor victor(int channel, DoubleToDoubleFunction speedLimiter) { } /** - * Creates a {@link TalonSRX} motor controlled by a Talon SRX with built-in current sensor and position (angle) sensor. - * The {@link CANTalon} object passed into this method should be already configured by the calling code. + * Creates a {@link TalonSRX} motor controlled by a Talon SRX with no sensors wired as inputs. + *

+ * The resulting {@link TalonSRX} will have a null {@link TalonSRX#getAnalogInput()} and a null + * {@link TalonSRX#getEncoderInput()}. * - * @param deviceNumber the CAN device number; may not be null - * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft + * @param deviceNumber the CAN device number for the Talon SRX; may not be null + * @return a {@link TalonSRX} motor; never null + */ + public static TalonSRX talonSRX(int deviceNumber) { + return talonSRX(deviceNumber, 0.0d, 0.0d); + } + + /** + * Creates a {@link TalonSRX} motor controlled by a Talon SRX with an optional quadrature encoder and no analog input + * wired into the Talon. + *

+ * The resulting {@link TalonSRX} will have a non-null {@link TalonSRX#getEncoderInput()} when the + * pulsesPerDegree is non-zero. But the resulting {@link TalonSRX} will always have a null + * {@link TalonSRX#getAnalogInput()}. + * + * @param deviceNumber the CAN device number for the Talon SRX; may not be null + * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft; may be 0 if unused * @return a {@link TalonSRX} motor; never null */ public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree) { + return talonSRX(deviceNumber, pulsesPerDegree, 0.0d); + } + + /** + * Creates a {@link TalonSRX} motor controlled by a Talon SRX with an optional quadrature encoder and no an analog 3.3V + * input wired into the Talon. + *

+ * The resulting {@link TalonSRX} will have a non-null {@link TalonSRX#getEncoderInput()} when the + * pulsesPerDegree is non-zero. Likewise, the resulting {@link TalonSRX} will have a non-null + * {@link TalonSRX#getAnalogInput()} when the analogTurnsOverVoltageRange is non-zero. + * + * @param deviceNumber the CAN device number for the Talon SRX; may not be null + * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft; may be 0 if unused + * @param analogTurnsOverVoltageRange the number of turns of an analog pot or analog encoder over the 0-3.3V range; may + * be 0 if unused + * @return a {@link TalonSRX} motor; never null + */ + public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree, double analogTurnsOverVoltageRange) { CANTalon talon = new CANTalon(deviceNumber); - return new HardwareTalonSRX(talon, pulsesPerDegree); + return new HardwareTalonSRX(talon, pulsesPerDegree, analogTurnsOverVoltageRange); } /** - * Creates a {@link TalonSRX} motor controlled by a Talon SRX with built-in current sensor and position (angle) sensor. - * The {@link CANTalon} object passed into this method should be already configured by the calling code. + * Creates a {@link TalonSRX} motor controlled by a Talon SRX with an optional (angle) sensor. The {@link CANTalon} + * object passed into this method should be already configured by the calling code. + *

+ * The resulting {@link TalonSRX} will have a non-null {@link TalonSRX#getEncoderInput()} when the + * pulsesPerDegree is non-zero. But the resulting {@link TalonSRX} will always have a null + * {@link TalonSRX#getAnalogInput()}. * * @param talon the already configured {@link CANTalon} instance; may not be null * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft * @return a {@link TalonSRX} motor; never null */ public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree) { + return talonSRX(talon, pulsesPerDegree, 0.0d); + } + + /** + * Creates a {@link TalonSRX} motor controlled by a Talon SRX with and optional quadrature encoder and/or an analog 3.3V + * input wired into the Talon. The {@link CANTalon} object passed into this method should be already configured by the + * calling code. + *

+ * The resulting {@link TalonSRX} will have a non-null {@link TalonSRX#getEncoderInput()} when the + * pulsesPerDegree is non-zero. Likewise, the resulting {@link TalonSRX} will have a non-null + * {@link TalonSRX#getAnalogInput()} when the analogTurnsOverVoltageRange is non-zero. + * + * @param talon the already configured {@link CANTalon} instance; may not be null + * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft + * @param analogTurnsOverVoltageRange the number of turns of an analog pot or analog encoder over the 0-3.3V range; may + * be 0 if unused + * @return a {@link TalonSRX} motor; never null + */ + public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree, double analogTurnsOverVoltageRange) { if (talon == null) throw new IllegalArgumentException("The CANTalon reference may not be null"); - return new HardwareTalonSRX(talon, pulsesPerDegree); + return new HardwareTalonSRX(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + } + + /** + * Creates a {@link TalonSRX} motor controller that follows another Talon SRX. The resulting TalonSRX will have neither + * a {@link TalonSRX#getAnalogInput()} or a {@link TalonSRX#getEncoderInput()}. + * + * @param deviceNumber the CAN device number for the Talon SRX; may not be null + * @param leader the Talon SRX that is to be followed; may not be null + * @param reverse true if the resulting Talon should have inverted output compared to the leader, or + * false if the output should exactly match the leader + * @return a {@link TalonSRX} motor controller that follows the leader; never null + */ + public static TalonSRX talonSRX(int deviceNumber, TalonSRX leader, boolean reverse) { + CANTalon talon = new CANTalon(deviceNumber); + talon.changeControlMode(ControlMode.Follower); + talon.set(leader.getDeviceID()); + talon.reverseOutput(reverse); + return talonSRX(talon, 0.0d, 0.0d); } } @@ -554,16 +631,24 @@ public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree) { public static final class Controllers { /** - * Create a component that manages and uses the hardware-based PID controller on the Talon SRX with the given CAN device - * number. + * Create a component that manages and uses the hardware-based PID controller on the Talon SRX with a quadrature encoder + * and/or an analog 3.3V input sensor wired into the Talon. + *

+ * The resulting {@link TalonSRX} will have a non-null {@link TalonSRX#getEncoderInput()} when the + * pulsesPerDegree is non-zero. Likewise, the resulting {@link TalonSRX} will have a non-null + * {@link TalonSRX#getAnalogInput()} when the analogTurnsOverVoltageRange is non-zero. * * @param deviceNumber the CAN device number; may not be null * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft + * @param analogTurnsOverVoltageRange the number of turns of an analog pot or analog encoder over the 0-3.3V range; may + * be 0 if unused * @return the interface for managing and using the Talon SRX hardware-based PID controller; never null */ - public static TalonController talonController(int deviceNumber, double pulsesPerDegree) { + public static TalonController talonController(int deviceNumber, double pulsesPerDegree, + double analogTurnsOverVoltageRange) { CANTalon talon = new CANTalon(deviceNumber); - return new HardwareTalonController(talon, pulsesPerDegree); + HardwareTalonController c = new HardwareTalonController(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + return c; } } diff --git a/strongback/src/org/strongback/hardware/HardwareTalonController.java b/strongback/src/org/strongback/hardware/HardwareTalonController.java index d809730..321fd1a 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonController.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonController.java @@ -87,16 +87,11 @@ public double getCloseLoopRampRate() { private volatile int currentProfile = 0; private final Set profiles = Collections.newSetFromMap(new ConcurrentHashMap<>()); - HardwareTalonController(CANTalon talon, double pulsesPerDegree) { - super(talon, pulsesPerDegree); + HardwareTalonController(CANTalon talon, double pulsesPerDegree, double analogTurnsOverVoltageRange) { + super(talon, pulsesPerDegree, analogTurnsOverVoltageRange); profiles.add(currentProfile); } - @Override - public int getDeviceID() { - return talon.getDeviceID(); - } - @Override public Controller disable() { talon.disable(); @@ -177,7 +172,7 @@ public TalonController setControlMode(ControlMode mode) { @Override public TalonController setFeedbackDevice(FeedbackDevice device) { - talon.setFeedbackDevice(edu.wpi.first.wpilibj.CANTalon.FeedbackDevice.valueOf(device.value())); + super.setFeedbackDevice(device); return this; } @@ -195,7 +190,7 @@ public TalonController reverseOutput(boolean flip) { @Override public TalonController reverseSensor(boolean flip) { - talon.reverseSensor(flip); + super.reverseSensor(flip); return this; } @@ -310,7 +305,7 @@ public TalonController.Gains getGainsForCurrentProfile() { @Override public TalonController setVoltageRampRate(double rampRate) { - talon.setVoltageRampRate(rampRate); + super.setVoltageRampRate(rampRate); return this; } diff --git a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java index 97e8918..d024499 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java @@ -16,9 +16,11 @@ package org.strongback.hardware; +import java.util.function.DoubleSupplier; + import org.strongback.annotation.Immutable; -import org.strongback.components.AngleSensor; import org.strongback.components.CurrentSensor; +import org.strongback.components.Gyroscope; import org.strongback.components.Switch; import org.strongback.components.TalonSRX; import org.strongback.components.TemperatureSensor; @@ -37,8 +39,90 @@ @Immutable class HardwareTalonSRX implements TalonSRX { + protected static final Gyroscope NO_OP_GYRO = new Gyroscope() { + + @Override + public double getAngle() { + return 0; + } + + @Override + public double getRate() { + return 0; + } + @Override + public Gyroscope zero() { + // do nothing + return this; + } + }; + + protected static Gyroscope encoderGyroscope(DoubleSupplier positionInPulses, DoubleSupplier velocityInPulsesPerCycle, + double pulsesPerDegree, DoubleSupplier cyclesPeriodInSeconds) { + if ( pulsesPerDegree <= 0.0000001d && pulsesPerDegree >= 0.0000001d ) return null; + return new Gyroscope() { + private double zero = 0.0; + + @Override + public double getAngle() { + // Units: (pulses) x (degrees/pulse) = degrees + return (positionInPulses.getAsDouble() - zero) / pulsesPerDegree; + } + + @Override + public double getRate() { + // Units: (pulses/cycle) x (degrees/pulse) x (cycles/second) = (degrees/second) + return velocityInPulsesPerCycle.getAsDouble() / pulsesPerDegree / cyclesPeriodInSeconds.getAsDouble(); + } + + @Override + public Gyroscope zero() { + zero = positionInPulses.getAsDouble(); + return this; + } + }; + } + + protected static Gyroscope analogGyroscope(DoubleSupplier analogPosition, DoubleSupplier changeInVoltsPerCycle, + double analogRange, double analogTurnsPerVolt, double voltageRange, DoubleSupplier cyclesPeriodInSeconds) { + if ( analogTurnsPerVolt <= 0.0000001d && analogTurnsPerVolt >= 0.0000001d ) return null; + return new Gyroscope() { + private double zero = 0.0; + + @Override + public double getAngle() { + // Units: (0-1023) / 1023 x (turns/volt) x (volts) x (degrees/turn) = degrees + return (analogPosition.getAsDouble() - zero) / analogRange * analogTurnsPerVolt * voltageRange * 360.0; + } + + @Override + public double getRate() { + // Units: (0-1023)/cycle / 1023 x (turns/volt) x (volts) x (degrees/turn) x (cycles/second) = (degrees/second) + return changeInVoltsPerCycle.getAsDouble() / analogRange * analogTurnsPerVolt * voltageRange * 360.0 + / cyclesPeriodInSeconds.getAsDouble(); + } + + @Override + public Gyroscope zero() { + zero = analogPosition.getAsDouble(); + return this; + } + }; + } + + private static final double DEFAULT_ANALOG_RATE = 0.100; + private static final double DEFAULT_QUADRATURE_RATE = 0.100; + private static final double DEFAULT_FEEDBACK_RATE = 0.020; + protected final CANTalon talon; - protected final AngleSensor encoder; + protected final Gyroscope encoderInput; + protected final Gyroscope analogInput; + protected final Gyroscope selectedEncoderInput; + protected final Gyroscope selectedAnalogInput; + protected volatile Gyroscope selectedInput = NO_OP_GYRO; + protected volatile double quadratureRateInSeconds = DEFAULT_QUADRATURE_RATE; + protected volatile double analogRateInSeconds = DEFAULT_ANALOG_RATE; + protected volatile double feedbackRateInSeconds = DEFAULT_FEEDBACK_RATE; protected final Switch forwardLimitSwitch; protected final Switch reverseLimitSwitch; protected final CurrentSensor outputCurrent; @@ -48,7 +132,7 @@ class HardwareTalonSRX implements TalonSRX { protected final Faults instantaneousFaults; protected final Faults stickyFaults; - HardwareTalonSRX(CANTalon talon, double pulsesPerDegree) { + HardwareTalonSRX(CANTalon talon, double pulsesPerDegree, double analogTurnsOverVoltageRange) { this.talon = talon; this.forwardLimitSwitch = talon::isRevLimitSwitchClosed; @@ -57,82 +141,105 @@ class HardwareTalonSRX implements TalonSRX { this.outputVoltage = talon::getOutputVoltage; this.busVoltage = talon::getBusVoltage; this.temperature = talon::getTemp; - this.encoder = new AngleSensor() { - private int zero = 0; - - @Override - public double getAngle() { - return (talon.getEncPosition() - zero) / pulsesPerDegree; - } - - @Override - public AngleSensor zero() { - zero = talon.getEncPosition(); - return this; - } - }; - instantaneousFaults = new Faults() { + this.encoderInput = encoderGyroscope(talon::getEncPosition, + talon::getEncVelocity, + pulsesPerDegree, + () -> quadratureRateInSeconds); + this.analogInput = analogGyroscope(talon::getAnalogInPosition, + talon::getAnalogInVelocity, + 1023, + analogTurnsOverVoltageRange / 3.3, + 3.3, + () -> analogRateInSeconds); + this.selectedEncoderInput = encoderGyroscope(talon::getPosition, + talon::getSpeed, + pulsesPerDegree, + () -> feedbackRateInSeconds); + this.selectedAnalogInput = analogGyroscope(talon::getPosition, + talon::getSpeed, + 1023, + analogTurnsOverVoltageRange / 3.3, + 3.3, + () -> feedbackRateInSeconds); + this.instantaneousFaults = new Faults() { @Override public Switch forwardLimitSwitch() { - return ()->talon.getFaultForLim() != 0; + return () -> talon.getFaultForLim() != 0; } + @Override public Switch reverseLimitSwitch() { - return ()->talon.getFaultRevLim() != 0; + return () -> talon.getFaultRevLim() != 0; } + @Override public Switch forwardSoftLimit() { - return ()->talon.getFaultForSoftLim() != 0; + return () -> talon.getFaultForSoftLim() != 0; } + @Override public Switch reverseSoftLimit() { - return ()->talon.getFaultRevSoftLim() != 0; + return () -> talon.getFaultRevSoftLim() != 0; } + @Override public Switch hardwareFailure() { - return ()->talon.getFaultHardwareFailure() != 0; + return () -> talon.getFaultHardwareFailure() != 0; } + @Override public Switch overTemperature() { - return ()->talon.getFaultOverTemp() != 0; + return () -> talon.getFaultOverTemp() != 0; } + @Override public Switch underVoltage() { - return ()->talon.getFaultUnderVoltage() != 0; + return () -> talon.getFaultUnderVoltage() != 0; } }; - stickyFaults = new Faults() { + this.stickyFaults = new Faults() { @Override public Switch forwardLimitSwitch() { - return ()->talon.getStickyFaultForLim() != 0; + return () -> talon.getStickyFaultForLim() != 0; } + @Override public Switch reverseLimitSwitch() { - return ()->talon.getStickyFaultRevLim() != 0; + return () -> talon.getStickyFaultRevLim() != 0; } + @Override public Switch forwardSoftLimit() { - return ()->talon.getStickyFaultForSoftLim() != 0; + return () -> talon.getStickyFaultForSoftLim() != 0; } + @Override public Switch reverseSoftLimit() { - return ()->talon.getStickyFaultRevSoftLim() != 0; + return () -> talon.getStickyFaultRevSoftLim() != 0; } + @Override public Switch hardwareFailure() { - return ()->talon.getFaultHardwareFailure() != 0; // no sticky version! + return () -> talon.getFaultHardwareFailure() != 0; // no sticky version! } + @Override public Switch overTemperature() { - return ()->talon.getStickyFaultOverTemp() != 0; + return () -> talon.getStickyFaultOverTemp() != 0; } + @Override public Switch underVoltage() { - return ()->talon.getStickyFaultUnderVoltage() != 0; + return () -> talon.getStickyFaultUnderVoltage() != 0; } }; } + @Override + public int getDeviceID() { + return talon.getDeviceID(); + } + @Override public double getSpeed() { talon.changeControlMode(ControlMode.PercentVbus); @@ -153,8 +260,65 @@ public void stop() { } @Override - public AngleSensor getAngleSensor() { - return encoder; + public Gyroscope getEncoderInput() { + return encoderInput; + } + + @Override + public Gyroscope getAnalogInput() { + return analogInput; + } + + @Override + public Gyroscope getSelectedSensor() { + return selectedInput; + } + + @Override + public TalonSRX setFeedbackDevice(FeedbackDevice device) { + talon.setFeedbackDevice(edu.wpi.first.wpilibj.CANTalon.FeedbackDevice.valueOf(device.value())); + switch(device) { + case ANALOG_POTENTIOMETER: + case ANALOG_ENCODER: + selectedInput = selectedAnalogInput; + break; + case QUADRATURE_ENCODER: + selectedInput = selectedEncoderInput; + break; + case ENCODER_FALLING: + case ENCODER_RISING: + // for 2015 the Talon SRX firmware did not support the falling or rising mode ... + selectedInput = NO_OP_GYRO; + break; + } + return this; + } + + @Override + public TalonSRX setStatusFrameRate(StatusFrameRate frameRate, int periodMillis) { + talon.setStatusFrameRateMs(edu.wpi.first.wpilibj.CANTalon.StatusFrameRate.valueOf(frameRate.value()), periodMillis); + double rateInSeconds = periodMillis / 1000.0; + switch(frameRate) { + case FEEDBACK: + feedbackRateInSeconds = rateInSeconds; + break; + case QUADRATURE_ENCODER: + quadratureRateInSeconds = rateInSeconds; + break; + case ANALOG_TEMPERATURE_BATTERY_VOLTAGE: + analogRateInSeconds = rateInSeconds; + break; + case GENERAL: + // do nothing + break; + } + return this; + } + + @Override + public TalonSRX reverseSensor(boolean flip) { + talon.reverseSensor(flip); + return this; } @Override @@ -235,6 +399,12 @@ public TalonSRX setReverseLimitSwitchNormallyOpen(boolean normallyOpen) { return this; } + @Override + public TalonSRX setVoltageRampRate(double rampRate) { + talon.setVoltageRampRate(rampRate); + return this; + } + @Override public Faults faults() { return instantaneousFaults; From 951c431f09c4c3f1cc45d9b60abaa9f6bb0997c4 Mon Sep 17 00:00:00 2001 From: Randall Hauch Date: Mon, 16 Nov 2015 17:20:28 -0600 Subject: [PATCH 3/6] Minor safety improvement if TalonSRX are not created correctly. --- .../strongback/hardware/HardwareTalonSRX.java | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java index d024499..2e65210 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java @@ -18,6 +18,7 @@ import java.util.function.DoubleSupplier; +import org.strongback.Strongback; import org.strongback.annotation.Immutable; import org.strongback.components.CurrentSensor; import org.strongback.components.Gyroscope; @@ -280,10 +281,20 @@ public TalonSRX setFeedbackDevice(FeedbackDevice device) { switch(device) { case ANALOG_POTENTIOMETER: case ANALOG_ENCODER: - selectedInput = selectedAnalogInput; + if ( selectedAnalogInput != null ) { + selectedInput = selectedAnalogInput; + } else { + Strongback.logger(getClass()).error("Unable to use the analog input for feedback, since the Talon SRX (device " + getDeviceID() + ") was not instantiated with an analog input. Check how this device was created using Strongback's Hardware class."); + selectedInput = NO_OP_GYRO; + } break; case QUADRATURE_ENCODER: - selectedInput = selectedEncoderInput; + if ( selectedEncoderInput != null ) { + selectedInput = selectedEncoderInput; + } else { + Strongback.logger(getClass()).error("Unable to use the quadrature encoder input for feedback, since the Talon SRX (device " + getDeviceID() + ") was not instantiated with an encoder input. Check how this device was created using Strongback's Hardware class."); + selectedInput = NO_OP_GYRO; + } break; case ENCODER_FALLING: case ENCODER_RISING: @@ -297,16 +308,16 @@ public TalonSRX setFeedbackDevice(FeedbackDevice device) { @Override public TalonSRX setStatusFrameRate(StatusFrameRate frameRate, int periodMillis) { talon.setStatusFrameRateMs(edu.wpi.first.wpilibj.CANTalon.StatusFrameRate.valueOf(frameRate.value()), periodMillis); - double rateInSeconds = periodMillis / 1000.0; + double periodInSeconds = periodMillis / 1000.0; switch(frameRate) { case FEEDBACK: - feedbackRateInSeconds = rateInSeconds; + feedbackRateInSeconds = periodInSeconds; break; case QUADRATURE_ENCODER: - quadratureRateInSeconds = rateInSeconds; + quadratureRateInSeconds = periodInSeconds; break; case ANALOG_TEMPERATURE_BATTERY_VOLTAGE: - analogRateInSeconds = rateInSeconds; + analogRateInSeconds = periodInSeconds; break; case GENERAL: // do nothing From cc9e73b88f19266935faac19b4e36de404772f02 Mon Sep 17 00:00:00 2001 From: Randall Hauch Date: Mon, 16 Nov 2015 21:54:33 -0600 Subject: [PATCH 4/6] Added unit tests for the computed Gyroscopes in the HardwareTalonSRX class. The unit tests verify the basic logic, but still don't match a real hardware test. --- .../strongback/mock/MockPIDController.java | 12 + ...ardwareTalonSRX_AnalogInputSensorTest.java | 119 ++++++++++ ...rdwareTalonSRX_EncoderInputSensorTest.java | 126 +++++++++++ .../org/strongback/components/TalonSRX.java | 93 ++++++-- .../org/strongback/control/PIDController.java | 6 + .../strongback/control/TalonController.java | 19 ++ .../src/org/strongback/hardware/Hardware.java | 2 +- .../hardware/HardwareTalonController.java | 9 +- .../strongback/hardware/HardwareTalonSRX.java | 212 ++++++++++++------ 9 files changed, 513 insertions(+), 85 deletions(-) create mode 100644 strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_AnalogInputSensorTest.java create mode 100644 strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_EncoderInputSensorTest.java diff --git a/strongback-testing/src/org/strongback/mock/MockPIDController.java b/strongback-testing/src/org/strongback/mock/MockPIDController.java index 389d231..ee5d355 100644 --- a/strongback-testing/src/org/strongback/mock/MockPIDController.java +++ b/strongback-testing/src/org/strongback/mock/MockPIDController.java @@ -36,6 +36,18 @@ public MockPIDController() { withGains(0.0,0.0,0.0); } + @Override + public MockPIDController withTarget(double target) { + super.withTarget(target); + return this; + } + + @Override + public MockPIDController withTolerance(double tolerance) { + super.withTolerance(tolerance); + return this; + } + @Override public Set getProfiles() { return Collections.unmodifiableSet(profiles.keySet()); diff --git a/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_AnalogInputSensorTest.java b/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_AnalogInputSensorTest.java new file mode 100644 index 0000000..da4db3c --- /dev/null +++ b/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_AnalogInputSensorTest.java @@ -0,0 +1,119 @@ +/* + * Strongback + * Copyright 2015, Strongback and individual contributors by the @authors tag. + * See the COPYRIGHT.txt in the distribution for a full listing of individual + * contributors. + * + * Licensed under the MIT License; you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * http://opensource.org/licenses/MIT + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.strongback.hardware; + +import static org.fest.assertions.Assertions.assertThat; + +import org.fest.assertions.Delta; +import org.junit.Before; +import org.junit.Test; +import org.strongback.hardware.HardwareTalonSRX.AnalogInputSensor; + +public class HardwareTalonSRX_AnalogInputSensorTest { + + private static final Delta DELTA = Delta.delta(1); + + private double analogPosition = 0.0; + private double analogVelocity = 0.0; // changes in volts per cycle + private double analogRange = 1023; // 10 bit values + private double analogTurnsOverVoltageRange = 1; + private double analogVoltageRange = 3.3; + private double cyclePeriodInSeconds = 0.1; + private AnalogInputSensor sensor; + + @Before + public void beforeEach() { + } + + protected AnalogInputSensor createSensor() { + sensor = new AnalogInputSensor(() -> analogPosition, + () -> analogVelocity, + analogRange, + analogTurnsOverVoltageRange / analogVoltageRange, + analogVoltageRange, + () -> cyclePeriodInSeconds); + return sensor; + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionAndVelocityAreZero() { + analogPosition = 0; + analogTurnsOverVoltageRange = 1; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(0.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(0.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(0.0d)).isEqualTo(analogPosition, DELTA); + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionIsHalfRotationAndVelocityAreZero() { + analogPosition = 1024/2 - 1; + analogTurnsOverVoltageRange = 1; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(180.0d)).isEqualTo(analogPosition, DELTA); + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionIsOneRotationAndVelocityAreZero() { + analogPosition = 1024-1; + analogTurnsOverVoltageRange = 1; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(360.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(0.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(360.0d)).isEqualTo(analogPosition, DELTA); + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionIsOneAndOneHalfRotationAndVelocityAreZero() { + analogPosition = (int)(1024 * 1.5) - 1; + analogTurnsOverVoltageRange = 1; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(540.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(540.0d)).isEqualTo(analogPosition, DELTA); + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionIsThreeAndOneHalfRotationAndVelocityAreZero() { + analogPosition = (int)(1024 * 3.5) - 1; + analogTurnsOverVoltageRange = 1; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(1260.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(1260.0d)).isEqualTo(analogPosition, Delta.delta(5.0)); + } + + @Test + public void shouldHandleMeasuredPositionAndVelocity() { + analogPosition = (int)(1024 * 3.5) - 1; + analogVelocity = 1023; // starts from 0 and does full rotation + analogTurnsOverVoltageRange = 1; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(1260.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(360.0 / cyclePeriodInSeconds, DELTA); // degrees per second + assertThat(sensor.rawPositionForAngleInDegrees(1260.0d)).isEqualTo(analogPosition, Delta.delta(5.0)); + } + +} diff --git a/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_EncoderInputSensorTest.java b/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_EncoderInputSensorTest.java new file mode 100644 index 0000000..6423bb3 --- /dev/null +++ b/strongback-tests/src/org/strongback/hardware/HardwareTalonSRX_EncoderInputSensorTest.java @@ -0,0 +1,126 @@ +/* + * Strongback + * Copyright 2015, Strongback and individual contributors by the @authors tag. + * See the COPYRIGHT.txt in the distribution for a full listing of individual + * contributors. + * + * Licensed under the MIT License; you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * http://opensource.org/licenses/MIT + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.strongback.hardware; + +import static org.fest.assertions.Assertions.assertThat; + +import org.fest.assertions.Delta; +import org.junit.Before; +import org.junit.Test; +import org.strongback.hardware.HardwareTalonSRX.EncoderInputSensor; + +public class HardwareTalonSRX_EncoderInputSensorTest { + + private static final Delta DELTA = Delta.delta(0.00001); + + private double positionInEdges = 0.0; + private double velocityInEdges = 0.0; + private double cyclePeriodInSeconds = 0.1; + private double rotationInDegrees = 0.0; + private int encoderPulsesPerRotation = 360; + private int encoderEdgesPerPulse = 4; // 4x mode, or 4 rising edges per pulse + private EncoderInputSensor sensor; + + @Before + public void beforeEach() { + cyclePeriodInSeconds = 0.1; + } + + protected EncoderInputSensor createSensor() { + sensor = new EncoderInputSensor(() -> positionInEdges, () -> velocityInEdges, encoderPulsesPerRotation / 360.0, // pulses + // per + // degree + encoderEdgesPerPulse, () -> cyclePeriodInSeconds); + return sensor; + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionAndVelocityAreZero() { + encoderPulsesPerRotation = 360; + positionInEdges = 0.0; + velocityInEdges = 0.0; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(0.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(0.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(0.0d)).isEqualTo(positionInEdges, DELTA); + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionIsHalfRotationAndVelocityAreZero() { + encoderPulsesPerRotation = 360; + positionInEdges = 180 * 4; // edges for 180 degrees + velocityInEdges = 0.0; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(180.0d)).isEqualTo(positionInEdges, DELTA); + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionIsOneAndOneHalfRotationAndVelocityAreZero() { + encoderPulsesPerRotation = 360; + positionInEdges = (180+360) * 4; // edges for 540 degrees + velocityInEdges = 0.0; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(540.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(540.0d)).isEqualTo(positionInEdges, DELTA); + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionIsThreeAndOneHalfRotationAndVelocityAreZero() { + encoderPulsesPerRotation = 360; + positionInEdges = (180+360*3) * 4; // edges for 1260 degrees + velocityInEdges = 0.0; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(1260.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(1260.0d)).isEqualTo(positionInEdges, DELTA); + } + + @Test + public void shouldReturnZeroWhenMeasuredPositionIsThreeAndOneHalfRotationAndVelocityAreZeroWithPrecisionEncoder() { + encoderPulsesPerRotation = 250; + rotationInDegrees = 180+360*3; + positionInEdges = rotationInDegrees * encoderPulsesPerRotation /360 * 4; + velocityInEdges = 0.0; + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(1260.0d, DELTA); + assertThat(sensor.getHeading()).isEqualTo(180.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(0.0d, DELTA); + assertThat(sensor.rawPositionForAngleInDegrees(1260.0d)).isEqualTo(positionInEdges, DELTA); + } + + @Test + public void shouldHandleMeasuredPositionOfOneAndVelocityOfTwenty() { + double rotationsPerSecond = 20; + encoderPulsesPerRotation = 360; + rotationInDegrees = 360; + positionInEdges = rotationInDegrees * encoderPulsesPerRotation /360 * 4; + velocityInEdges = rotationsPerSecond * 4 * 360.0 * cyclePeriodInSeconds; // per cycle, or 28800 position units / second + createSensor(); + assertThat(sensor.getAngle()).isEqualTo(360.0, DELTA); + assertThat(sensor.getHeading()).isEqualTo(0.0d, DELTA); + assertThat(sensor.getRate()).isEqualTo(rotationsPerSecond * 360.0, DELTA); // degrees per second + assertThat(sensor.rawPositionForAngleInDegrees(360.0d)).isEqualTo(positionInEdges, DELTA); + } + +} diff --git a/strongback/src/org/strongback/components/TalonSRX.java b/strongback/src/org/strongback/components/TalonSRX.java index 669313b..9f7e8ea 100644 --- a/strongback/src/org/strongback/components/TalonSRX.java +++ b/strongback/src/org/strongback/components/TalonSRX.java @@ -297,18 +297,27 @@ public enum FeedbackDevice { /** * Use Quadrature Encoder. */ - QUADRATURE_ENCODER(0), /** - * Analog potentiometer, 0-3.3V - */ - ANALOG_POTENTIOMETER(2), /** - * Analog encoder or any other analog device, 0-3.3V - */ - ANALOG_ENCODER(3), /** - * Encoder that increments position per rising edge (and never decrements) on Quadrature-A. - */ - ENCODER_RISING(4), /** - * Encoder that increments position per falling edge (and never decrements) on Quadrature-A. - */ + QUADRATURE_ENCODER(0), + + /** + * Analog potentiometer or any other analog device, 0-3.3V + */ + ANALOG_POTENTIOMETER(2), + + /** + * Analog encoder or any other analog device, 0-3.3V + */ + ANALOG_ENCODER(3), + + /** + * Encoder that increments position per rising edge (and never decrements) on Quadrature-A. + */ + ENCODER_RISING(4), + + /** + * Encoder that increments position per falling edge (and never decrements) on Quadrature-A. + * Note: Was not supported in 2015 firmware, per the Talon SRX Software Reference Manual, section 21.3 + */ ENCODER_FALLING(5); public int value; @@ -335,7 +344,65 @@ public int value() { * Types of status frame rates. */ public enum StatusFrameRate { - GENERAL(0), FEEDBACK(1), QUADRATURE_ENCODER(2), ANALOG_TEMPERATURE_BATTERY_VOLTAGE(3); + /** + * The General Status frame has a default period of 10ms, and provides: + *

    + *
  • Closed Loop Error - the closed-loop target minus actual position/velocity.
    • + *
  • Throttle - the current 10bit motor output duty cycle (-1023 full reverse to +1023 full forward).
    • + *
  • Forward Limit Switch Pin State
    • + *
  • Reverse Limit Switch Pin State
    • + *
  • Fault bits
    • + *
  • Applied Control Mode
    • + *
+ */ + GENERAL(0), + + /** + * The Feedback Status frame has a default period of 20ms, and provides: + *
    + *
  • Sensor Position - position of the selected sensor
    • + *
  • Sensor Velocity - velocity of the selected sensor
    • + *
  • Motor Current
    • + *
  • Sticky Faults
    • + *
  • Brake Neutral State
    • + *
  • Motor Control Profile Select
    • + *
+ */ + FEEDBACK(1), + + /** + * The Quadrature Encoder Status frame has a default period of 100ms, and provides: + *
    + *
  • Encoder Position - position of the quadrature sensor
    • + *
  • Encoder Velocity - velocity of the selected sensor
    • + *
  • Number of rising edges counted on the Index Pin.
    • + *
  • Quad A pin state.
    • + *
  • Quad B pin state.
    • + *
  • Quad Index pin state.
    • + *
+ * The quadrature decoder is always engaged, whether the feedback device is selected or not, and whether a quadrature + * encoder is actually wired or not. This means that the Quadrature Encoder signals are always available in programming + * API regardless of how the Talon is used. The 100ms update rate is sufficient for logging, instrumentation and + * debugging. If a faster update rate is required the robot application can select the appropriate sensor and leverage + * the Sensor Position and Sensor Velocity sent over the {@link #FEEDBACK} frame every 20ms (by default). + */ + QUADRATURE_ENCODER(2), + + /** + * The Analog, Temperature, and Battery Voltage status frame has a default period of 100ms, and provides: + *
    + *
  • Analog Position - position of the analog sensor
    • + *
  • Analog Velocity - velocity of the analog sensor
    • + *
  • Temperature
    • + *
  • Battery Voltage
    • + *
+ * The Analog to Digital Converter is always engaged, whether the feedback device is selected or not, and whether an + * analog sensor is actually wired or not. This means that the Analog In signals are always available in programming API + * regardless of how the Talon is used. The 100ms update rate is sufficient for logging, instrumentation and debugging. + * If a faster update rate is required the robot application can select the appropriate sensor and leverage the Sensor + * Position and Sensor Velocity sent over the {@link #FEEDBACK} frame every 20ms (by default). + */ + ANALOG_TEMPERATURE_BATTERY_VOLTAGE(3); public int value; public static StatusFrameRate valueOf(int value) { diff --git a/strongback/src/org/strongback/control/PIDController.java b/strongback/src/org/strongback/control/PIDController.java index 59ff1e4..bd94514 100644 --- a/strongback/src/org/strongback/control/PIDController.java +++ b/strongback/src/org/strongback/control/PIDController.java @@ -41,6 +41,12 @@ public interface PIDController extends Controller { */ public static int DEFAULT_PROFILE = 0; + @Override + public PIDController withTarget(double target); + + @Override + public PIDController withTolerance(double tolerance); + /** * Set the control gains on this controller's current profile. * diff --git a/strongback/src/org/strongback/control/TalonController.java b/strongback/src/org/strongback/control/TalonController.java index 6f58d4d..d410bf0 100644 --- a/strongback/src/org/strongback/control/TalonController.java +++ b/strongback/src/org/strongback/control/TalonController.java @@ -172,6 +172,25 @@ public int value() { @Override public TalonController setFeedbackDevice(FeedbackDevice device); + /** + * Get the current target angle for this controller as defined by the selected input sensor in degrees. + * @return the target angle in degrees as defined by the selected input sensor + * @see #withTarget(double) + */ + @Override + public double getTarget(); + + /** + * Sets the target angle for the controller's selected input sensor in degrees. + * + * @param angleInDegrees the desired angle in degrees that this controller will use as a target as defined by the selected + * input sensor + * @return this object so that methods can be chained; never null + * @see #getTarget() + */ + @Override + public TalonController withTarget(double angleInDegrees); + @Override public TalonController setStatusFrameRate(StatusFrameRate frameRate, int periodMillis); diff --git a/strongback/src/org/strongback/hardware/Hardware.java b/strongback/src/org/strongback/hardware/Hardware.java index 9a26d0a..0617b03 100644 --- a/strongback/src/org/strongback/hardware/Hardware.java +++ b/strongback/src/org/strongback/hardware/Hardware.java @@ -567,7 +567,7 @@ public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree) { */ public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree, double analogTurnsOverVoltageRange) { CANTalon talon = new CANTalon(deviceNumber); - return new HardwareTalonSRX(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + return talonSRX(talon, pulsesPerDegree, analogTurnsOverVoltageRange); } /** diff --git a/strongback/src/org/strongback/hardware/HardwareTalonController.java b/strongback/src/org/strongback/hardware/HardwareTalonController.java index 321fd1a..0a65125 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonController.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonController.java @@ -116,12 +116,13 @@ public double getValue() { @Override public double getTarget() { - return talon.getSetpoint(); + double targetPosition = talon.getSetpoint(); + return this.selectedInput.angleInDegreesFromRawPosition(targetPosition); } @Override - public Controller withTarget(double target) { - talon.set(target); + public TalonController withTarget(double angleInDegrees) { + talon.set(this.selectedInput.rawPositionForAngleInDegrees(angleInDegrees)); return this; } @@ -131,7 +132,7 @@ public double getTolerance() { } @Override - public Controller withTolerance(double tolerance) { + public TalonController withTolerance(double tolerance) { this.tolerance = tolerance; return this; } diff --git a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java index 2e65210..81fa46c 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java @@ -40,7 +40,12 @@ @Immutable class HardwareTalonSRX implements TalonSRX { - protected static final Gyroscope NO_OP_GYRO = new Gyroscope() { + protected static interface InputSensor extends Gyroscope { + public double rawPositionForAngleInDegrees( double angle ); + public double angleInDegreesFromRawPosition( double position ); + } + + protected static final InputSensor NO_OP_SENSOR = new InputSensor() { @Override public double getAngle() { @@ -52,75 +57,146 @@ public double getRate() { return 0; } @Override - public Gyroscope zero() { - // do nothing - return this; + public InputSensor zero() { + return this; // do nothing + } + + @Override + public double rawPositionForAngleInDegrees(double angleInDegrees) { + return 0.0; + } + + @Override + public double angleInDegreesFromRawPosition(double position) { + return 0.0; } }; - protected static Gyroscope encoderGyroscope(DoubleSupplier positionInPulses, DoubleSupplier velocityInPulsesPerCycle, - double pulsesPerDegree, DoubleSupplier cyclesPeriodInSeconds) { - if ( pulsesPerDegree <= 0.0000001d && pulsesPerDegree >= 0.0000001d ) return null; - return new Gyroscope() { - private double zero = 0.0; + protected static final class EncoderInputSensor implements InputSensor { + private double zero = 0.0; + private final DoubleSupplier positionInEdges; + private final DoubleSupplier velocityInEdgesPerCycle; + private final DoubleSupplier cyclePeriodInSeconds; + private final double pulsesPerDegree; + private final double edgesPerPulse; + + protected EncoderInputSensor( DoubleSupplier positionInEdges, DoubleSupplier velocityInEdgesPerCycle, + double pulsesPerDegree, int edgesPerPulse, DoubleSupplier cyclesPeriodInSeconds ) { + this.positionInEdges = positionInEdges; + this.velocityInEdgesPerCycle = velocityInEdgesPerCycle; + this.cyclePeriodInSeconds = cyclesPeriodInSeconds; + this.pulsesPerDegree = pulsesPerDegree; + this.edgesPerPulse = edgesPerPulse; + } - @Override - public double getAngle() { - // Units: (pulses) x (degrees/pulse) = degrees - return (positionInPulses.getAsDouble() - zero) / pulsesPerDegree; - } + @Override + public double rawPositionForAngleInDegrees(double angle) { + // Units: (degrees) x (pulses/degrees) * (edges/pulses) = (edges) + double relativeInput = angle * pulsesPerDegree * edgesPerPulse; + return relativeInput + zero; + } - @Override - public double getRate() { - // Units: (pulses/cycle) x (degrees/pulse) x (cycles/second) = (degrees/second) - return velocityInPulsesPerCycle.getAsDouble() / pulsesPerDegree / cyclesPeriodInSeconds.getAsDouble(); - } + @Override + public double angleInDegreesFromRawPosition(double position) { + // Units: (edges) x (pulse/edges) x (degrees/pulse) = degrees + return (position - zero) / edgesPerPulse / pulsesPerDegree; + } - @Override - public Gyroscope zero() { - zero = positionInPulses.getAsDouble(); - return this; - } - }; + @Override + public double getAngle() { + return angleInDegreesFromRawPosition(positionInEdges.getAsDouble()); + } + + @Override + public double getRate() { + // Units: (edges/cycle) * (pulses/edge) x (degrees/pulse) x (cycles/second) = (degrees/second) + return velocityInEdgesPerCycle.getAsDouble() / edgesPerPulse / pulsesPerDegree / cyclePeriodInSeconds.getAsDouble(); + } + + @Override + public Gyroscope zero() { + zero = positionInEdges.getAsDouble(); + return this; + } } - protected static Gyroscope analogGyroscope(DoubleSupplier analogPosition, DoubleSupplier changeInVoltsPerCycle, - double analogRange, double analogTurnsPerVolt, double voltageRange, DoubleSupplier cyclesPeriodInSeconds) { - if ( analogTurnsPerVolt <= 0.0000001d && analogTurnsPerVolt >= 0.0000001d ) return null; - return new Gyroscope() { - private double zero = 0.0; + protected static final class AnalogInputSensor implements InputSensor { + private double zero = 0.0; + private final DoubleSupplier analogPosition; + private final DoubleSupplier changeInVoltsPerCycle; + private final DoubleSupplier cyclePeriodInSeconds; + private final double analogRange; + private final double analogTurnsPerVolt; + private final double voltageRange; + + protected AnalogInputSensor( DoubleSupplier analogPosition, DoubleSupplier changeInVoltsPerCycle, + double analogRange, double analogTurnsPerVolt, double voltageRange, DoubleSupplier cyclePeriodInSeconds ) { + this.analogPosition = analogPosition; + this.changeInVoltsPerCycle = changeInVoltsPerCycle; + this.cyclePeriodInSeconds = cyclePeriodInSeconds; + this.analogRange = analogRange; + this.analogTurnsPerVolt = analogTurnsPerVolt; + this.voltageRange = voltageRange; + } - @Override - public double getAngle() { - // Units: (0-1023) / 1023 x (turns/volt) x (volts) x (degrees/turn) = degrees - return (analogPosition.getAsDouble() - zero) / analogRange * analogTurnsPerVolt * voltageRange * 360.0; - } + @Override + public double rawPositionForAngleInDegrees(double angle) { + // Units: (0-1023) / 1023 x (turns/volt) x (volts) x (degrees/turn) = degrees + // Units: (degrees) x (turns/degrees) x (1/volts) x (volts/turn) * 1023 = (0-1023) + double relativeInput = angle / 360.0 / voltageRange / analogTurnsPerVolt * analogRange; + return relativeInput + zero; + } - @Override - public double getRate() { - // Units: (0-1023)/cycle / 1023 x (turns/volt) x (volts) x (degrees/turn) x (cycles/second) = (degrees/second) - return changeInVoltsPerCycle.getAsDouble() / analogRange * analogTurnsPerVolt * voltageRange * 360.0 - / cyclesPeriodInSeconds.getAsDouble(); - } + @Override + public double angleInDegreesFromRawPosition(double position) { + // Units: (0-1023) / 1023 x (turns/volt) x (volts) x (degrees/turn) = degrees + return (position - zero) / analogRange * analogTurnsPerVolt * voltageRange * 360.0; + } + @Override + public double getAngle() { + return angleInDegreesFromRawPosition(analogPosition.getAsDouble()); + } - @Override - public Gyroscope zero() { - zero = analogPosition.getAsDouble(); - return this; - } - }; + @Override + public double getRate() { + // Units: (0-1023)/cycle / 1023 x (turns/volt) x (volts) x (degrees/turn) x (cycles/second) = (degrees/second) + return changeInVoltsPerCycle.getAsDouble() / analogRange * analogTurnsPerVolt * voltageRange * 360.0 + / cyclePeriodInSeconds.getAsDouble(); + } + + @Override + public Gyroscope zero() { + zero = analogPosition.getAsDouble(); + return this; + } + } + + protected static EncoderInputSensor encoderInput(DoubleSupplier positionInPulses, DoubleSupplier velocityInPulsesPerCycle, + double pulsesPerDegree, int risesPerPulse, DoubleSupplier cyclePeriodInSeconds) { + if ( pulsesPerDegree <= 0.0000001d && pulsesPerDegree >= 0.0000001d ) return null; + return new EncoderInputSensor(positionInPulses, velocityInPulsesPerCycle, pulsesPerDegree, risesPerPulse, cyclePeriodInSeconds); + } + + protected static AnalogInputSensor analogInput(DoubleSupplier analogPosition, DoubleSupplier changeInVoltsPerCycle, + double analogRange, double analogTurnsPerVolt, double voltageRange, DoubleSupplier cyclesPeriodInSeconds) { + if ( analogTurnsPerVolt <= 0.0000001d && analogTurnsPerVolt >= 0.0000001d ) return null; + return new AnalogInputSensor(analogPosition, changeInVoltsPerCycle, analogRange, analogTurnsPerVolt, voltageRange, cyclesPeriodInSeconds); } private static final double DEFAULT_ANALOG_RATE = 0.100; private static final double DEFAULT_QUADRATURE_RATE = 0.100; private static final double DEFAULT_FEEDBACK_RATE = 0.020; + private static final int RISES_PER_PULSE = 4; // 4x mode + private static final double MAX_ANALOG_VOLTAGE = 3.3; // 0-3.3V + private static final double MAX_ANALOG_RANGE = 1023; // 10 bits non-continuous + protected final CANTalon talon; - protected final Gyroscope encoderInput; - protected final Gyroscope analogInput; - protected final Gyroscope selectedEncoderInput; - protected final Gyroscope selectedAnalogInput; - protected volatile Gyroscope selectedInput = NO_OP_GYRO; + protected final InputSensor encoderInput; + protected final InputSensor analogInput; + protected final InputSensor selectedEncoderInput; + protected final InputSensor selectedAnalogInput; + protected volatile InputSensor selectedInput = NO_OP_SENSOR; protected volatile double quadratureRateInSeconds = DEFAULT_QUADRATURE_RATE; protected volatile double analogRateInSeconds = DEFAULT_ANALOG_RATE; protected volatile double feedbackRateInSeconds = DEFAULT_FEEDBACK_RATE; @@ -142,25 +218,27 @@ public Gyroscope zero() { this.outputVoltage = talon::getOutputVoltage; this.busVoltage = talon::getBusVoltage; this.temperature = talon::getTemp; - this.encoderInput = encoderGyroscope(talon::getEncPosition, + this.encoderInput = encoderInput(talon::getEncPosition, talon::getEncVelocity, pulsesPerDegree, + RISES_PER_PULSE, () -> quadratureRateInSeconds); - this.analogInput = analogGyroscope(talon::getAnalogInPosition, + this.analogInput = analogInput(talon::getAnalogInPosition, talon::getAnalogInVelocity, - 1023, - analogTurnsOverVoltageRange / 3.3, - 3.3, + MAX_ANALOG_RANGE, + analogTurnsOverVoltageRange / MAX_ANALOG_VOLTAGE, + MAX_ANALOG_VOLTAGE, () -> analogRateInSeconds); - this.selectedEncoderInput = encoderGyroscope(talon::getPosition, + this.selectedEncoderInput = encoderInput(talon::getPosition, talon::getSpeed, pulsesPerDegree, + RISES_PER_PULSE, () -> feedbackRateInSeconds); - this.selectedAnalogInput = analogGyroscope(talon::getPosition, + this.selectedAnalogInput = analogInput(talon::getPosition, talon::getSpeed, - 1023, - analogTurnsOverVoltageRange / 3.3, - 3.3, + MAX_ANALOG_RANGE, + analogTurnsOverVoltageRange / MAX_ANALOG_VOLTAGE, + MAX_ANALOG_VOLTAGE, () -> feedbackRateInSeconds); this.instantaneousFaults = new Faults() { @Override @@ -285,21 +363,21 @@ public TalonSRX setFeedbackDevice(FeedbackDevice device) { selectedInput = selectedAnalogInput; } else { Strongback.logger(getClass()).error("Unable to use the analog input for feedback, since the Talon SRX (device " + getDeviceID() + ") was not instantiated with an analog input. Check how this device was created using Strongback's Hardware class."); - selectedInput = NO_OP_GYRO; + selectedInput = NO_OP_SENSOR; } break; case QUADRATURE_ENCODER: + case ENCODER_RISING: if ( selectedEncoderInput != null ) { selectedInput = selectedEncoderInput; } else { Strongback.logger(getClass()).error("Unable to use the quadrature encoder input for feedback, since the Talon SRX (device " + getDeviceID() + ") was not instantiated with an encoder input. Check how this device was created using Strongback's Hardware class."); - selectedInput = NO_OP_GYRO; + selectedInput = NO_OP_SENSOR; } break; case ENCODER_FALLING: - case ENCODER_RISING: // for 2015 the Talon SRX firmware did not support the falling or rising mode ... - selectedInput = NO_OP_GYRO; + selectedInput = NO_OP_SENSOR; break; } return this; @@ -320,7 +398,7 @@ public TalonSRX setStatusFrameRate(StatusFrameRate frameRate, int periodMillis) analogRateInSeconds = periodInSeconds; break; case GENERAL: - // do nothing + // nothing to set, since our code doesn't use the "general" frames break; } return this; From d6d174ef6a080e6a48393657d351daceb665d3b7 Mon Sep 17 00:00:00 2001 From: Randall Hauch Date: Mon, 16 Nov 2015 22:04:26 -0600 Subject: [PATCH 5/6] Added @Experimental tag to TalonSRX and TalonController --- .../org/strongback/components/TalonSRX.java | 13 ++++++++++-- .../strongback/control/TalonController.java | 6 ++++++ .../src/org/strongback/hardware/Hardware.java | 20 +++++++++++++++++++ 3 files changed, 37 insertions(+), 2 deletions(-) diff --git a/strongback/src/org/strongback/components/TalonSRX.java b/strongback/src/org/strongback/components/TalonSRX.java index 9f7e8ea..013b9aa 100644 --- a/strongback/src/org/strongback/components/TalonSRX.java +++ b/strongback/src/org/strongback/components/TalonSRX.java @@ -16,11 +16,18 @@ package org.strongback.components; +import org.strongback.annotation.Experimental; + /** * A motor controlled by a Talon SRX with built-in current sensor, position (angle) sensor, and optional external limit switches * wired into the SRX so that it can automatically stop the forward and reverse directions when the limit switches are * triggered. + *

+ * This class is currently experimental. It certainly works as a simple motor, but most of this interface exposes functionality + * of the Talon SRX motor controller, including various input sensors. Little beyond setting and reading the speed has been + * tested. */ +@Experimental public interface TalonSRX extends LimitedMotor { @Override @@ -315,8 +322,10 @@ public enum FeedbackDevice { ENCODER_RISING(4), /** - * Encoder that increments position per falling edge (and never decrements) on Quadrature-A. - * Note: Was not supported in 2015 firmware, per the Talon SRX Software Reference Manual, section 21.3 + * Encoder that increments position per falling edge (and never decrements) on Quadrature-A. Note: Was not supported in + * 2015 firmware, per the + * Talon SRX Software + * Reference Manual, section 21.3 */ ENCODER_FALLING(5); diff --git a/strongback/src/org/strongback/control/TalonController.java b/strongback/src/org/strongback/control/TalonController.java index d410bf0..4a6f044 100644 --- a/strongback/src/org/strongback/control/TalonController.java +++ b/strongback/src/org/strongback/control/TalonController.java @@ -16,6 +16,7 @@ package org.strongback.control; +import org.strongback.annotation.Experimental; import org.strongback.components.TalonSRX; /** @@ -85,7 +86,12 @@ * * The {@link #reverseOutput(boolean)} method can be used to invert the output of a follower Talon. This may be useful if a * follower and leader Talon are wired out of phase with each other. + *

+ * This class is currently experimental. It certainly works as a simple motor, but most of this interface exposes functionality + * of the Talon SRX motor controller, including various input sensors. Little beyond setting and reading the speed has been + * tested. */ +@Experimental public interface TalonController extends PIDController, TalonSRX { /** diff --git a/strongback/src/org/strongback/hardware/Hardware.java b/strongback/src/org/strongback/hardware/Hardware.java index 0617b03..371dec3 100644 --- a/strongback/src/org/strongback/hardware/Hardware.java +++ b/strongback/src/org/strongback/hardware/Hardware.java @@ -16,6 +16,7 @@ package org.strongback.hardware; +import org.strongback.annotation.Experimental; import org.strongback.components.Accelerometer; import org.strongback.components.AngleSensor; import org.strongback.components.DistanceSensor; @@ -547,6 +548,7 @@ public static TalonSRX talonSRX(int deviceNumber) { * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft; may be 0 if unused * @return a {@link TalonSRX} motor; never null */ + @Experimental public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree) { return talonSRX(deviceNumber, pulsesPerDegree, 0.0d); } @@ -565,11 +567,26 @@ public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree) { * be 0 if unused * @return a {@link TalonSRX} motor; never null */ + @Experimental public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree, double analogTurnsOverVoltageRange) { CANTalon talon = new CANTalon(deviceNumber); return talonSRX(talon, pulsesPerDegree, analogTurnsOverVoltageRange); } + /** + * Creates a {@link TalonSRX} motor controlled by a Talon SRX. The {@link CANTalon} object passed into this method + * should be already configured by the calling code. + *

+ * The resulting {@link TalonSRX} will have a null {@link TalonSRX#getEncoderInput()} and a null + * {@link TalonSRX#getAnalogInput()}, and the {@link TalonSRX#getSelectedSensor()} will always return 0. + * + * @param talon the already configured {@link CANTalon} instance; may not be null + * @return a {@link TalonSRX} motor; never null + */ + public static TalonSRX talonSRX(CANTalon talon) { + return talonSRX(talon, 0.0, 0.0d); + } + /** * Creates a {@link TalonSRX} motor controlled by a Talon SRX with an optional (angle) sensor. The {@link CANTalon} * object passed into this method should be already configured by the calling code. @@ -582,6 +599,7 @@ public static TalonSRX talonSRX(int deviceNumber, double pulsesPerDegree, double * @param pulsesPerDegree the number of encoder pulses per degree of revolution of the final shaft * @return a {@link TalonSRX} motor; never null */ + @Experimental public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree) { return talonSRX(talon, pulsesPerDegree, 0.0d); } @@ -601,6 +619,7 @@ public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree) { * be 0 if unused * @return a {@link TalonSRX} motor; never null */ + @Experimental public static TalonSRX talonSRX(CANTalon talon, double pulsesPerDegree, double analogTurnsOverVoltageRange) { if (talon == null) throw new IllegalArgumentException("The CANTalon reference may not be null"); return new HardwareTalonSRX(talon, pulsesPerDegree, analogTurnsOverVoltageRange); @@ -644,6 +663,7 @@ public static final class Controllers { * be 0 if unused * @return the interface for managing and using the Talon SRX hardware-based PID controller; never null */ + @Experimental public static TalonController talonController(int deviceNumber, double pulsesPerDegree, double analogTurnsOverVoltageRange) { CANTalon talon = new CANTalon(deviceNumber); From f5c0a713e9d71c74f9672e7cf1d23454e4e53a0f Mon Sep 17 00:00:00 2001 From: Randall Hauch Date: Wed, 18 Nov 2015 20:49:20 -0600 Subject: [PATCH 6/6] Changed release to be an alpha --- build.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.properties b/build.properties index caa0e72..9ac9741 100644 --- a/build.properties +++ b/build.properties @@ -1,4 +1,4 @@ -strongback.version=1.1.0-Alpha2-SNAPSHOT +strongback.version=1.1.0-Alpha3-SNAPSHOT # # The build will download a specific version of the WPILib given by the following URL # and install it into the 'libs/wpilib' folder. To use a different version of WPILib,