Add safety check for dig controller

[Fworg64]

1) backhoe must be curled when coming up from a dig 2) backhoe must be extended beyond a certain CD angle up