Final tweaks to finally drive robot