[Levansic]

OK. Your stuff triggered by button 5 and 6 need to move over to periodic tasks. Teleop has to finish in 20 ms or less, and you have waits in both.

Set a global variable when the button is pressed, and read the variable to trigger those actions in a timed loop in periodic.

Global variables are specified in Robot Global Data.vi.