The system was programmed in c++ to run on an Arduino Nano. We used a PID controller with angle as the error to produce the PWM output for the motor. We also used the Madgwick filter to reduce gyroscope drift. This filter works by using the gravity vector to counteract the drift. Click either of the sections below to learn more about the code.
Full Code
// Code will appear here