Create in tolerance method for Units

This will simplify our code
```java
return (mechanismPosition.compareTo(desiredPosition.minus(tolerance)) > 0 &&
mechanismPosition.compareTo(desiredPosition.plus(tolerance)) < 0)
```