Remove some unneeded code.
Change timestep to 20ms, which is 50hz