Added some code to test PWM along with initial Quadcopter balance