Improved quadcopter sketch, but still not stable