Improve goal avoidance and wall follow

Improve goal avoidance and wall follow.
Update IR & Touch classes to use Filter

Update IR class to use Filter to easily get last change time.
Update touch sensor to properly use Filter.