Fixed several errors/bugs. Set motor gain to 20s^-1 NOT (20s)^-1