class Sensors(object):
def __init__(self):
self.__ipcon = IPConnection('localhost', 4223)
def __del__(self):
self.__ipcon.destroy()
def add(self, pSensor):
self.__ipcon.add_device(pSensor)
# -*- coding: utf-8 -*-
# this one creates an array of connected devices called dev
# and acts as enum in standalone mode
from tinkerforge.ip_connection import IPConnection
from settings import HOST, PORT
dev = []
def cb_enumerate(uid, name, stack_id, is_new):
if is_new:
print("New device:")
dev.append([name, uid, stack_id])
else:
print("Removed device:")
for i in dev:
if i[1] == uid:
dev.remove(i)
if __name__ == "__main__":
print(" Name: " + name)
print(" UID: " + uid)
print(" Stack ID: " + str(stack_id))
print("")
if __name__ == "__main__":
ipcon = IPConnection(HOST, PORT) # Create IP connection to brickd
ipcon.enumerate(cb_enumerate) # Enumerate Bricks and Bricklets
raw_input('Press key to exit\n') # Use input() in Python 3
ipcon.destroy()
ipcon = IPConnection(HOST, PORT)
# Create device objects
dc0 = DC(UIDdc0)
dc1 = DC(UIDdc1)
lcd = LCD20x4(UIDdpl)
mst = Master(UIDmaster)
# Connect to devices
ipcon.add_device(dc0)
ipcon.add_device(dc1)
ipcon.add_device(lcd)
ipcon.add_device(mst)
# Hier wird ein Initial Program gestartet um eventuelle sonderwuensche entgegenzunehmen
velol = 0
velor = 0
hz = 0
acc = 0
getVars(velol, velor, hz, acc)
#Hier der zweite thread der sich um die Steuerung kuemmert, er laueft deutlich schneller!
#Hier wird nun auch der Status abgefragt - ca. je 1sec * 6 / 1000 !!
Control(velol, velor, hz, acc)
#Display off - hier doof weil das display staendig resetet wird und am ende gehts nicht aus
# DisplayClear()
#lcd.backlight_off()
#raw_input('Press key to exit\n') # Use input() in Python 3
ipcon.destroy()
class TomIMU: def __init__(self,host,port,uid,callbackPeriodMS=100): self.host=host self.port=port self.uid=uid self._imu = IMU(uid) # Create device object self._ipcon = IPConnection(self.host,self.port) # Create IPconnection to brickd self._ipcon.add_device(self._imu) # Add device to IP connection self.ready = True # Don't use device before it is added to a connection # Set period for quaternion callback (defaults to 100ms) self._imu.set_quaternion_period(callbackPeriodMS) # Register quaternion callback self._imu.register_callback(self._imu.CALLBACK_QUATERNION, self._QuaternionCallback) self._imu.leds_off() # Turn LEDs off. self._imu.set_convergence_speed(5) # 5ms convergence. # Orientation origin and most recent values q = self._imu.get_quaternion() # Get a temp quaternion from current pose. self.rel_x = q.x self.rel_y = q.y self.rel_z = q.z self.rel_w = q.w self.x = q.x self.y = q.y self.z = q.z self.w = q.w def __destroy__(self): self._ipcon.destroy() def _QuaternionCallback(self, x, y, z, w): """ Records the most recent quaternion orientation values. """ self.x,self.y,self.z,self.w = x,y,z,w def SetOrientationOrigin(self, origin=None): """ Resets the orientation origin to the given values, or the latest reading if none. """ if origin is None: self.rel_x, self.rel_y, self.rel_z, self.rel_w = self.x, self.y, self.z, self.w else: self.rel_x, self.rel_y, self.rel_z, self.rel_w = origin def GetEulerOrientation(self): x,y,z,w = self.GetQuaternionOrientation() from math import atan2, asin roll = atan2(2.0*y*w - 2.0*x*z, 1.0 - 2.0*y*y - 2.0*z*z) pitch = atan2(2.0*x*w - 2.0*y*z, 1.0 - 2.0*x*x - 2.0*z*z) yaw = asin(2.0*x*y + 2.0*z*w) return roll,pitch,yaw def GetQuaternionOrientation(self): # Conjugate x,y,z = -self.x, -self.y, -self.z w = self.w # Multiply wn = w * self.rel_w - x * self.rel_x - y * self.rel_y - z * self.rel_z xn = w * self.rel_x + x * self.rel_w + y * self.rel_z - z * self.rel_y yn = w * self.rel_y - x * self.rel_z + y * self.rel_w + z * self.rel_x zn = w * self.rel_z + x * self.rel_y - y * self.rel_x + z * self.rel_w return xn,yn,zn,wn def GetConvergenceSpeed(self): return self._imu.get_convergence_speed() def SetConvergenceSpeed(self, speed): self._imu.set_convergence_speed(speed)
