[Greg McKaskle]

I didn't try to run your code, but just looked at it.

Please run the code and give some indication of CPU usage from the DS chart. If your usage is high while the robot is disabled, your periodic tasks may be running too fast or doing too much.

If teleOp has high CPU usage when the panel is open, it is because you have lots of debug info shown and updated frequently. When you run without opening the panel, it should be far better, and will be better still when built and deployed.

Don't forget that you can also put the Support Code/Elapsed Times into your teleOp code to see how often it runs. Your log file will also how how many of your teleOp control packets are not being processed.

Please post some more data and I'll be happy to make a suggestion.
Greg McKaskle