Hi Walter,
Thank you for your input and assistance with this project. Years ago when I was working on a compass things were similar with requiring complex code and higher math which were not necessary as I later discovered. I have to believe that the people building robots must have a simple way to keep their robot "balanced". Again, going back to the Subtech APC-3 and 4 these Automatic Pitch Controllers which drive a servo were on the market long before the code using the "trig" include was created. My use is to take the outputs from the sensor store in in a variable to display on a "BOB-4" on screen display. The sub has a periscope camera which provides the video for a FPV with the subs operational data displayed. Best, Ed