@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testSetMaxIntegralError() { YoVariableRegistry registry = new YoVariableRegistry("mike"); PIDController pid = new PIDController("", registry); pid.setMaxIntegralError(5.0); assertEquals(5.0, pid.getMaxIntegralError(), 0.001); }
public ValkyrieRosControlEffortJointControlCommandCalculator(YoEffortJointHandleHolder yoEffortJointHandleHolder, double torqueOffset, double controlDT, YoVariableRegistry parentRegistry) { this.yoEffortJointHandleHolder = yoEffortJointHandleHolder; this.controlDT = controlDT; String pdControllerBaseName = yoEffortJointHandleHolder.getName(); YoVariableRegistry registry = new YoVariableRegistry(pdControllerBaseName + "Command"); pidController = new PIDController(pdControllerBaseName + "LowLevelControl", registry); tauOffset = new YoDouble("tau_offset_" + pdControllerBaseName, registry); pidController.setMaxIntegralError(50.0); pidController.setCumulativeError(0.0); tauOffset.set(torqueOffset); parentRegistry.addChild(registry); }
public void setGains(PIDGainsReadOnly gains) { pdController.setGains(gains); setMaximumOutputLimit(gains.getMaximumFeedback()); setIntegralLeakRatio(gains.getIntegralLeakRatio()); setIntegralGain(gains.getKi()); setMaxIntegralError(gains.getMaxIntegralError()); }
jointPositionController.setProportionalGain(15.0 * subtreeMass); jointPositionController.setIntegralGain(35.0 * subtreeMass); jointPositionController.setMaxIntegralError(0.3); jointPositionController.setIntegralLeakRatio(integralLeakRatio); jointPositionController.setDerivativeGain(2.0 * subtreeMass); jointVelocityController.setMaxIntegralError(0.3); jointVelocityController.setIntegralLeakRatio(integralLeakRatio); jointVelocityController.setMaximumOutputLimit(40.0 * subtreeMass); jointPositionController.setProportionalGain(145.0 * subtreeMass); jointPositionController.setIntegralGain(9.0 * 50.0 * subtreeMass); jointPositionController.setMaxIntegralError(0.2); jointPositionController.setIntegralLeakRatio(integralLeakRatio); jointPositionController.setDerivativeGain(4.5 * subtreeMass); jointVelocityController.setMaxIntegralError(0.2); jointVelocityController.setIntegralLeakRatio(integralLeakRatio); jointVelocityController.setMaximumOutputLimit(3.5 * subtreeMass); jointPositionController.setMaxIntegralError(0.2); jointPositionController.setIntegralLeakRatio(integralLeakRatio); jointPositionController.setDerivativeGain(200.0); jointVelocityController.setMaxIntegralError(0.2); jointVelocityController.setIntegralLeakRatio(integralLeakRatio); jointVelocityController.setMaximumOutputLimit(400.0); jointPositionController.setMaxIntegralError(0.2);
jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass); jointController.setMaxIntegralError(0.3); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(2.0 * subtreeMass); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(200.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0);
jointController.setProportionalGain(15.0 * subtreeMass); jointController.setIntegralGain(35.0 * subtreeMass); jointController.setMaxIntegralError(0.3); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(2.0 * subtreeMass); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(200.0); jointController.setMaxIntegralError(0.2); jointController.setIntegralLeakRatio(integralLeakRatio); jointController.setDerivativeGain(500.0);
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout=300000) public void testCompute_all_PID() { PIDController pid = new PIDController("", null); pid.setProportionalGain(2.0); pid.setIntegralGain(3.0); pid.setDerivativeGain(4.0); pid.setMaxIntegralError(10.0); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 2.0; assertEquals((10.0 + 1.5 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((10.0 + 3.0 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((10.0 + 18.0 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.0), 0.001); // tests max integration error assertEquals((10.0 + 30.0 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.01), 0.001); }
@ContinuousIntegrationTest(estimatedDuration = 0.3) @Test(timeout=300000) public void testCompute_all_PID_withDeadband() { PIDController pid = new PIDController("", null); pid.setProportionalGain(2.0); pid.setIntegralGain(3.0); pid.setDerivativeGain(4.0); pid.setMaxIntegralError(10.0); pid.setPositionDeadband(1.5); double currentPosition = 0.0; double desiredPosition = 5.0; double currentRate = 0.0; double desiredRate = 2.0; assertEquals((7.0 + 1.05 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((7.0 + 2.1 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 0.1), 0.001); assertEquals((7.0 + 12.6 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 1.0), 0.001); // tests max integration error assertEquals((7.0 + 30.0 + 8.0), pid.compute(currentPosition, desiredPosition, currentRate, desiredRate, 3.0), 0.001); }
pid2.setIntegralGain(integral); pid2.setDerivativeGain(derivative); pid2.setMaxIntegralError(maxIntegral);