Command to turn to angle

[rwodonnell]

Does anyone have some sample code that turns to a specified angle, implemented as a command? We have done it in Robot.java (https://github.com/BHSRobotix/BenchtopForShowbot/blob/master/BenchtopForShowbot/src/org/usfirst/frc/team2876/robot/Robot.java) but our first attempt to do it as a command (https://github.com/BHSRobotix/Stronghold2016/blob/master/src/org/usfirst/frc2876/Stronghold2016/commands/AutoDriveTurn.java) did not seem to work. Specifically, it was not possible to drive afterward - it just continued to want to face in the direction we gave it. My assumption is that the command is not really ending, or somehow the PID is still actively trying to correct the angle, but at this point I think I am too tired to help the students debug any further! Links are to different github projects if you care to see the code.

[pblankenbaker]

It looks like your command enables a PID controller to perform the turn. This is OK (we do this as well). However, I don't see where you turn the PID controller off (disable it) in your command's end() and interrupted() methods.

I suspect this is why you are unable to drive after running your command (the command's PID continues to run in the background and "fights" with your normal drive command at setting the motor power).

Adding pid.disable() to the end() method and having interrupt() invoke end() should clean up this issue.