def PIDThread(keepRunning = True):
#global drone, imu, compass, yawAngleOriginal
global drone, yawAngleOriginal
while keepRunning:
imu = IMU()
compass = Compass()
rollAngle = imu.getRoll()
pitchAngle = imu.getPitch()
yawAngle = compass.getYaw()
rollPID = int(PID_Roll.update(rollAngle))
pitchPID = int(PID_Pitch.update(pitchAngle))
yawPID = int(PID_Yaw.update(yawAngle))
#print rollAngle, pitchAngle, yawAngle
#print rollPID, pitchPID, yawPID
#print int(rollPID), int(pitchPID), int(yawPID)
drone.motor1.setThrottle(drone.throttle + pitchPID - yawPID) #Front
drone.motor3.setThrottle(drone.throttle - pitchPID - yawPID) #Back
drone.motor4.setThrottle(drone.throttle + rollPID + yawPID) #Left
drone.motor2.setThrottle(drone.throttle - rollPID + yawPID) #Right
time.sleep(0.1)
from Drone import Drone
from PID import PID
from Sensors import IMU, Compass
import time
import readchar
import threading
drone = Drone(throttle = 1300)
imu = IMU()
compass = Compass()
initialRollAngle = imu.getRoll()
initialPitchAngle = imu.getPitch()
initialYawAngle = compass.getYaw()
PID_Roll = PID(initialRollAngle)
PID_Pitch = PID(initialPitchAngle)
PID_Yaw = PID(initialYawAngle)
def PIDThread(keepRunning = True):
#global drone, imu, compass, yawAngleOriginal
global drone, yawAngleOriginal
while keepRunning:
imu = IMU()
compass = Compass()
