Revert "Revert "All changes from 1st test run of both robots.""

This reverts commit aa34e470d42093c1c26fc51803c4123e94e020d0.
Fixed screwed up revert