Added some test and modicfied the code to cancel PID.cpp's IntegralError = 0. Added a switch case for programs to fly