[WillNess]

Thank you, I think this should work!

Originally Posted by MrRoboSteve

I think this is closer to what you want. The logic is simpler if you move the timer check into disabledPeriodic(). The result of timer.getFPGATimestamp() is probably not what you want.

bool teleopAfterAutonomous = false;

autonomousInit() {
  teleopAfterAutonomous = true;
}

disabledInit(){
  disabledTimer.Reset();
  disabledTimer.Start();
}

disabledPeriodic(){
  if (disabledTimer.Get() > 1) {
    // If we're disabled for more than one second, assume that we are not in a match
    // Might be necessary to bump 1 above to a somewhat higher value.    
    teleopAfterAutonomous = false;
  }
}

teleopInit(){
   if(teleopAfterAutonomous || DriverStation.getInstance().isFMSAttached()){
      shoulder.set(shoulderDocked);
      elbow.set(elbowDocked);
   }
}