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