Plenz Geschrieben June 11, 2012 at 22:02 Geschrieben June 11, 2012 at 22:02 Hier ein kleines Programm, das Pitch, Roll und Yaw graphisch darstellt, natürlich mit den neuen Formeln, die die Winkel unabhängig voneinander machen. Man kann wählen, welche Seite des IMU-Bricks "vorn" ist. #!/usr/bin/env python # -*- coding: utf-8 -*- # # IMU demo by Plenz 12.06.2012 - public domain from Tkinter import * from tinkerforge.ip_connection import IPConnection from tinkerforge.brick_imu import IMU import math import time import threading class TF: HOST = "localhost" PORT = 4223 UID_IMU = "9yEBJGvcxtQ" # IMU Brick 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_IMU) # 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.imu.set_convergence_speed(30) # 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) self.set_base_coordinates() self.imu.set_quaternion_period(200) 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) pi = math.atan2(2.0*(y*z - w*x), 1.0 - 2.0*(x*x + y*y)) ro = math.atan2(2.0*(x*z + w*y), 1.0 - 2.0*(x*x + y*y)) ya = math.atan2(2.0*(x*y + w*z), 1.0 - 2.0*(x*x + z*z)) di = app.dir.get() if di == 0: pi = -pi if di == 1: pi, ro = -ro, -pi if di == 2: ro = -ro if di == 3: pi, ro = ro, pi self.pi = pi self.ro = ro self.ya = ya def set_base_coordinates(self): q = self.imu.get_quaternion() self.base_x = q.x self.base_y = q.y self.base_z = q.z self.base_w = q.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 ) class App: def __init__(self, master): root.geometry("200x300+30+30") frame = Frame(master) frame.place(x=1, y=1, width=199, height=299) self.button = Button(frame, text="Reset", command=tf.set_base_coordinates) self.button.place(x=5, y=5, width=90, height=25) self.button = Button(frame, text="QUIT", command=self.ende) self.button.place(x=102, y=5, width=90, height=25) self.message_pit = Label(frame, width=10, text="pitch") self.message_pit.place(x=13, y=35, width=50, height=20) self.message_rol = Label(frame, width=10, text="roll") self.message_rol.place(x=73, y=35, width=50, height=20) self.message_yaw = Label(frame, width=10, text="yaw") self.message_yaw.place(x=133, y=35, width=50, height=20) self.rpy_canvas = Canvas(frame, width=190, height=64, relief=SUNKEN, borderwidth=1) self.rpy_canvas.place(x=3, y=55, width=190, height=70) self.message_dir = Label(frame, width=10, text="Direction:") self.message_dir.place(x=75, y=130, width=50, height=20) self.imu_canvas = Canvas(frame, width=100, height=100, relief=SUNKEN, borderwidth=1) self.imu_canvas.place(x=50, y=170, width=100, height=100) self.imu_canvas.create_rectangle(0, 0, 100, 100, fill="#333333") self.imu_canvas.create_rectangle(0, 30, 15, 70, fill="#EEEEEE") self.imu_canvas.create_rectangle(85, 30, 100, 70, fill="#EEEEEE") self.imu_canvas.create_rectangle(40, 75, 60, 100, fill="#CCCCFF") self.imu_canvas.create_oval(20, 5, 44, 29, fill="#FFFFFF") self.imu_canvas.create_oval(27, 12, 37, 22, fill="#333333") self.imu_canvas.create_rectangle(54, 7, 62, 27, outline="#FFFFFF", width=2) self.imu_canvas.create_rectangle(54, 19, 74, 27, outline="#FFFFFF", width=2) self.dir = IntVar() self.dir1 = Radiobutton(frame, text="", variable=self.dir, value="0") self.dir1.place(x=90, y=150) self.dir2 = Radiobutton(frame, text="", variable=self.dir, value="1") self.dir2.place(x=155, y=210) self.dir3 = Radiobutton(frame, text="", variable=self.dir, value="2") self.dir3.place(x=90, y=275) self.dir4 = Radiobutton(frame, text="", variable=self.dir, value="3") self.dir4.place(x=25, y=210) self.dir1.select() def circ(self, nu, wi): rax = 10 ray = 9 ofx = 25 + rax ofy = 25 + ray ax = rax + nu * 60 ex = ax + 50 dr = math.pi * 30 /180 self.rpy_canvas.create_oval(ax, ray, ex, ray + 50, fill="yellow") ax = math.cos(wi) * -25 + nu * 60 + ofx ay = math.sin(wi) * 25 + ofy ex = math.cos(wi) * 25 + nu * 60 + ofx ey = math.sin(wi) * -25 + ofy if nu == 0: self.rpy_canvas.create_line(ax, ay, ex, ey, fill="red", width=4) bx = math.cos(wi + dr) * -15 + ofx by = math.sin(wi + dr) * 15 + ofy cx = math.cos(wi - dr) * -15 + ofx cy = math.sin(wi - dr) * 15 + ofy points = [ax, ay, bx, by, cx, cy] self.rpy_canvas.create_polygon(points, outline="red", fill="red") if nu == 1: cx = nu * 60 + ofx cy = ofy self.rpy_canvas.create_line(ax, ay, cx, cy, fill="red", width=4) self.rpy_canvas.create_line(cx, cy, ex, ey, fill="green", width=4) if nu == 2: self.rpy_canvas.create_line(ax, ay, ex, ey, fill="black", width=4) ax = math.sin(-wi) * -25 + nu * 60 + ofx ay = math.cos(-wi) * 25 + ofy ex = math.sin(-wi) * 25 + nu * 60 + ofx ey = math.cos(-wi) * -25 + ofy self.rpy_canvas.create_line(ax, ay, ex, ey, fill="red", width=4) bx = math.sin(-wi + dr) * -15 + ofx + nu * 60 by = math.cos(-wi + dr) * 15 + ofy cx = math.sin(-wi - dr) * -15 + ofx + nu * 60 cy = math.cos(-wi - dr) * 15 + ofy points = [ax, ay, bx, by, cx, cy] self.rpy_canvas.create_polygon(points, outline="red", fill="red") def ShowAngle(self): self.rpy_canvas.create_rectangle(0, 0, 190, 70, fill="#DDDDDD") pi = int(-tf.pi * 180 / math.pi + .5) ro = int(-tf.ro * 180 / math.pi + .5) ya = int(tf.ya * 180 / math.pi + .5) self.message_pit.config(text = "pitch " + str(pi) + "°") self.message_rol.config(text = "roll " + str(ro) + "°") self.message_yaw.config(text = "yaw " + str(ya) + "°") ya = tf.ya if ya < math.pi: ya = ya + math.pi else: ya = ya - math.pi ya = -ya self.circ(0, tf.pi) self.circ(1, tf.ro) self.circ(2, ya) if tf.term == 0: # nicht mehr ausführen nach QUIT timer1 = threading.Timer(0.25, self.ShowAngle) timer1.start() else: tf.term = 2 # länger warten nach QUIT def ende(self): print " === waiting for callbacks..." tf.imu.set_quaternion_period(0) tf.term = 2 while tf.term > 1: tf.term = 1 time.sleep(0.5) tf.ipcon.destroy() print "IP destroyed." root.destroy() if __name__ == "__main__": tf = TF() root = Tk() app = App(root) root.title(" IMU Viewer by Plenz") root.resizable(0,0) # Register quaternion callback tf.imu.register_callback(tf.imu.CALLBACK_QUATERNION, tf.quaternion_cb) tf.term = 0 timer1 = threading.Timer(0.25, app.ShowAngle) timer1.start() root.mainloop() Zitieren
Recommended Posts
Join the conversation
You can post now and register later. If you have an account, sign in now to post with your account.