Create controller.c

Supertwisting controller to control the attitude and PID controller to the altitude