Skip to content

Commit

Permalink
General good behavior of Arm and Shooter (#108)
Browse files Browse the repository at this point in the history 
* Shooter PID tuning

* Clean up properties

* Arm PID tuning and power limits

* updating defaults

* Fix bugs and tests

* use newer commonlib

* PR feedback
  • Loading branch information
@JohnGilb
JohnGilb authored Feb 19, 2024
1 parent 934f3d0 commit 67fe5d8
Show file tree
Hide file tree
Showing 20 changed files with 342 additions and 134 deletions.
2 changes: 1 addition & 1 deletion SeriouslyCommonLib
Submodule SeriouslyCommonLib updated 9 files
+8 −3 src/main/java/edu/wpi/first/wpilibj/MockDigitalInput.java
+1 −1 src/main/java/xbot/common/command/BaseMaintainerCommand.java
+5 −3 src/main/java/xbot/common/controls/sensors/XDigitalInput.java
+6 −3 src/main/java/xbot/common/controls/sensors/wpi_adapters/DigitalInputWPIAdapter.java
+2 −2 src/main/java/xbot/common/properties/BooleanProperty.java
+2 −2 src/main/java/xbot/common/properties/DoubleProperty.java
+2 −2 src/main/java/xbot/common/properties/StringProperty.java
+5 −3 src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java
+1 −1 src/test/java/xbot/common/injection/factories/TestAllFactoryClasses.java
18 changes: 16 additions & 2 deletions src/main/java/competition/operator_interface/OperatorCommandMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,9 @@
import competition.subsystems.schoocher.commands.EjectScoocherCommand;
import competition.subsystems.schoocher.commands.IntakeScoocherCommand;
import competition.subsystems.shooter.ShooterWheelSubsystem;
import competition.subsystems.shooter.ShooterWheelTargetSpeeds;
import competition.subsystems.shooter.commands.WarmUpShooterCommand;
import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
Expand Down Expand Up @@ -54,7 +56,10 @@ public void setupFundamentalCommands(
WarmUpShooterCommand shooterWarmUp,
FireCollectorCommand fireCollectorCommand,
SetArmAngleCommand armAngle,
ArmMaintainerCommand armMaintainer
ArmMaintainerCommand armMaintainer,
WarmUpShooterCommand warmUpShooterSlow,
WarmUpShooterCommand warmUpShooterFast,
WarmUpShooterRPMCommand warmUpShooterDifferentialRPM
) {
// Scooch
oi.operatorGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(scoocherIntakeProvider.get());
Expand All @@ -74,6 +79,15 @@ public void setupFundamentalCommands(
var scoochNote = scoocherIntakeProvider.get();
scoochNote.alongWith(armAngle);
// TODO: bind scoochNote action to a button in operatorGamepad

warmUpShooterSlow.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SAFE);
warmUpShooterFast.setTargetRpm(ShooterWheelSubsystem.TargetRPM.NEARSHOT);

warmUpShooterDifferentialRPM.setTargetRpm(new ShooterWheelTargetSpeeds(1000, 2000));

oi.operatorGamepad.getPovIfAvailable(270).whileTrue(warmUpShooterSlow);
oi.operatorGamepad.getPovIfAvailable(90).whileTrue(warmUpShooterFast);
oi.operatorGamepad.getPovIfAvailable(0).whileTrue(warmUpShooterDifferentialRPM);
}

// Example for setting up a command to fire when a button is pressed:
Expand Down Expand Up @@ -199,7 +213,7 @@ public void setupArmPIDCommands(
oi.operatorGamepadSecond.getXboxButton(XboxButton.X).onTrue(homeArm);
oi.operatorGamepadSecond.getXboxButton(XboxButton.B).onTrue(highArm);

oi.operatorGamepadSecond.getXboxButton(XboxButton.Start).onTrue(calibrateArmsManuallyCommand);
oi.operatorGamepad.getXboxButton(XboxButton.Start).onTrue(calibrateArmsManuallyCommand);
}

private SwerveSimpleTrajectoryCommand createAndConfigureTypicalSwerveCommand(
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/competition/simulation/Simulator2024.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ public void update() {
}

// The shooter wheel should pretty much always be in velocity mode.
var shooterMockMotor = (MockCANSparkMax)shooter.leader;
var shooterMockMotor = (MockCANSparkMax)shooter.upperWheelMotor;
shooterVelocityCalculator.add(shooterMockMotor.getReference());
if (shooterMockMotor.getControlType() == CANSparkBase.ControlType.kVelocity) {
shooterMockMotor.setVelocity(shooterVelocityCalculator.getAverage());
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/competition/subsystems/NeoTrellisGamepadSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,8 +61,9 @@ public void fillButtonColors(Color8Bit color) {

@Override
public void refreshDataFrame() {
aKitLog.record("ButtonColors",
buttonColors.stream().map(Color8Bit::toHexString).toArray(String[]::new));
// Temporarily commenting out, as we are running into issues with too many properties on NetworkTables
//aKitLog.record("ButtonColors",
// buttonColors.stream().map(Color8Bit::toHexString).toArray(String[]::new));
}

/**
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import competition.subsystems.schoocher.ScoocherSubsystem;
import competition.subsystems.schoocher.commands.StopScoocherCommand;
import competition.subsystems.shooter.ShooterWheelSubsystem;
import competition.subsystems.shooter.ShooterWheelTargetSpeeds;
import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand;
import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand;
import xbot.common.injection.swerve.FrontLeftDrive;
Expand Down Expand Up @@ -53,7 +54,7 @@ public void setupShooterWheelSubsystem(ShooterWheelSubsystem shooterWheelSubsyst
ShooterWheelMaintainerCommand command,
WarmUpShooterRPMCommand setToZero) {
shooterWheelSubsystem.setDefaultCommand(command);
setToZero.setTargetRpm(0.0);
setToZero.setTargetRpm(new ShooterWheelTargetSpeeds(0.0, 0.0));
shooterWheelSubsystem.getSetpointLock().setDefaultCommand(setToZero);
}
}
122 changes: 83 additions & 39 deletions src/main/java/competition/subsystems/arm/ArmSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import xbot.common.controls.actuators.XCANSparkMax;
import xbot.common.controls.actuators.XSolenoid;
import xbot.common.controls.sensors.XSparkAbsoluteEncoder;
import xbot.common.controls.sensors.XTimer;
import xbot.common.math.MathUtils;
import xbot.common.properties.DoubleProperty;
import xbot.common.properties.PropertyFactory;
Expand All @@ -33,28 +34,30 @@ public class ArmSubsystem extends BaseSetpointSubsystem<Double> implements DataF

public ArmState armState;

public final DoubleProperty extendPower;
public final DoubleProperty retractPower;
public double extendPower;
public double retractPower;

private final DoubleProperty powerMax;
private final DoubleProperty powerMin;
public final DoubleProperty powerMax;
public final DoubleProperty powerMin;


public final DoubleProperty extensionMmPerRevolution; // Millimeters
private double armMotorLeftRevolutionOffset; // # of revolutions
private double armMotorRightRevolutionOffset;
public final DoubleProperty upperLimitInMm;
public final DoubleProperty upperLegalLimitMm;
public final DoubleProperty absoluteEncoderOffset;
public final DoubleProperty absoluteEncoderRevolutionsPerArmDegree;
public final DoubleProperty softUpperLimitInMm;
public final DoubleProperty softLowerLimitInMm;
public final DoubleProperty softUpperLimitSpeed;
public final DoubleProperty softLowerLimitSpeed;
public final DoubleProperty speedLimitForNotCalibrated;
public final DoubleProperty upperSlowZoneThresholdMm;
public final DoubleProperty lowerSlowZoneThresholdMm;
public final DoubleProperty lowerExtremelySlowZoneThresholdMm;
public final DoubleProperty upperSlowZonePowerLimit;
public final DoubleProperty lowerSlowZonePowerLimit;
public final DoubleProperty lowerExtremelySlowZonePowerLimit;
public final DoubleProperty powerLimitForNotCalibrated;
public final DoubleProperty angleTrim;
boolean hasCalibratedLeft;
boolean hasCalibratedRight;
private final DoubleProperty maximumExtensionDesyncInMm;
private final DoubleProperty maximumExtensionDesyncMm;

private double targetExtension;
private final DoubleProperty overallPowerClampForTesting;
Expand All @@ -65,6 +68,10 @@ public class ArmSubsystem extends BaseSetpointSubsystem<Double> implements DataF
// what angle does the arm make with the pivot when it's at our concept of zero?
public final double armPivotAngleAtArmAngleZero = 45;

private double timeSinceNewTarget = -Double.MAX_VALUE;
private final DoubleProperty powerRampDurationSec;
private boolean powerRampingEnabled = true;

public enum ArmState {
EXTENDING,
RETRACTING,
Expand Down Expand Up @@ -97,14 +104,14 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
pf.setPrefix(this);
this.contract = contract;
setArmBrakeSolenoid(false);
extendPower = pf.createPersistentProperty("ExtendPower", 0.1);
retractPower = pf.createPersistentProperty("RetractPower", 0.1);
extendPower = 0.1;
retractPower = -0.1;

powerMax = pf.createPersistentProperty("PowerMax", 0.5);
powerMin = pf.createPersistentProperty("PowerMin", -0.3);
powerMax = pf.createPersistentProperty("PowerMax", 0.45);
powerMin = pf.createPersistentProperty("PowerMin", -0.25);

extensionMmPerRevolution = pf.createPersistentProperty("ExtensionMmPerRevolution", 5.715352326);
upperLimitInMm = pf.createPersistentProperty("UpperLimitInMm", 250);
upperLegalLimitMm = pf.createPersistentProperty("UpperLegalLimitMm", 238);

angleTrim = pf.createPersistentProperty("AngleTrim", 0);

Expand All @@ -113,18 +120,24 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
absoluteEncoderRevolutionsPerArmDegree = pf.createPersistentProperty(
"AbsoluteEncoderRevolutionPerArmDegree", 1);

softLowerLimitInMm = pf.createPersistentProperty(
"SoftLowerLimit", upperLimitInMm.get() * 0.15);
softUpperLimitInMm = pf.createPersistentProperty(
"SoftUpperLimit", upperLimitInMm.get() * 0.85);
softLowerLimitSpeed = pf.createPersistentProperty("SoftLowerLimitSpeed", -0.05);
softUpperLimitSpeed = pf.createPersistentProperty("SoftUpperLimitSpeed", 0.10);
upperSlowZoneThresholdMm = pf.createPersistentProperty(
"UpperSlowZoneThresholdMm", upperLegalLimitMm.get() * 0.85);
lowerSlowZoneThresholdMm = pf.createPersistentProperty(
"LowerSlowZoneThresholdMm", upperLegalLimitMm.get() * 0.15);
lowerExtremelySlowZoneThresholdMm = pf.createPersistentProperty(
"LowerExtremelySlowZoneThresholdMm", upperLegalLimitMm.get() * 0.05);

upperSlowZonePowerLimit = pf.createPersistentProperty("UpperSlowZonePowerLimit", 0.10);
lowerSlowZonePowerLimit = pf.createPersistentProperty("LowerSlowZonePowerLimit", -0.05);
lowerExtremelySlowZonePowerLimit = pf.createPersistentProperty("LowerExtremelySlowZonePowerLimit", -0.02);

speedLimitForNotCalibrated = pf.createPersistentProperty(
"SpeedLimitForNotCalibrated", -0.1);
powerLimitForNotCalibrated = pf.createPersistentProperty(
"PowerLimitForNotCalibrated", -0.02);

overallPowerClampForTesting = pf.createPersistentProperty("overallTestingPowerClamp", 0.3);
maximumExtensionDesyncInMm = pf.createPersistentProperty("MaximumExtensionDesyncInMm", 5);
overallPowerClampForTesting = pf.createPersistentProperty("overallTestingPowerClamp", 0.45);
maximumExtensionDesyncMm = pf.createPersistentProperty("MaximumExtensionDesyncMm", 0.5);

powerRampDurationSec = pf.createPersistentProperty("PowerRampDurationSec", 0.5);

hasCalibratedLeft = false;
hasCalibratedRight = false;
Expand All @@ -149,9 +162,9 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa

// Enable hardware limits
armMotorLeft.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
armMotorLeft.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
armMotorLeft.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, false);
armMotorRight.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
armMotorRight.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
armMotorRight.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, false);

armMotorLeft.setIdleMode(CANSparkBase.IdleMode.kBrake);
armMotorRight.setIdleMode(CANSparkBase.IdleMode.kBrake);
Expand All @@ -170,10 +183,14 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
}

public double constrainPowerIfNearLimit(double power, double actualPosition) {
if (actualPosition >= softUpperLimitInMm.get()) {
power = MathUtils.constrainDouble(power, powerMin.get(), softUpperLimitSpeed.get());
} else if (actualPosition <= softLowerLimitInMm.get()) {
power = MathUtils.constrainDouble(power, softLowerLimitSpeed.get(), powerMax.get());
if (actualPosition >= upperLegalLimitMm.get()) {
power = MathUtils.constrainDouble(power, powerMin.get(), 0);
} else if (actualPosition >= upperSlowZoneThresholdMm.get()) {
power = MathUtils.constrainDouble(power, powerMin.get(), upperSlowZonePowerLimit.get());
} else if (actualPosition <= lowerSlowZoneThresholdMm.get() && actualPosition > lowerExtremelySlowZoneThresholdMm.get()) {
power = MathUtils.constrainDouble(power, lowerSlowZonePowerLimit.get(), powerMax.get());
} else if (actualPosition <= lowerExtremelySlowZoneThresholdMm.get()) {
power = MathUtils.constrainDouble(power, lowerExtremelySlowZonePowerLimit.get(), powerMax.get());
}
return power;
}
Expand Down Expand Up @@ -213,7 +230,7 @@ public void setPowerToLeftAndRightArms(double leftPower, double rightPower) {
// If we have to make any changes to power, do so by a factor proportional to the maximum
// allowed desync in mm. At 50% of the desync, it would restrict power by 50%.

double potentialReductionFactor = Math.max(0, 1 - Math.abs(distanceLeftAhead) / maximumExtensionDesyncInMm.get());
double potentialReductionFactor = Math.max(0, 1 - Math.abs(distanceLeftAhead) / maximumExtensionDesyncMm.get());
aKitLog.record("PotentialReductionFactor", potentialReductionFactor);

// If left arm is ahead
Expand Down Expand Up @@ -249,7 +266,7 @@ else if (distanceLeftAhead < 0) {

// Next, a sanity check; if we have been grossly misconfigured to where the
// max/min powers are out of bounds (e.g. a max smaller than min), freeze the arm entirely.
if (powerMax.get() < 0 || powerMin.get() > 0 || speedLimitForNotCalibrated.get() > 0) {
if (powerMax.get() < 0 || powerMin.get() > 0 || powerLimitForNotCalibrated.get() > 0) {
armMotorLeft.set(0);
armMotorRight.set(0);
if (!unsafeMinOrMax) {
Expand All @@ -262,8 +279,8 @@ else if (distanceLeftAhead < 0) {

// If not calibrated, motor can only go down at slow rate since we don't know where we are.
if (!(hasCalibratedLeft && hasCalibratedRight)) {
leftPower = MathUtils.constrainDouble(leftPower, speedLimitForNotCalibrated.get(), 0);
rightPower = MathUtils.constrainDouble(rightPower, speedLimitForNotCalibrated.get(), 0);
leftPower = MathUtils.constrainDouble(leftPower, powerLimitForNotCalibrated.get(), 0);
rightPower = MathUtils.constrainDouble(rightPower, powerLimitForNotCalibrated.get(), 0);
}

// If calibrated, but near limits, slow the system down a bit so we
Expand All @@ -286,6 +303,17 @@ else if (distanceLeftAhead < 0) {
leftPower = MathUtils.constrainDouble(leftPower, powerMin.get(), powerMax.get());
rightPower = MathUtils.constrainDouble(rightPower, powerMin.get(), powerMax.get());

// Try to ramp power - a smoother start will definitely help reduce high current shock loads,
// and may reduce instability if both sides can get up to "cruise speed" together
if (powerRampingEnabled && powerRampDurationSec.get() > 0) {
double timeSince = XTimer.getFPGATimestamp() - timeSinceNewTarget;
if (timeSince < powerRampDurationSec.get()) {
double rampFactor = timeSince / powerRampDurationSec.get();
leftPower *= rampFactor;
rightPower *= rampFactor;
}
}

if (contract.isArmReady()) {
armMotorLeft.set(leftPower);
armMotorRight.set(rightPower);
Expand All @@ -294,10 +322,18 @@ else if (distanceLeftAhead < 0) {
//brake solenoid
public void setArmBrakeSolenoid(boolean on){armBrakeSolenoid.setOn(on);}

double previousPower;

@Override
public void setPower(Double power) {

if (previousPower == 0 && power != 0) {
initializeRampingPowerTarget();
}

aKitLog.record("RequestedArmPower", power);
setPowerToLeftAndRightArms(power, power);
previousPower = power;
}

public void dangerousManualSetPowerToBothArms(double power) {
Expand All @@ -306,12 +342,12 @@ public void dangerousManualSetPowerToBothArms(double power) {
}

public void extend() {
setPower(extendPower.get());
setPower(extendPower);
armState = ArmState.EXTENDING;
}

public void retract() {
setPower(retractPower.get());
setPower(retractPower);
armState = ArmState.RETRACTING;
}

Expand Down Expand Up @@ -466,6 +502,14 @@ public void setClampLimit(double clampPower) {
overallPowerClampForTesting.set(Math.abs(clampPower));
}

public void initializeRampingPowerTarget() {
timeSinceNewTarget = XTimer.getFPGATimestamp();
}

public void setRampingPowerEnabled(boolean enabled) {
powerRampingEnabled = enabled;
}

public void periodic() {
if (contract.isArmReady()) {
recordArmEncoderValues();
Expand Down Expand Up @@ -503,7 +547,7 @@ public double getAngleFromRange() {
return getArmAngleFromDistance(pose.getDistanceFromSpeaker());
}

public void calibrateArmsManually() {
public void markArmsAsCalibratedAgainstLowerPhyscalLimit() {
hasCalibratedLeft = true;
armMotorLeftRevolutionOffset = -armMotorLeft.getPosition();
hasCalibratedRight = true;
Expand Down
Loading

0 comments on commit 67fe5d8

Please sign in to comment.