ESC2 = 27
ESC3 = 19
ESC4 = 26

ELEV = 5
AILE = 6
THRO = 13
RUDD = 12
GEAR = 16
AUX1 = 20
AUX2 = 21

inPINS = [ELEV, AILE, THRO, RUDD, GEAR, AUX1, AUX2]

pi = pigpio.pi()
pot = MCP3202(channel=0)

l3g4200d = L3G4200D_lib.L3G4200D()
adxl345 = ADXL345_lib.ADXL345()
hmc5883l = HMC5883L_lib.HMC5883L()
kalmanX = Kalman.KalmanAngle()
kalmanY = Kalman.KalmanAngle()

pi.set_servo_pulsewidth(ESC1, 1000)
pi.set_servo_pulsewidth(ESC2, 1000)
pi.set_servo_pulsewidth(ESC3, 1000)
pi.set_servo_pulsewidth(ESC4, 1000)
time.sleep(3)


#Calculate lipo voltage
from gpiozero import MCP3202
import time
import math
import matplotlib.pyplot as plt
from functools import reduce

samples = 300

voltage = MCP3202(channel=1, differential=False, max_voltage=5.0)
current = MCP3202(channel=0, differential=False, max_voltage=5.0)

voltageValues = []
currentValues = []

for x in range(0, samples - 1):
    voltageValues.append(voltage.value * 5)
    #print(str(voltage.value)+ " " + str(current.value))
    currentValues.append(current.value * 5)

t = range(0, samples - 1)
data1 = voltageValues
data2 = currentValues

fig, ax1 = plt.subplots()

color = 'tab:red'
ax1.set_xlabel('samples (n)')
ax1.set_ylabel('Voltage (V)', color=color)
ax1.plot(t, data1, color=color)
ax1.tick_params(axis='y', labelcolor=color)