I’m glad you got it, but I’m gonna give a hypothetical of how I would tackle something like this, just for anyone else who may stumble through this thread!
So I don’t know exactly what this variable you are using is, but I’m gonna take a guess that its the position for an arm? Perhaps the input you’re passing to a PID Controller in your Shoulder subsystem or something along that line.
First I’d setup the Subsystem to track this position in a variable (3468 absolutely does this when we use the SparkMax PID Controller as there is no way to get the last setpoint/reference back out once you set it…). Then, add some methods to be able to read and manipulate this setpoint. I’m not gonna specify anything explicit about what PID Controller Class or config I’m using, just to keep this vauge as it would work generally for a WPILib PIDController object or PID Subsystem, or the PID Object from either CTRE or REV for their respective Motor Controllers.
public class Shoulder extends SubsystemBase {
private double m_setpoint = Constants.ShoulderConstants.kStartingPosition;
private ... m_pidConroller = ...;
...
public double getSetpoint() {
return m_setpoint;
}
public void setSetpoint(double setpoint) {
m_setpoint = setpoint;
m_pidController.setReference(m_setpoint);
}
...
}
Now, we can have our commands use this! In your example, I am going to assume that your “Manual” strings modified the position by some amount (added/subtracted Y from the current position setpoint) and that “Low” and the corresponding Medium/High/Intake/Shelf/whatever were setting explicit preset positions to go to. For this, I’d create to commands.
- ShoulderSetPosition - where you pass in an exact position to go to
- ShoulderAdjustPosition - where you pass in a delta to apply to the position
These would look a little something like:
public class ShoulderSetPosition extends CommandBase {
private Shoulder m_subsystem;
private DoubleSupplier m_setpoint;
public ShoulderSetPosition(DoubleSupplier setpoint, Shoulder subsystem) {
m_subsystem = subsystem;
m_setpoint = setpoint;
addRequirements(m_subsystem);
}
...
public void execute() {
m_subsystem.setSetpoint(m_setpoint.getAsDouble());
}
...
}
public class ShoulderAdjustPosition extends CommandBase {
private Shoulder m_subsystem;
private DoubleSupplier m_delta;
public ShoulderSetPosition(DoubleSupplier delta, Shoulder subsystem) {
m_subsystem = subsystem;
m_delta = delta;
addRequirements(m_subsystem);
}
...
public void execute() {
// This will continuously adjust your setpoint by the delta every run of the scheduler (20ms)
// if you want a version that only adjusted it once per scheduling of the command
// put this in the initialize method
double setpoint = m_subsystem.getSetpoint();
m_subsystem.setSetpoint(setpoint+m_delta.getAsDouble());
}
...
}
Finally, you should be able to use these commands in your RobotContainer without worrying about your position going out of sync.
m_downButton.whileTrue(new ShoulderAdjustPosition(() -> Constants.ShoulderConstants.kManualDownDelta, m_shoulder));
m_upButton.whileTrue(new ShoulderAdjustPosition(() -> Constants.ShoulderConstants.kManualUpDelta, m_shoulder));
m_rightBumper.and(m_aButton).onTrue(new ShoulderSetPosition(() -> Constants.ShoulderConstants.kLowPosition, m_shoulder));
Now just to clear up anything about my approach to using Constants.ShoulderConstants all over the place, I would normally import the Constants class and then just use ShoulderConstants.k<variable> to make it shorter, but left it explicit in all uses here. I’d have these values setup in the Constants class like so:
public final class Constants {
...
public static class ShoulderConstants {
public static final double kStartingPosition = ...;
public static final double kLowPosition = ...;
...
public static final double kDownDelta = -10; // using numbers from your example
public static final double kUpDelta = 10;
...
}
...
}
Also, even though I am passing constants here, the reason I used DoubleSuppliers in my commands is so I have the flexibility to re-use the same commands if for instance I write a method that uses a sensor (like a camera) or something to figure out the setpoint dynamically I would use that as the input (though this is probably more common for Velocity PID Controllers in a Shooter subsystem with a flywheel than a Position subsystem though I still use this convention for consistency in my code design so you always know “setpoint input is going to be a DoubleSupplier” regardless), or if I used a joystick axis as the delta. Some examples:
new ShoulderSetPosition(() -> m_camera.getAutoShoulderPosition(), m_shoulder);
new ShoulderAdjustPosition(() -> m_controller.getLeftYAxis(), m_shoulder);
