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)
def _setup_sensors(self, config_path):
"""
This sets up the sensors and adds it up to the SM
:param config_path: config yaml file
:type config_path: str
:return:
"""
# open and load the yaml file
with open(config_path, "r") as loader:
config = yaml.load(loader)
# loop through the config and set up the sensors
for name in config:
item = config[name]
byte_list = [item.get("block1"), item.get("block2")]
sensor_type = item.get("type")
location = item.get("location")
side = item.get("side")
axis = item.get("axis")
orientation = item.get("orientation")
if sensor_type == "Accel":
self.sensors[name] = Accel.Accel(name, byte_list, side)
elif sensor_type == "Gyro":
self.sensors[name] = Gyro.Gyro(name, byte_list, side)
elif sensor_type == "FSR":
self.sensors[name] = FSR.FSR(name, byte_list, side)
print orientation
self.sensors[name].orientation = orientation
elif sensor_type == "Pot":
self.sensors[name] = Pot.Pot(name, byte_list, side)
elif sensor_type == "Temperature":
self.sensors[name] = Temperature.Temperature(
name, byte_list, side)
elif sensor_type == "rshal":
pass
# set up IMUs
for name in config:
item = config[name]
if "IMU" in name:
accel = item.get("accel")
gyro = item.get("gyro")
temp = item.get("temp")
counter = item.get("counter")
rshal = item.get("rshal")
print name
imu = IMU.IMU(name, self.sensors[accel],
self.sensors[gyro], self.sensors[temp], None,
None)
self._imus[name] = imu
self._sensor_manager.registar_all_sensors(self.sensors.values())
for key, sensor in self.sensors.iteritems():
self._filter_manager.registar([BaseFilter.BaseFilter(sensor)],
sensor.name)
def __init__(self):
self.imu = IMU.IMU()
self.distanceSensor = us.UltrasonicSensor()
#self.serial = ac.ArduinoCommunication()
self.distance = 0
self.imu_gyro_x = 0
self.imu_gyro_y = 0
self.imu_gyro_z = 0
self.speed = 0
self.threads = []
super().__init__()
from Sensors import IMU
import time
#imu = IMU()
while True:
imu = IMU()
imu.wakeAndPrint()
time.sleep(1)
def sensors():
data = ""
data += Depth()
data += Temp()
data += IMU()
return data
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()
