Clean up

Still a lot to do

Now tilt and pitch are working but it need PID
Some extra