Updated motor controller code with what we tested in lab today.