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, diff --git a/strongback-testing/src/org/strongback/mock/Mock.java b/strongback-testing/src/org/strongback/mock/Mock.java index b582e1e..c9bd73a 100644 --- a/strongback-testing/src/org/strongback/mock/Mock.java +++ b/strongback-testing/src/org/strongback/mock/Mock.java @@ -16,7 +16,12 @@ 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; +import org.strongback.control.PIDController; /** * Factory for mock components. @@ -25,6 +30,8 @@ */ public class Mock { + private static final AtomicLong CAN_DEVICE_ID_GENERATOR = new AtomicLong(); + /** * Create a mock power panel. * @@ -230,21 +237,64 @@ public static MockMotor runningMotor(double speed) { } /** - * Create a stopped mock Talon SRX motor. + * 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()); } /** - * 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 */ 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() { + 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..ee5d355 --- /dev/null +++ b/strongback-testing/src/org/strongback/mock/MockPIDController.java @@ -0,0 +1,112 @@ +/* + * 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 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()); + } + + @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..aa8beb7 100644 --- a/strongback-testing/src/org/strongback/mock/MockTalonSRX.java +++ b/strongback-testing/src/org/strongback/mock/MockTalonSRX.java @@ -16,21 +16,41 @@ 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; +import org.strongback.components.VoltageSensor; -/** - * @author Randall Hauch - * - */ 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(); + 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) { + protected MockTalonSRX(int deviceId, double speed) { super(speed); + this.deviceId = deviceId; + } + + @Override + public int getDeviceID() { + return deviceId; } @Override @@ -40,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 @@ -49,6 +112,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 +137,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-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 5e47a6e..013b9aa 100644 --- a/strongback/src/org/strongback/components/TalonSRX.java +++ b/strongback/src/org/strongback/components/TalonSRX.java @@ -16,28 +16,474 @@ 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 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 AngleSensor getAngleSensor(); + public Gyroscope getEncoderInput(); /** - * Get the Talon SRX's current sensor. + * Get the current analog angle and rate, regardless of whether it is the current feedback device. * - * @return the current sensor; never null + * @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 Gyroscope getSelectedSensor(); + + /** + * Get the Talon SRX's output current sensor. + * + * @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 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. + * + * @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); + + /** + * 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. + * + * @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 type of feedback sensor used by this Talon controller. + */ + public enum FeedbackDevice { + /** + * Use Quadrature Encoder. + */ + 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; + + 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 { + /** + * The General Status frame has a default period of 10ms, and provides: + *

+ */ + GENERAL(0), + + /** + * The Feedback Status frame has a default period of 20ms, and provides: + * + */ + FEEDBACK(1), + + /** + * The Quadrature Encoder Status frame has a default period of 100ms, and provides: + * + * 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: + * + * 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) { + 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. + */ + 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..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. * @@ -49,7 +55,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 +68,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 +92,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..c5c1690 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 static 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..4a6f044 --- /dev/null +++ b/strongback/src/org/strongback/control/TalonController.java @@ -0,0 +1,298 @@ +/* + * 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.annotation.Experimental; +import org.strongback.components.TalonSRX; + +/** + * 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. + *

+ * 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 { + + /** + * 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; + } + } + + /** + * 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); + + @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); + + @Override + 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(); + + @Override + 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..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; @@ -32,6 +33,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; @@ -43,6 +45,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; @@ -521,16 +524,151 @@ 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 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 + */ + @Experimental + 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 + */ + @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. + *

+ * 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 */ + @Experimental 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 + */ + @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); + 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); + } + } + + /** + * 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 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 + */ + @Experimental + public static TalonController talonController(int deviceNumber, double pulsesPerDegree, + double analogTurnsOverVoltageRange) { + CANTalon talon = new CANTalon(deviceNumber); + HardwareTalonController c = new HardwareTalonController(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + return c; } } 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..0a65125 --- /dev/null +++ b/strongback/src/org/strongback/hardware/HardwareTalonController.java @@ -0,0 +1,330 @@ +/* + * 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, double analogTurnsOverVoltageRange) { + super(talon, pulsesPerDegree, analogTurnsOverVoltageRange); + profiles.add(currentProfile); + } + + @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() { + double targetPosition = talon.getSetpoint(); + return this.selectedInput.angleInDegreesFromRawPosition(targetPosition); + } + + @Override + public TalonController withTarget(double angleInDegrees) { + talon.set(this.selectedInput.rawPositionForAngleInDegrees(angleInDegrees)); + return this; + } + + @Override + public double getTolerance() { + return this.tolerance; + } + + @Override + public TalonController 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) { + super.setFeedbackDevice(device); + 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) { + super.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) { + super.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..81fa46c 100644 --- a/strongback/src/org/strongback/hardware/HardwareTalonSRX.java +++ b/strongback/src/org/strongback/hardware/HardwareTalonSRX.java @@ -16,11 +16,16 @@ package org.strongback.hardware; +import java.util.function.DoubleSupplier; + +import org.strongback.Strongback; 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; +import org.strongback.components.VoltageSensor; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CANTalon.ControlMode; @@ -33,36 +38,287 @@ * @see CANTalon */ @Immutable -final class HardwareTalonSRX implements TalonSRX { +class HardwareTalonSRX implements TalonSRX { + + 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() { + return 0; + } + + @Override + public double getRate() { + return 0; + } + @Override + 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 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 rawPositionForAngleInDegrees(double angle) { + // Units: (degrees) x (pulses/degrees) * (edges/pulses) = (edges) + double relativeInput = angle * pulsesPerDegree * edgesPerPulse; + return relativeInput + zero; + } + + @Override + public double angleInDegreesFromRawPosition(double position) { + // Units: (edges) x (pulse/edges) x (degrees/pulse) = degrees + return (position - zero) / edgesPerPulse / pulsesPerDegree; + } + + @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 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 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 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()); + } - private final CANTalon talon; - private final AngleSensor encoder; - private final Switch forwardLimitSwitch; - private final Switch reverseLimitSwitch; - private final CurrentSensor current; + @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(); + } - HardwareTalonSRX(CANTalon talon, double pulsesPerDegree) { + @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 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; + 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, double analogTurnsOverVoltageRange) { this.talon = talon; this.forwardLimitSwitch = talon::isRevLimitSwitchClosed; this.reverseLimitSwitch = talon::isFwdLimitSwitchClosed; - this.current = talon::getOutputCurrent; - this.encoder = new AngleSensor() { - private int zero = 0; + this.outputCurrent = talon::getOutputCurrent; + this.outputVoltage = talon::getOutputVoltage; + this.busVoltage = talon::getBusVoltage; + this.temperature = talon::getTemp; + this.encoderInput = encoderInput(talon::getEncPosition, + talon::getEncVelocity, + pulsesPerDegree, + RISES_PER_PULSE, + () -> quadratureRateInSeconds); + this.analogInput = analogInput(talon::getAnalogInPosition, + talon::getAnalogInVelocity, + MAX_ANALOG_RANGE, + analogTurnsOverVoltageRange / MAX_ANALOG_VOLTAGE, + MAX_ANALOG_VOLTAGE, + () -> analogRateInSeconds); + this.selectedEncoderInput = encoderInput(talon::getPosition, + talon::getSpeed, + pulsesPerDegree, + RISES_PER_PULSE, + () -> feedbackRateInSeconds); + this.selectedAnalogInput = analogInput(talon::getPosition, + talon::getSpeed, + MAX_ANALOG_RANGE, + analogTurnsOverVoltageRange / MAX_ANALOG_VOLTAGE, + MAX_ANALOG_VOLTAGE, + () -> feedbackRateInSeconds); + 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 double getAngle() { - return (talon.getEncPosition() - zero) / pulsesPerDegree; + public Switch hardwareFailure() { + return () -> talon.getFaultHardwareFailure() != 0; } @Override - public AngleSensor zero() { - zero = talon.getEncPosition(); - return this; + public Switch overTemperature() { + return () -> talon.getFaultOverTemp() != 0; + } + + @Override + public Switch underVoltage() { + return () -> talon.getFaultUnderVoltage() != 0; + } + }; + this.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 + public int getDeviceID() { + return talon.getDeviceID(); + } + @Override public double getSpeed() { talon.changeControlMode(ControlMode.PercentVbus); @@ -83,8 +339,75 @@ 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: + 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_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_SENSOR; + } + break; + case ENCODER_FALLING: + // for 2015 the Talon SRX firmware did not support the falling or rising mode ... + selectedInput = NO_OP_SENSOR; + break; + } + return this; + } + + @Override + public TalonSRX setStatusFrameRate(StatusFrameRate frameRate, int periodMillis) { + talon.setStatusFrameRateMs(edu.wpi.first.wpilibj.CANTalon.StatusFrameRate.valueOf(frameRate.value()), periodMillis); + double periodInSeconds = periodMillis / 1000.0; + switch(frameRate) { + case FEEDBACK: + feedbackRateInSeconds = periodInSeconds; + break; + case QUADRATURE_ENCODER: + quadratureRateInSeconds = periodInSeconds; + break; + case ANALOG_TEMPERATURE_BATTERY_VOLTAGE: + analogRateInSeconds = periodInSeconds; + break; + case GENERAL: + // nothing to set, since our code doesn't use the "general" frames + break; + } + return this; + } + + @Override + public TalonSRX reverseSensor(boolean flip) { + talon.reverseSensor(flip); + return this; } @Override @@ -99,6 +422,123 @@ 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 TalonSRX setVoltageRampRate(double rampRate) { + talon.setVoltageRampRate(rampRate); + 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