Untested, just made quite a few changes.
Just making a bit of progress.
Just ran a successful test... no clue if it will work on the real robot,
but it might