There's currently a "sanity check" in the setPosition function. It's not clear that this is needed, or that it couldn't be triggered by noise. Furthermore, it's not clear that the way it's handled (break) is the proper fault response.
We should look into robustifying this by clearly defining the error condition we need to detect, and ensuring the response is appropriate (perhaps alarm?)
There's currently a "sanity check" in the setPosition function. It's not clear that this is needed, or that it couldn't be triggered by noise. Furthermore, it's not clear that the way it's handled (break) is the proper fault response.
We should look into robustifying this by clearly defining the error condition we need to detect, and ensuring the response is appropriate (perhaps alarm?)