added pwm = 1600 at initialisation stage.

still need to fix bug for controlling pwm and heading.