Command Not Working As Intended

[SeebeckZachary]

Hey all, so I think I’m dealing with a weird race condition here, and I need another set of eyes on this.

So here’s the intention. We want when a button is pressed, our intake to tilt to a specific position, then shoot the note through our shooter. It’s supposed to be waiting for the intake to get to it’s given position. Sometimes, this works properly. Sometimes, it immediately shoots the note out.

For code, the command starts in RobotContainer.java at line 119. The code is here.

For context. m_pickupSubsytem is our tilting arm, and m_shooterSubsystem is our actual shooter. We run the shooter during the entire command, so that way it has time to spin up, but the intake is what holds the note until it’s shot.

Any help would be greatly appreciated, and if there’s any other context you need from me, just let me know. Thanks all!

[Waz]

From PickupSubsystem, this looks wrong…

  public boolean InWithinRange() {
    return wantedTiltPosition >= tiltEncoder.get() + (wantedTiltPosition + .01) 
      || wantedTiltPosition <= tiltEncoder.get() + (wantedTiltPosition - .01);
  }

You appear to be checking for out of bounds… is above range OR is it below range. If I am reading this right, it will return true when out of range and false when in range.

[NewtonCrosby]

I would suggest you simplify your boolean conditions to track down what is going on.

Your periodic method actually has the ability to set isPosition to false and true in the same loop. That’s probably not your intended behavior.

// Check to see of we are moving the tilt in, if so, ignore the lower deadzone.
    if (
      MathUtil.clamp(pidOutput, -0.5, 0.5) < -0.3 &&
      !(tiltEncoder.getAbsolutePosition() < Constants.Intake.TiltPIDCutoffPositions.CutoffIn)
    ) {
      SmartDashboard.putString("Tilt Movement Position", "Move In");
      isAtPosition = false;
      m_Tilt.set(MathUtil.clamp(pidOutput, -0.5, 0.5));
    }

    if (
      MathUtil.clamp(pidOutput, -0.5, 0.5) > 0.3 &&
      !(tiltEncoder.getAbsolutePosition() > Constants.Intake.TiltPIDCutoffPositions.CutoffOut)
    ) {
      SmartDashboard.putString("Tilt Movement Position", "Move Out");
      isAtPosition = false;
      m_Tilt.set(MathUtil.clamp(pidOutput, -0.5, 0.5));
    }

    if ( 
      !(
        MathUtil.clamp(pidOutput, -0.5, 0.5) < -0.3 &&
        !(tiltEncoder.getAbsolutePosition() < Constants.Intake.TiltPIDCutoffPositions.CutoffIn)
        ) &&
      !(
        MathUtil.clamp(pidOutput, -0.5, 0.5) > 0.3 &&
        !(tiltEncoder.getAbsolutePosition() > Constants.Intake.TiltPIDCutoffPositions.CutoffOut)
      )
    ) {
      SmartDashboard.putString("Tilt Movement Position", "Don't Move");
      isAtPosition = true;
      m_Tilt.set(0);
    }

I think you’d probably have simpler logic if you used the referenced setpoint in your pid controller with a tolerance.

public boolean inPosition() {
    return tiltPID.atSetpoint();
}

Something like that.