PID & One Second Delay Between Auto and TeleOp

[WillNess]

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);
   }
}

Except during teleopInit() I think I'd have to set the teleopAfterAutonomous = false. Because if I do the practice mode and then I start teleop the teleopAfterAutonomous would be true and then would go to the docked positions so it would look like this: (Correct me if I'm wrong)

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