Jump to content

[Python] IMU-Demo mit graphischer Oberfläche


Recommended Posts

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

IMU-Demo.png.1a08722bf1ee38af654aa03d73cedee0.png

Link to comment
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

 Share

×
×
  • Create New...