According to abhi, the fetcher is stuck on wall 50 to 75 percent of the time.
We can solve this issue by detecting if the fetcher is stuck on the wall by introducing this new function, isStationary()
Essentially, after we sent a command out to fetch the cone on the stacks, if the fetcher is not reaching the setpoint (!isDone()) AND if it is not moving towards the point (isStationary()), then we assume the fetcher is stuck.
There is a minor issue, and that is that if we move the fetcher back even by a tiny bit, isStationary() will be false, so we can just move the robot back only once by a constant, since you can only overshoot the fetcher by so much (usually 20 ish)
The implementation of isStationary() is also very simple, we have a queue structure that keeps track of the setpoint in the past 10-20 cycles, and then we compare the maximum and minimum in that queue. Since regular movement are generally not oscilating, this would give us the average speed (is it?) over that 10-20 cycles.
According to abhi, the fetcher is stuck on wall 50 to 75 percent of the time.
We can solve this issue by detecting if the fetcher is stuck on the wall by introducing this new function,
isStationary()Essentially, after we sent a command out to fetch the cone on the stacks, if the fetcher is not reaching the setpoint (
!isDone()) AND if it is not moving towards the point (isStationary()), then we assume the fetcher is stuck.There is a minor issue, and that is that if we move the fetcher back even by a tiny bit,
isStationary()will be false, so we can just move the robot back only once by a constant, since you can only overshoot the fetcher by so much (usually 20 ish)The implementation of
isStationary()is also very simple, we have a queue structure that keeps track of the setpoint in the past 10-20 cycles, and then we compare the maximum and minimum in that queue. Since regular movement are generally not oscilating, this would give us the average speed (is it?) over that 10-20 cycles.ok time to implement 🤩