Create more elegant mechanism collision safety

[TaylerUva]

What we have works fine but I don't think it the most elegant way to do this. I feel we need to build this into subsystem itself so that the safety is always called when moving. (Maybe have the check in execute?)

This is fine for this PR but let's revisit in the future. We should think of how we design code for mechanical collisions since it happens every year.

Originally posted by @TaylerUva in #77 (comment)