def collect_data(suffix):
global SAMPLE_RATE
global w1
global row
print "Now recording " + suffix
ipcon = IPConnection() # Create IP connection
imu = IMU(UID, ipcon) # Create device object
ipcon.connect(HOST, PORT) # Connect to brickd
# Don't use device before ipcon is connected
# Set period for quaternion callback to 1s
imu.set_all_data_period(SAMPLE_RATE)
imu.set_orientation_period(SAMPLE_RATE)
imu.set_quaternion_period(SAMPLE_RATE)
f1 = open('data/letters/all_data_'+suffix+'.csv', 'wb')
w1 = csv.writer(f1)
row = []
# Register quaternion callback
imu.register_callback(imu.CALLBACK_ALL_DATA, cb_all_data)
imu.register_callback(imu.CALLBACK_ORIENTATION, cb_orientation_data)
imu.register_callback(imu.CALLBACK_QUATERNION, cb_quaternion_data)
raw_input('Press key to quit recording ' + suffix + ' \n') # Use input() in Python 3
ipcon.disconnect()
class Q:
HOST = "localhost"
PORT = 4223
UID = "aeoUQwwyAvY" # Change to your UID
def __init__(self):
self.base_x = 0.0
self.base_y = 0.0
self.base_z = 0.0
self.base_w = 0.0
self.imu = IMU(self.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
# Don't use device before it is added to a connection
# Wait for IMU to settle
print 'Set IMU to base position and wait for 10 seconds'
print 'Base position will be 0 for all angles'
time.sleep(10)
q = self.imu.get_quaternion()
self.set_base_coordinates(q.x, q.y, q.z, q.w)
# Set period for quaternion callback to 10ms
self.imu.set_quaternion_period(10)
# Register quaternion callback
self.imu.register_callback(self.imu.CALLBACK_QUATERNION, self.quaternion_cb)
def quaternion_cb(self, x, y, z, w):
# Use conjugate of quaternion to rotate coordinates according to base system
x, y, z, w = self.make_relative_coordinates(-x, -y, -z, w)
x_angle = int(math.atan2(2.0*(y*z - w*x), 1.0 - 2.0*(x*x + y*y))*180/math.pi)
y_angle = int(math.atan2(2.0*(x*z + w*y), 1.0 - 2.0*(x*x + y*y))*180/math.pi)
z_angle = int(math.atan2(2.0*(x*y + w*z), 1.0 - 2.0*(x*x + z*z))*180/math.pi)
print 'x: {0}, y: {1}, z: {2}'.format(x_angle, y_angle, z_angle)
def set_base_coordinates(self, x, y, z, w):
self.base_x = x
self.base_y = y
self.base_z = z
self.base_w = w
def make_relative_coordinates(self, x, y, z, w):
# Multiply base quaternion with current quaternion
return (
w * self.base_x + x * self.base_w + y * self.base_z - z * self.base_y,
w * self.base_y - x * self.base_z + y * self.base_w + z * self.base_x,
w * self.base_z + x * self.base_y - y * self.base_x + z * self.base_w,
w * self.base_w - x * self.base_x - y * self.base_y - z * self.base_z
)
HOST = "localhost"
PORT = 4223
UID = "ayQskyoNrCW" # Change to your UID
from tinkerforge.ip_connection import IPConnection
from tinkerforge.brick_imu import IMU
# Quaternion callback
def cb_quaternion(x, y, z, w):
print("x: " + str(x) + "\ny: " + str(y) + "\nz: " + str(z) + "\nw: " +
str(w) + "\n")
if __name__ == "__main__":
ipcon = IPConnection() # Create IP connection
imu = IMU(UID, ipcon) # Create device object
ipcon.connect(HOST, PORT) # Connect to brickd
# Don't use device before ipcon is connected
# Set period for quaternion callback to 1s
imu.set_quaternion_period(1000)
# Register quaternion callback
imu.register_callback(imu.CALLBACK_QUATERNION, cb_quaternion)
raw_input('Press key to exit\n') # Use input() in Python 3
ipcon.disconnect()
# -*- coding: utf-8 -*-
HOST = "localhost"
PORT = 4223
UID = "a4JritAp6Go" # Change to your UID
from tinkerforge.ip_connection import IPConnection
from tinkerforge.brick_imu import IMU
imu = IMU(UID) # Create device object
# Quaternion callback
def quaternion_cb(x, y, z, w):
print("x: " + str(x) + "\ny: " + str(y) + "\nz: " + str(z) + "\nw: " + str(w) + "\n")
if __name__ == "__main__":
ipcon = IPConnection(HOST, PORT) # Create IPconnection to brickd
ipcon.add_device(imu) # Add device to IP connection
# Don't use device before it is added to a connection
# Set period for quaternion callback to 1s
imu.set_quaternion_period(100)
# Register quaternion callback
imu.register_callback(imu.CALLBACK_QUATERNION, quaternion_cb)
imu.leds_off() # Turn LEDs 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)
