Code in depth

This code uses the

• MPU6050_light.h library to track angular position using an MPU 6050 gyroscope

• Servo.h library to write the correct PWM signal for an ESC to read

• Wire.h library to enable an I2C connection with the MPU 6050 gyroscope

All 4 motor outputs are then declared as 'servos'

The Power, Roll, and Pitch of the drone are all declared as integers and will represent the commands coming from a remote control.

The gains for proportional integral and derivative are all set at values experimentally determined.

The proportional integral derivative and the last error for each axis of rotation were declared as doubles, and the last error was set to 0.

Timers for calculation and updating data from the remote control were all declared.

All 4 motor outputs were attached to their respective pin and had their ranges defined, as the ESCs cannot interpret anything outside the range.

The ESCs were calibrated as the program began, by first giving them the highest throttle value, waiting for 5 seconds, giving them the lowest throttle value, and waiting another 5 seconds. A series of beeps from the motors would confirm proper calibration.

An I2C connection was stated with the Wire.begin() command

The MPU 6050 was connected too, and calibrated before the setup would be completed.

Whenever 1 millisecond had passed, the PWM signals coming from a radio receiver would have the times for a HIGH pulse recorded, and run through the Proc() to produce a value between 0 and 180, Proc() defined later in the code.

The natural position of the roll and pitch sticks on the remote control produce a value of 90, and the range of the values produced is too wide, so 90 is substracted from the original value and is then divided by 5 to produce a smaller range of values

The X Y and Z angles are updated and recorded into the X_Error Y_Error and Z_Error variables and will represent the Error for the PID system. A tolerance of +/- 1 deg is then applied to the angles.

The change of the error over time (derivative) is then calculated and multiplied by the derivative gain for each axis of rotation is calculated every millisecond

The roll and pitch are then added to the X and Y error respectively, this was done after the derivative, as moving the sticks too fast would cause the derivative to calculate an inaccurate value

The proportional is then calculated every millisecond by multiplying the error by the proportional gain for each axis of rotation.

The integral is then calculated every millisecond by Riemann sum, with each step multiplied by the integral gain. Then a max and min integral value is defined to avoid wind-up that may occur with the integral. This is done for each axis of rotation.

The PID for each axis of rotation is then calculated by summating the proportional derivative and integral.

For each motor, the base power from the remote control then had its respective PID added or substracted to it, depending on its orientation to the gyroscope.

If the power stick of the controller is set all the way down, the drone overrides the motors to stop as a safety mechanism.

Because only a value in between 0 and 180 can be output to the motors, all motor values less than 0 are set to 0, and all motor values greater than 180 are set to 180.

Each motor value is then output using the servo.h library

The function takes the time of the High pulse of the PWM signals from the radio receiver and converts it to a value from 0 to 180