Here's the code. It is command based and a bit too inconvenient to put in a post so I uploaded it to github.
https://github.com/zackd97/RobotProject5
The problem areas are in the subsystem GateServoTwo and the commands CloseGateTwo, OpenGateTwo, and the command group GateTwo.