Plenz Geschrieben June 11, 2012 at 22:02 Share 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 Link zu diesem Kommentar Share on other sites More sharing options...
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.