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()