コード例 #1
0
ファイル: imu.py プロジェクト: smunix/brickv
    def __init__(self, ipcon, uid):
        PluginBase.__init__(self, ipcon, uid)
        self.setupUi(self)

        self.imu = brick_imu.IMU(self.uid)
        self.device = self.imu
        self.ipcon.add_device(self.imu)
        self.version = '.'.join(map(str, self.imu.get_version()[1]))

        self.update_timer = QTimer()
        self.update_timer.timeout.connect(self.update_data)

        # Import IMUGLWidget here, not global. If globally included we get
        # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate'
        # as soon as IMU is set as device_class in __init__.
        # No idea why this happens, doesn't make sense.
        from imu_gl_widget import IMUGLWidget

        self.imu_gl = IMUGLWidget(self)
        self.imu_gl.setMinimumSize(200, 200)
        self.min_x = 0
        self.min_y = 0
        self.min_z = 0
        self.max_x = 0
        self.max_y = 0
        self.max_z = 0

        self.counter = 0

        self.mag_plot = Plot("Magnetic Field [mG]",
                             [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])
        self.acc_plot = Plot("Acceleration [mG]",
                             [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])
        self.gyr_plot = Plot("Angular Velocity [%c/s]" % 0xB0,
                             [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])

        self.tem_plot = Plot("Temperature [%cC]" % 0xB0, [["t", Qt.red]])

        self.grid_layout.addWidget(self.gyr_plot, 0, 0)
        self.grid_layout.addWidget(self.acc_plot, 0, 2)
        self.grid_layout.addWidget(self.mag_plot, 0, 4)
        self.grid_layout.addWidget(self.imu_gl, 2, 2)
        self.grid_layout.addWidget(self.tem_plot, 2, 4)

        self.save_orientation.clicked.connect(self.imu_gl.save_orientation)
        self.clear_graphs.clicked.connect(self.clear_graphs_clicked)
コード例 #2
0
ファイル: imu.py プロジェクト: smunix/brickv
 def __init__ (self, ipcon, uid):
     PluginBase.__init__(self, ipcon, uid)
     self.setupUi(self)
     
     self.imu = brick_imu.IMU(self.uid)
     self.device = self.imu
     self.ipcon.add_device(self.imu)
     self.version = '.'.join(map(str, self.imu.get_version()[1]))
     
     self.update_timer = QTimer()
     self.update_timer.timeout.connect(self.update_data)
     
     # Import IMUGLWidget here, not global. If globally included we get
     # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate'
     # as soon as IMU is set as device_class in __init__. 
     # No idea why this happens, doesn't make sense.
     from imu_gl_widget import IMUGLWidget
     
     self.imu_gl = IMUGLWidget(self)
     self.imu_gl.setMinimumSize(200, 200)
     self.min_x = 0
     self.min_y = 0
     self.min_z = 0
     self.max_x = 0 
     self.max_y = 0
     self.max_z = 0
     
     self.counter = 0
     
     self.mag_plot = Plot("Magnetic Field [mG]",
                          [["X", Qt.red],
                           ["Y", Qt.green],
                           ["Z", Qt.blue]])
     self.acc_plot = Plot("Acceleration [mG]",
                          [["X", Qt.red],
                           ["Y", Qt.green],
                           ["Z", Qt.blue]])
     self.gyr_plot = Plot("Angular Velocity [%c/s]" % 0xB0,
                          [["X", Qt.red],
                           ["Y", Qt.green],
                           ["Z", Qt.blue]])
     
     self.tem_plot = Plot("Temperature [%cC]" % 0xB0,
                          [["t", Qt.red]])
     
     self.grid_layout.addWidget(self.gyr_plot, 0, 0)
     self.grid_layout.addWidget(self.acc_plot, 0, 2)
     self.grid_layout.addWidget(self.mag_plot, 0, 4)
     self.grid_layout.addWidget(self.imu_gl, 2, 2)
     self.grid_layout.addWidget(self.tem_plot, 2, 4)
     
     self.save_orientation.clicked.connect(self.imu_gl.save_orientation)
     self.clear_graphs.clicked.connect(self.clear_graphs_clicked)
コード例 #3
0
ファイル: imu.py プロジェクト: Loremipsum1988/brickv
    def __init__(self, *args):
        PluginBase.__init__(self, BrickIMU, *args)

        self.setupUi(self)

        self.imu = self.device

        self.acc_x = 0
        self.acc_y = 0
        self.acc_z = 0
        self.mag_x = 0
        self.mag_y = 0
        self.mag_z = 0
        self.gyr_x = 0
        self.gyr_y = 0
        self.gyr_z = 0
        self.tem   = 0
        self.roll  = 0
        self.pitch = 0
        self.yaw   = 0
        self.qua_x = 0
        self.qua_y = 0
        self.qua_z = 0
        self.qua_w = 0

        self.old_time = 0

        self.update_timer = QTimer()
        self.update_timer.timeout.connect(self.update_data)

        self.cbe_all_data = CallbackEmulator(self.imu.get_all_data,
                                             self.all_data_callback,
                                             self.increase_error_count,
                                             use_data_signal=False)
        self.cbe_orientation = CallbackEmulator(self.imu.get_orientation,
                                                self.orientation_callback,
                                                self.increase_error_count,
                                                use_data_signal=False)
        self.cbe_quaternion = CallbackEmulator(self.imu.get_quaternion,
                                               self.quaternion_callback,
                                               self.increase_error_count,
                                               use_data_signal=False)

        # Import IMUGLWidget here, not global. If globally included we get
        # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate'
        # as soon as IMU is set as device_class in __init__.
        # No idea why this happens, doesn't make sense.
        try:
            from .imu_gl_widget import IMUGLWidget
        except:
            from imu_gl_widget import IMUGLWidget

        self.imu_gl = IMUGLWidget(self)
        self.imu_gl.setMinimumSize(150, 150)
        self.imu_gl.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
        self.min_x = 0
        self.min_y = 0
        self.min_z = 0
        self.max_x = 0
        self.max_y = 0
        self.max_z = 0

        self.update_counter = 0

        self.mag_plot_widget = PlotWidget("Magnetic Field [mG]",
                                          [["X", Qt.red, self.get_mag_x],
                                           ["Y", Qt.darkGreen, self.get_mag_y],
                                           ["Z", Qt.blue, self.get_mag_z]],
                                          self.clear_graphs)
        self.acc_plot_widget = PlotWidget("Acceleration [mG]",
                                          [["X", Qt.red, self.get_acc_x],
                                           ["Y", Qt.darkGreen, self.get_acc_y],
                                           ["Z", Qt.blue, self.get_acc_z]],
                                          self.clear_graphs)
        self.gyr_plot_widget = PlotWidget("Angular Velocity [%c/s]" % 0xB0,
                                          [["X", Qt.red, self.get_gyr_x],
                                           ["Y", Qt.darkGreen, self.get_gyr_y],
                                           ["Z", Qt.blue, self.get_gyr_z]],
                                          self.clear_graphs)
        self.tem_plot_widget = PlotWidget("Temperature [%cC]" % 0xB0,
                                          [["t", Qt.red, self.get_tem]],
                                          self.clear_graphs)

        self.mag_plot_widget.setMinimumSize(250, 200)
        self.acc_plot_widget.setMinimumSize(250, 200)
        self.gyr_plot_widget.setMinimumSize(250, 200)
        self.tem_plot_widget.setMinimumSize(250, 200)

        self.orientation_label = QLabel("""Position your IMU Brick as shown \
in the image above, then press "Save Orientation".""")
        self.orientation_label.setWordWrap(True)
        self.orientation_label.setAlignment(Qt.AlignHCenter)
        self.gl_layout = QVBoxLayout()
        self.gl_layout.addWidget(self.imu_gl)
        self.gl_layout.addWidget(self.orientation_label)

        self.layout_top.addWidget(self.gyr_plot_widget)
        self.layout_top.addWidget(self.acc_plot_widget)
        self.layout_top.addWidget(self.mag_plot_widget)
        self.layout_bottom.addLayout(self.gl_layout)
        self.layout_bottom.addWidget(self.tem_plot_widget)

        self.save_orientation.clicked.connect(self.imu_gl.save_orientation)
        self.calibrate.clicked.connect(self.calibrate_clicked)
        self.led_button.clicked.connect(self.led_clicked)
        self.speed_spinbox.editingFinished.connect(self.speed_finished)

        self.calibrate = None
        self.alive = True

        if self.firmware_version >= (1, 0, 7):
            reset = QAction('Reset', self)
            reset.triggered.connect(lambda: self.imu.reset())
            self.set_actions(reset)
コード例 #4
0
ファイル: imu.py プロジェクト: Loremipsum1988/brickv
class IMU(PluginBase, Ui_IMU):
    def __init__(self, *args):
        PluginBase.__init__(self, BrickIMU, *args)

        self.setupUi(self)

        self.imu = self.device

        self.acc_x = 0
        self.acc_y = 0
        self.acc_z = 0
        self.mag_x = 0
        self.mag_y = 0
        self.mag_z = 0
        self.gyr_x = 0
        self.gyr_y = 0
        self.gyr_z = 0
        self.tem   = 0
        self.roll  = 0
        self.pitch = 0
        self.yaw   = 0
        self.qua_x = 0
        self.qua_y = 0
        self.qua_z = 0
        self.qua_w = 0

        self.old_time = 0

        self.update_timer = QTimer()
        self.update_timer.timeout.connect(self.update_data)

        self.cbe_all_data = CallbackEmulator(self.imu.get_all_data,
                                             self.all_data_callback,
                                             self.increase_error_count,
                                             use_data_signal=False)
        self.cbe_orientation = CallbackEmulator(self.imu.get_orientation,
                                                self.orientation_callback,
                                                self.increase_error_count,
                                                use_data_signal=False)
        self.cbe_quaternion = CallbackEmulator(self.imu.get_quaternion,
                                               self.quaternion_callback,
                                               self.increase_error_count,
                                               use_data_signal=False)

        # Import IMUGLWidget here, not global. If globally included we get
        # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate'
        # as soon as IMU is set as device_class in __init__.
        # No idea why this happens, doesn't make sense.
        try:
            from .imu_gl_widget import IMUGLWidget
        except:
            from imu_gl_widget import IMUGLWidget

        self.imu_gl = IMUGLWidget(self)
        self.imu_gl.setMinimumSize(150, 150)
        self.imu_gl.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
        self.min_x = 0
        self.min_y = 0
        self.min_z = 0
        self.max_x = 0
        self.max_y = 0
        self.max_z = 0

        self.update_counter = 0

        self.mag_plot_widget = PlotWidget("Magnetic Field [mG]",
                                          [["X", Qt.red, self.get_mag_x],
                                           ["Y", Qt.darkGreen, self.get_mag_y],
                                           ["Z", Qt.blue, self.get_mag_z]],
                                          self.clear_graphs)
        self.acc_plot_widget = PlotWidget("Acceleration [mG]",
                                          [["X", Qt.red, self.get_acc_x],
                                           ["Y", Qt.darkGreen, self.get_acc_y],
                                           ["Z", Qt.blue, self.get_acc_z]],
                                          self.clear_graphs)
        self.gyr_plot_widget = PlotWidget("Angular Velocity [%c/s]" % 0xB0,
                                          [["X", Qt.red, self.get_gyr_x],
                                           ["Y", Qt.darkGreen, self.get_gyr_y],
                                           ["Z", Qt.blue, self.get_gyr_z]],
                                          self.clear_graphs)
        self.tem_plot_widget = PlotWidget("Temperature [%cC]" % 0xB0,
                                          [["t", Qt.red, self.get_tem]],
                                          self.clear_graphs)

        self.mag_plot_widget.setMinimumSize(250, 200)
        self.acc_plot_widget.setMinimumSize(250, 200)
        self.gyr_plot_widget.setMinimumSize(250, 200)
        self.tem_plot_widget.setMinimumSize(250, 200)

        self.orientation_label = QLabel("""Position your IMU Brick as shown \
in the image above, then press "Save Orientation".""")
        self.orientation_label.setWordWrap(True)
        self.orientation_label.setAlignment(Qt.AlignHCenter)
        self.gl_layout = QVBoxLayout()
        self.gl_layout.addWidget(self.imu_gl)
        self.gl_layout.addWidget(self.orientation_label)

        self.layout_top.addWidget(self.gyr_plot_widget)
        self.layout_top.addWidget(self.acc_plot_widget)
        self.layout_top.addWidget(self.mag_plot_widget)
        self.layout_bottom.addLayout(self.gl_layout)
        self.layout_bottom.addWidget(self.tem_plot_widget)

        self.save_orientation.clicked.connect(self.imu_gl.save_orientation)
        self.calibrate.clicked.connect(self.calibrate_clicked)
        self.led_button.clicked.connect(self.led_clicked)
        self.speed_spinbox.editingFinished.connect(self.speed_finished)

        self.calibrate = None
        self.alive = True

        if self.firmware_version >= (1, 0, 7):
            reset = QAction('Reset', self)
            reset.triggered.connect(lambda: self.imu.reset())
            self.set_actions(reset)

    def start(self):
        if not self.alive:
            return

        self.gl_layout.activate()
        self.cbe_all_data.set_period(100)
        self.cbe_orientation.set_period(100)
        self.cbe_quaternion.set_period(50)
        self.update_timer.start(50)

        async_call(self.imu.get_convergence_speed, None, self.speed_spinbox.setValue, self.increase_error_count)

        self.mag_plot_widget.stop = False
        self.acc_plot_widget.stop = False
        self.gyr_plot_widget.stop = False
        self.tem_plot_widget.stop = False

    def stop(self):
        self.mag_plot_widget.stop = True
        self.acc_plot_widget.stop = True
        self.gyr_plot_widget.stop = True
        self.tem_plot_widget.stop = True

        self.update_timer.stop()
        self.cbe_all_data.set_period(0)
        self.cbe_orientation.set_period(0)
        self.cbe_quaternion.set_period(0)

    def destroy(self):
        self.alive = False
        if self.calibrate:
            self.calibrate.close()

    def get_url_part(self):
        return 'imu'

    @staticmethod
    def has_device_identifier(device_identifier):
        return device_identifier == BrickIMU.DEVICE_IDENTIFIER

    def all_data_callback(self, data):
        acc_x, acc_y, acc_z, mag_x, mag_y, mag_z, gyr_x, gyr_y, gyr_z, tem = data
        self.acc_x = acc_x
        self.acc_y = acc_y
        self.acc_z = acc_z
        self.mag_x = mag_x
        self.mag_y = mag_y
        self.mag_z = mag_z
        self.gyr_x = gyr_x
        self.gyr_y = gyr_y
        self.gyr_z = gyr_z
        self.tem = tem

    def quaternion_callback(self, data):
        qua_x, qua_y, qua_z, qua_w = data
        self.qua_x = qua_x
        self.qua_y = qua_y
        self.qua_z = qua_z
        self.qua_w = qua_w

    def orientation_callback(self, data):
        roll, pitch, yaw = data
        self.roll = roll
        self.pitch = pitch
        self.yaw = yaw

    def led_clicked(self):
        if 'On' in self.led_button.text():
            self.led_button.setText('Turn LEDs Off')
            self.imu.leds_on()
        elif 'Off' in self.led_button.text():
            self.led_button.setText('Turn LEDs On')
            self.imu.leds_off()

    def get_acc_x(self):
        return self.acc_x

    def get_acc_y(self):
        return self.acc_y

    def get_acc_z(self):
        return self.acc_z

    def get_mag_x(self):
        return self.mag_x

    def get_mag_y(self):
        return self.mag_y

    def get_mag_z(self):
        return self.mag_z

    def get_gyr_x(self):
        return self.gyr_x/14.375

    def get_gyr_y(self):
        return self.gyr_y/14.375

    def get_gyr_z(self):
        return self.gyr_z/14.375

    def get_tem(self):
        return self.tem/100.0

    def update_data(self):
        self.update_counter += 1

        self.imu_gl.update(self.qua_x, self.qua_y, self.qua_z, self.qua_w)

        if self.update_counter % 2:
            gyr_x = self.gyr_x/14.375
            gyr_y = self.gyr_y/14.375
            gyr_z = self.gyr_z/14.375

            self.acceleration_update(self.acc_x, self.acc_y, self.acc_z)
            self.magnetometer_update(self.mag_x, self.mag_y, self.mag_z)
            self.gyroscope_update(gyr_x, gyr_y, gyr_z)
            self.orientation_update(self.roll, self.pitch, self.yaw)
            self.temperature_update(self.tem)

    def acceleration_update(self, x, y, z):
        x_str = "%g" % x
        y_str = "%g" % y
        z_str = "%g" % z
        self.acc_y_label.setText(y_str)
        self.acc_x_label.setText(x_str)
        self.acc_z_label.setText(z_str)

    def magnetometer_update(self, x, y, z):
        # Earth magnetic field. 0.5 Gauss
        x_str = "%g" % x
        y_str = "%g" % y
        z_str = "%g" % z
        self.mag_x_label.setText(x_str)
        self.mag_y_label.setText(y_str)
        self.mag_z_label.setText(z_str)

    def gyroscope_update(self, x, y, z):
        x_str = "%g" % int(x)
        y_str = "%g" % int(y)
        z_str = "%g" % int(z)
        self.gyr_x_label.setText(x_str)
        self.gyr_y_label.setText(y_str)
        self.gyr_z_label.setText(z_str)

    def orientation_update(self, r, p, y):
        r_str = "%g" % (r/100)
        p_str = "%g" % (p/100)
        y_str = "%g" % (y/100)
        self.roll_label.setText(r_str)
        self.pitch_label.setText(p_str)
        self.yaw_label.setText(y_str)

    def temperature_update(self, t):
        t_str = "%.2f" % (t/100.0)
        self.tem_label.setText(t_str)

    def calibrate_clicked(self):
        self.stop()
        if self.calibrate is None:
            self.calibrate = CalibrateWindow(self)

        self.calibrate.refresh_values()
        self.calibrate.show()

    def speed_finished(self):
        speed = self.speed_spinbox.value()
        self.imu.set_convergence_speed(speed)
コード例 #5
0
ファイル: imu.py プロジェクト: tewdreyer/brickv
    def __init__(self, ipcon, uid):
        PluginBase.__init__(self, ipcon, uid)
        self.setupUi(self)

        self.imu = brick_imu.IMU(self.uid)
        self.device = self.imu
        self.ipcon.add_device(self.imu)

        version = self.imu.get_version()
        self.version = ".".join(map(str, version[1]))
        self.version_minor = version[1][1]
        self.version_release = version[1][2]

        self.acc_x = 0
        self.acc_y = 0
        self.acc_z = 0
        self.mag_x = 0
        self.mag_y = 0
        self.mag_z = 0
        self.gyr_x = 0
        self.gyr_y = 0
        self.gyr_z = 0
        self.tem = 0
        self.roll = 0
        self.pitch = 0
        self.yaw = 0
        self.qua_x = 0
        self.qua_y = 0
        self.qua_z = 0
        self.qua_w = 0

        self.old_time = 0

        self.update_timer = QTimer()
        self.update_timer.timeout.connect(self.update_data)

        self.imu.register_callback(self.imu.CALLBACK_ALL_DATA, self.all_data_callback)
        self.imu.register_callback(self.imu.CALLBACK_ORIENTATION, self.orientation_callback)
        self.imu.register_callback(self.imu.CALLBACK_QUATERNION, self.quaternion_callback)

        # Import IMUGLWidget here, not global. If globally included we get
        # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate'
        # as soon as IMU is set as device_class in __init__.
        # No idea why this happens, doesn't make sense.
        from imu_gl_widget import IMUGLWidget

        self.imu_gl = IMUGLWidget(self)
        self.imu_gl.setMinimumSize(150, 150)
        self.imu_gl.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
        self.min_x = 0
        self.min_y = 0
        self.min_z = 0
        self.max_x = 0
        self.max_y = 0
        self.max_z = 0

        self.counter = 0
        self.update_counter = 0

        self.mag_plot = Plot("Magnetic Field [mG]", [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])
        self.acc_plot = Plot("Acceleration [mG]", [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])
        self.gyr_plot = Plot("Angular Velocity [%c/s]" % 0xB0, [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])

        self.tem_plot = Plot("Temperature [%cC]" % 0xB0, [["t", Qt.red]])

        self.mag_plot.setMinimumSize(250, 200)
        self.acc_plot.setMinimumSize(250, 200)
        self.gyr_plot.setMinimumSize(250, 200)
        self.tem_plot.setMinimumSize(250, 200)

        self.orientation_label = QLabel(
            """Position your IMU Brick as shown \
in the image above, then press "Save Orientation"."""
        )
        self.orientation_label.setWordWrap(True)
        self.orientation_label.setAlignment(Qt.AlignHCenter)
        self.gl_layout = QVBoxLayout()
        self.gl_layout.addWidget(self.imu_gl)
        self.gl_layout.addWidget(self.orientation_label)

        self.layout_top.addWidget(self.gyr_plot)
        self.layout_top.addWidget(self.acc_plot)
        self.layout_top.addWidget(self.mag_plot)
        self.layout_bottom.addLayout(self.gl_layout)
        self.layout_bottom.addWidget(self.tem_plot)

        self.save_orientation.clicked.connect(self.imu_gl.save_orientation)
        self.clear_graphs.clicked.connect(self.clear_graphs_clicked)
        self.calibrate.clicked.connect(self.calibrate_pressed)
        self.led_button.clicked.connect(self.led_clicked)
        self.speed_spinbox.editingFinished.connect(self.speed_finished)
コード例 #6
0
ファイル: imu.py プロジェクト: tewdreyer/brickv
class IMU(PluginBase, Ui_IMU):
    def __init__(self, ipcon, uid):
        PluginBase.__init__(self, ipcon, uid)
        self.setupUi(self)

        self.imu = brick_imu.IMU(self.uid)
        self.device = self.imu
        self.ipcon.add_device(self.imu)

        version = self.imu.get_version()
        self.version = ".".join(map(str, version[1]))
        self.version_minor = version[1][1]
        self.version_release = version[1][2]

        self.acc_x = 0
        self.acc_y = 0
        self.acc_z = 0
        self.mag_x = 0
        self.mag_y = 0
        self.mag_z = 0
        self.gyr_x = 0
        self.gyr_y = 0
        self.gyr_z = 0
        self.tem = 0
        self.roll = 0
        self.pitch = 0
        self.yaw = 0
        self.qua_x = 0
        self.qua_y = 0
        self.qua_z = 0
        self.qua_w = 0

        self.old_time = 0

        self.update_timer = QTimer()
        self.update_timer.timeout.connect(self.update_data)

        self.imu.register_callback(self.imu.CALLBACK_ALL_DATA, self.all_data_callback)
        self.imu.register_callback(self.imu.CALLBACK_ORIENTATION, self.orientation_callback)
        self.imu.register_callback(self.imu.CALLBACK_QUATERNION, self.quaternion_callback)

        # Import IMUGLWidget here, not global. If globally included we get
        # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate'
        # as soon as IMU is set as device_class in __init__.
        # No idea why this happens, doesn't make sense.
        from imu_gl_widget import IMUGLWidget

        self.imu_gl = IMUGLWidget(self)
        self.imu_gl.setMinimumSize(150, 150)
        self.imu_gl.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
        self.min_x = 0
        self.min_y = 0
        self.min_z = 0
        self.max_x = 0
        self.max_y = 0
        self.max_z = 0

        self.counter = 0
        self.update_counter = 0

        self.mag_plot = Plot("Magnetic Field [mG]", [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])
        self.acc_plot = Plot("Acceleration [mG]", [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])
        self.gyr_plot = Plot("Angular Velocity [%c/s]" % 0xB0, [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])

        self.tem_plot = Plot("Temperature [%cC]" % 0xB0, [["t", Qt.red]])

        self.mag_plot.setMinimumSize(250, 200)
        self.acc_plot.setMinimumSize(250, 200)
        self.gyr_plot.setMinimumSize(250, 200)
        self.tem_plot.setMinimumSize(250, 200)

        self.orientation_label = QLabel(
            """Position your IMU Brick as shown \
in the image above, then press "Save Orientation"."""
        )
        self.orientation_label.setWordWrap(True)
        self.orientation_label.setAlignment(Qt.AlignHCenter)
        self.gl_layout = QVBoxLayout()
        self.gl_layout.addWidget(self.imu_gl)
        self.gl_layout.addWidget(self.orientation_label)

        self.layout_top.addWidget(self.gyr_plot)
        self.layout_top.addWidget(self.acc_plot)
        self.layout_top.addWidget(self.mag_plot)
        self.layout_bottom.addLayout(self.gl_layout)
        self.layout_bottom.addWidget(self.tem_plot)

        self.save_orientation.clicked.connect(self.imu_gl.save_orientation)
        self.clear_graphs.clicked.connect(self.clear_graphs_clicked)
        self.calibrate.clicked.connect(self.calibrate_pressed)
        self.led_button.clicked.connect(self.led_clicked)
        self.speed_spinbox.editingFinished.connect(self.speed_finished)

    def start(self):
        self.gl_layout.activate()
        self.imu.set_all_data_period(100)
        self.imu.set_orientation_period(100)
        self.imu.set_quaternion_period(50)
        self.update_timer.start(50)

        speed = self.imu.get_convergence_speed()
        self.speed_spinbox.setValue(speed)

    def stop(self):
        self.update_timer.stop()
        self.imu.set_all_data_period(0)
        self.imu.set_orientation_period(0)
        self.imu.set_quaternion_period(0)

    def has_reset_device(self):
        return self.version_minor > 0 or (self.version_minor == 0 and self.version_release > 6)

    def reset_device(self):
        if self.has_reset_device():
            self.imu.reset()

    def get_chip_temperature(self):
        if self.version_minor > 0 or (self.version_minor == 0 and self.version_release > 6):
            return u"{0} °C".format(self.imu.get_chip_temperature() / 10.0)
        else:
            return "(> 1.0.6 needed)"

    @staticmethod
    def has_name(name):
        return "IMU Brick" in name

    def all_data_callback(self, acc_x, acc_y, acc_z, mag_x, mag_y, mag_z, gyr_x, gyr_y, gyr_z, tem):
        self.acc_x = acc_x
        self.acc_y = acc_y
        self.acc_z = acc_z
        self.mag_x = mag_x
        self.mag_y = mag_y
        self.mag_z = mag_z
        self.gyr_x = gyr_x
        self.gyr_y = gyr_y
        self.gyr_z = gyr_z
        self.tem = tem

    def quaternion_callback(self, qua_x, qua_y, qua_z, qua_w):
        self.qua_x = qua_x
        self.qua_y = qua_y
        self.qua_z = qua_z
        self.qua_w = qua_w

    def orientation_callback(self, roll, pitch, yaw):
        self.roll = roll
        self.pitch = pitch
        self.yaw = yaw

    def clear_graphs_clicked(self):
        self.counter = 0
        self.mag_plot.clear_graph()
        self.acc_plot.clear_graph()
        self.gyr_plot.clear_graph()
        self.tem_plot.clear_graph()

    def led_clicked(self):
        if "On" in self.led_button.text():
            self.led_button.setText("Turn LEDs Off")
            self.imu.leds_on()
        elif "Off" in self.led_button.text():
            self.led_button.setText("Turn LEDs On")
            self.imu.leds_off()

    def update_data(self):
        self.update_counter += 1

        self.imu_gl.update(self.qua_x, self.qua_y, self.qua_z, self.qua_w)

        if self.update_counter % 2:
            gyr_x = self.gyr_x / 14.375
            gyr_y = self.gyr_y / 14.375
            gyr_z = self.gyr_z / 14.375

            self.acceleration_update(self.acc_x, self.acc_y, self.acc_z)
            self.magnetometer_update(self.mag_x, self.mag_y, self.mag_z)
            self.gyroscope_update(gyr_x, gyr_y, gyr_z)
            self.orientation_update(self.roll, self.pitch, self.yaw)
            self.temperature_update(self.tem)

            self.gyr_plot.add_data(0, self.counter, gyr_x)
            self.gyr_plot.add_data(1, self.counter, gyr_y)
            self.gyr_plot.add_data(2, self.counter, gyr_z)

            self.acc_plot.add_data(0, self.counter, self.acc_x)
            self.acc_plot.add_data(1, self.counter, self.acc_y)
            self.acc_plot.add_data(2, self.counter, self.acc_z)

            self.mag_plot.add_data(0, self.counter, self.mag_x)
            self.mag_plot.add_data(1, self.counter, self.mag_y)
            self.mag_plot.add_data(2, self.counter, self.mag_z)

            self.tem_plot.add_data(0, self.counter, self.tem / 100.0)

            self.counter += 0.1

    def acceleration_update(self, x, y, z):
        x_str = "%g" % x
        y_str = "%g" % y
        z_str = "%g" % z
        self.acc_y_label.setText(y_str)
        self.acc_x_label.setText(x_str)
        self.acc_z_label.setText(z_str)

    def magnetometer_update(self, x, y, z):
        # Earth magnetic field. 0.5 Gauss
        x_str = "%g" % x
        y_str = "%g" % y
        z_str = "%g" % z
        self.mag_x_label.setText(x_str)
        self.mag_y_label.setText(y_str)
        self.mag_z_label.setText(z_str)

    def gyroscope_update(self, x, y, z):
        x_str = "%g" % int(x)
        y_str = "%g" % int(y)
        z_str = "%g" % int(z)
        self.gyr_x_label.setText(x_str)
        self.gyr_y_label.setText(y_str)
        self.gyr_z_label.setText(z_str)

    def orientation_update(self, r, p, y):
        r_str = "%g" % (r / 100)
        p_str = "%g" % (p / 100)
        y_str = "%g" % (y / 100)
        self.roll_label.setText(r_str)
        self.pitch_label.setText(p_str)
        self.yaw_label.setText(y_str)

    def temperature_update(self, t):
        t_str = "%.2f" % (t / 100.0)
        self.tem_label.setText(t_str)

    def calibrate_pressed(self):
        self.stop()
        aw = CalibrateWindow(self)
        aw.setAttribute(Qt.WA_QuitOnClose)
        aw.show()

    def speed_finished(self):
        speed = self.speed_spinbox.value()
        self.imu.set_convergence_speed(speed)
コード例 #7
0
ファイル: imu.py プロジェクト: smunix/brickv
class IMU(PluginBase, Ui_IMU):
    def __init__ (self, ipcon, uid):
        PluginBase.__init__(self, ipcon, uid)
        self.setupUi(self)
        
        self.imu = brick_imu.IMU(self.uid)
        self.device = self.imu
        self.ipcon.add_device(self.imu)
        self.version = '.'.join(map(str, self.imu.get_version()[1]))
        
        self.update_timer = QTimer()
        self.update_timer.timeout.connect(self.update_data)
        
        # Import IMUGLWidget here, not global. If globally included we get
        # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate'
        # as soon as IMU is set as device_class in __init__. 
        # No idea why this happens, doesn't make sense.
        from imu_gl_widget import IMUGLWidget
        
        self.imu_gl = IMUGLWidget(self)
        self.imu_gl.setMinimumSize(200, 200)
        self.min_x = 0
        self.min_y = 0
        self.min_z = 0
        self.max_x = 0 
        self.max_y = 0
        self.max_z = 0
        
        self.counter = 0
        
        self.mag_plot = Plot("Magnetic Field [mG]",
                             [["X", Qt.red],
                              ["Y", Qt.green],
                              ["Z", Qt.blue]])
        self.acc_plot = Plot("Acceleration [mG]",
                             [["X", Qt.red],
                              ["Y", Qt.green],
                              ["Z", Qt.blue]])
        self.gyr_plot = Plot("Angular Velocity [%c/s]" % 0xB0,
                             [["X", Qt.red],
                              ["Y", Qt.green],
                              ["Z", Qt.blue]])
        
        self.tem_plot = Plot("Temperature [%cC]" % 0xB0,
                             [["t", Qt.red]])
        
        self.grid_layout.addWidget(self.gyr_plot, 0, 0)
        self.grid_layout.addWidget(self.acc_plot, 0, 2)
        self.grid_layout.addWidget(self.mag_plot, 0, 4)
        self.grid_layout.addWidget(self.imu_gl, 2, 2)
        self.grid_layout.addWidget(self.tem_plot, 2, 4)
        
        self.save_orientation.clicked.connect(self.imu_gl.save_orientation)
        self.clear_graphs.clicked.connect(self.clear_graphs_clicked)
        
        #max: (189, 769, 467)
        #min: (-872, -252, -392)

    def start(self):
        self.update_timer.start(50)

    def stop(self):
        self.update_timer.stop()
        
    @staticmethod
    def has_name(name):
        return 'IMU Brick' in name 
        
    def clear_graphs_clicked(self):
        self.counter = 0
        self.mag_plot.clear_graph()
        self.acc_plot.clear_graph()
        self.gyr_plot.clear_graph()
        self.tem_plot.clear_graph()
        
    def update_data(self):
        try:
            acc_x, acc_y, acc_z, \
            mag_x, mag_y, mag_z, \
            gyr_x, gyr_y, gyr_z, tem = self.imu.get_all_data()
            qua_x, qua_y, qua_z, qua_w = self.imu.get_quaternion()
            roll, pitch, yaw = self.imu.get_orientation()
        except ip_connection.Error:
            return
        
        gyr_x = gyr_x/14.375
        gyr_y = gyr_y/14.375
        gyr_z = gyr_z/14.375
        tem = 35.0 + (tem + 13200)/280.0
        
        self.acceleration_update(acc_x, acc_y, acc_z)
        self.magnetometer_update(mag_x, mag_y, mag_z)
        self.gyroscope_update(gyr_x, gyr_y, gyr_z)
        self.orientation_update(roll, pitch, yaw)
        self.temperature_update(tem)
        
        #print qua_x, qua_y, qua_z, qua_w
        
#        import math
#        print math.atan2((float((mag_x-self.min_x))/(self.max_x-self.min_x))*2 - 1.0, 
#                         (float((mag_y-self.min_y))/(self.max_y-self.min_y))*2 - 1.0)*360.0/math.pi

        # MAGNETORMETER
        #print mag_x, mag_y, mag_z
        
        #if mag_x > self.max_x:
        #    self.max_x = int(mag_x)
        #if mag_x < self.min_x:
        #    self.min_x = int(mag_x)
            
        #if mag_y > self.max_y:
        #    self.max_y = int(mag_y)
        #if mag_y < self.min_y:
        #    self.min_y = int(mag_y)
            
        #if mag_z > self.max_z:
        #    self.max_z = int(mag_z)
        #if mag_z < self.min_z:
        #    self.min_z = int(mag_z)
            
        # ACCELEROMETER
        #print acc_x, acc_y, acc_z
        
        #if acc_x > self.max_x:
        #    self.max_x = int(acc_x)
        #if acc_x < self.min_x:
        #    self.min_x = int(acc_x)
           
        #if acc_y > self.max_y:
        #    self.max_y = int(acc_y)
        #if acc_y < self.min_y:
        #    self.min_y = int(acc_y)
            
        #if acc_z > self.max_z:
        #    self.max_z = int(acc_z)
        #if acc_z < self.min_z:
        #    self.min_z = int(acc_z)
        
        # GYROSCOPE
        #print self.counter, gyr_x, gyr_y, gyr_z

        #if self.counter % 1000 == 0:
        #    print "avg: " + str((self.max_x/1000.0, self.max_y/1000.0, self.max_z/1000.0))
        #    self.max_x = 0
        #    self.max_y = 0
        #    self.max_z = 0
        #    if self.counter != 0:
        #        exit()
        
        #self.max_x += gyr_x
        #self.max_y += gyr_y
        #self.max_z += gyr_z
        
        #self.counter += 1
            
        #print "max: " + str((self.max_x, self.max_y, self.max_z))
        #print "min: " + str((self.min_x, self.min_y, self.min_z))
        
        
        self.imu_gl.update(qua_x, qua_y, qua_z, qua_w, roll, pitch, yaw)
        #self.imu_gl.update(1.0, 0.0, 0.0, 0.0)
        
        
        self.gyr_plot.add_data(0, self.counter, gyr_x)
        self.gyr_plot.add_data(1, self.counter, gyr_y)
        self.gyr_plot.add_data(2, self.counter, gyr_z)
        
        self.acc_plot.add_data(0, self.counter, acc_x)
        self.acc_plot.add_data(1, self.counter, acc_y)
        self.acc_plot.add_data(2, self.counter, acc_z)
        
        self.mag_plot.add_data(0, self.counter, mag_x)
        self.mag_plot.add_data(1, self.counter, mag_y)
        self.mag_plot.add_data(2, self.counter, mag_z)
        
        self.tem_plot.add_data(0, self.counter, tem)
        
        self.counter += 0.1
        
    def acceleration_update(self, x, y, z):
        x_str = "%g" % x
        y_str = "%g" % y
        z_str = "%g" % z
        self.acc_y_label.setText(y_str)
        self.acc_x_label.setText(x_str)
        self.acc_z_label.setText(z_str)
        
    def magnetometer_update(self, x, y, z):
        # Earth magnetic field. 0.5 Gauss
        x_str = "%g" % x
        y_str = "%g" % y
        z_str = "%g" % z
        self.mag_x_label.setText(x_str)
        self.mag_y_label.setText(y_str)
        self.mag_z_label.setText(z_str)
        
    def gyroscope_update(self, x, y, z):
        x_str = "%g" % int(x)
        y_str = "%g" % int(y)
        z_str = "%g" % int(z)
        self.gyr_x_label.setText(x_str)
        self.gyr_y_label.setText(y_str)
        self.gyr_z_label.setText(z_str)
        
    def orientation_update(self, r, p, y):
        r_str = "%g" % r
        p_str = "%g" % p
        y_str = "%g" % y
        self.roll_label.setText(r_str)
        self.pitch_label.setText(p_str)
        self.yaw_label.setText(y_str)
        
    def temperature_update(self, t):
        t_str = "%.2f" % t
        self.tem_label.setText(t_str)
コード例 #8
0
ファイル: imu.py プロジェクト: smunix/brickv
class IMU(PluginBase, Ui_IMU):
    def __init__(self, ipcon, uid):
        PluginBase.__init__(self, ipcon, uid)
        self.setupUi(self)

        self.imu = brick_imu.IMU(self.uid)
        self.device = self.imu
        self.ipcon.add_device(self.imu)
        self.version = '.'.join(map(str, self.imu.get_version()[1]))

        self.update_timer = QTimer()
        self.update_timer.timeout.connect(self.update_data)

        # Import IMUGLWidget here, not global. If globally included we get
        # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate'
        # as soon as IMU is set as device_class in __init__.
        # No idea why this happens, doesn't make sense.
        from imu_gl_widget import IMUGLWidget

        self.imu_gl = IMUGLWidget(self)
        self.imu_gl.setMinimumSize(200, 200)
        self.min_x = 0
        self.min_y = 0
        self.min_z = 0
        self.max_x = 0
        self.max_y = 0
        self.max_z = 0

        self.counter = 0

        self.mag_plot = Plot("Magnetic Field [mG]",
                             [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])
        self.acc_plot = Plot("Acceleration [mG]",
                             [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])
        self.gyr_plot = Plot("Angular Velocity [%c/s]" % 0xB0,
                             [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]])

        self.tem_plot = Plot("Temperature [%cC]" % 0xB0, [["t", Qt.red]])

        self.grid_layout.addWidget(self.gyr_plot, 0, 0)
        self.grid_layout.addWidget(self.acc_plot, 0, 2)
        self.grid_layout.addWidget(self.mag_plot, 0, 4)
        self.grid_layout.addWidget(self.imu_gl, 2, 2)
        self.grid_layout.addWidget(self.tem_plot, 2, 4)

        self.save_orientation.clicked.connect(self.imu_gl.save_orientation)
        self.clear_graphs.clicked.connect(self.clear_graphs_clicked)

        #max: (189, 769, 467)
        #min: (-872, -252, -392)

    def start(self):
        self.update_timer.start(50)

    def stop(self):
        self.update_timer.stop()

    @staticmethod
    def has_name(name):
        return 'IMU Brick' in name

    def clear_graphs_clicked(self):
        self.counter = 0
        self.mag_plot.clear_graph()
        self.acc_plot.clear_graph()
        self.gyr_plot.clear_graph()
        self.tem_plot.clear_graph()

    def update_data(self):
        try:
            acc_x, acc_y, acc_z, \
            mag_x, mag_y, mag_z, \
            gyr_x, gyr_y, gyr_z, tem = self.imu.get_all_data()
            qua_x, qua_y, qua_z, qua_w = self.imu.get_quaternion()
            roll, pitch, yaw = self.imu.get_orientation()
        except ip_connection.Error:
            return

        gyr_x = gyr_x / 14.375
        gyr_y = gyr_y / 14.375
        gyr_z = gyr_z / 14.375
        tem = 35.0 + (tem + 13200) / 280.0

        self.acceleration_update(acc_x, acc_y, acc_z)
        self.magnetometer_update(mag_x, mag_y, mag_z)
        self.gyroscope_update(gyr_x, gyr_y, gyr_z)
        self.orientation_update(roll, pitch, yaw)
        self.temperature_update(tem)

        #print qua_x, qua_y, qua_z, qua_w

        #        import math
        #        print math.atan2((float((mag_x-self.min_x))/(self.max_x-self.min_x))*2 - 1.0,
        #                         (float((mag_y-self.min_y))/(self.max_y-self.min_y))*2 - 1.0)*360.0/math.pi

        # MAGNETORMETER
        #print mag_x, mag_y, mag_z

        #if mag_x > self.max_x:
        #    self.max_x = int(mag_x)
        #if mag_x < self.min_x:
        #    self.min_x = int(mag_x)

        #if mag_y > self.max_y:
        #    self.max_y = int(mag_y)
        #if mag_y < self.min_y:
        #    self.min_y = int(mag_y)

        #if mag_z > self.max_z:
        #    self.max_z = int(mag_z)
        #if mag_z < self.min_z:
        #    self.min_z = int(mag_z)

        # ACCELEROMETER
        #print acc_x, acc_y, acc_z

        #if acc_x > self.max_x:
        #    self.max_x = int(acc_x)
        #if acc_x < self.min_x:
        #    self.min_x = int(acc_x)

        #if acc_y > self.max_y:
        #    self.max_y = int(acc_y)
        #if acc_y < self.min_y:
        #    self.min_y = int(acc_y)

        #if acc_z > self.max_z:
        #    self.max_z = int(acc_z)
        #if acc_z < self.min_z:
        #    self.min_z = int(acc_z)

        # GYROSCOPE
        #print self.counter, gyr_x, gyr_y, gyr_z

        #if self.counter % 1000 == 0:
        #    print "avg: " + str((self.max_x/1000.0, self.max_y/1000.0, self.max_z/1000.0))
        #    self.max_x = 0
        #    self.max_y = 0
        #    self.max_z = 0
        #    if self.counter != 0:
        #        exit()

        #self.max_x += gyr_x
        #self.max_y += gyr_y
        #self.max_z += gyr_z

        #self.counter += 1

        #print "max: " + str((self.max_x, self.max_y, self.max_z))
        #print "min: " + str((self.min_x, self.min_y, self.min_z))

        self.imu_gl.update(qua_x, qua_y, qua_z, qua_w, roll, pitch, yaw)
        #self.imu_gl.update(1.0, 0.0, 0.0, 0.0)

        self.gyr_plot.add_data(0, self.counter, gyr_x)
        self.gyr_plot.add_data(1, self.counter, gyr_y)
        self.gyr_plot.add_data(2, self.counter, gyr_z)

        self.acc_plot.add_data(0, self.counter, acc_x)
        self.acc_plot.add_data(1, self.counter, acc_y)
        self.acc_plot.add_data(2, self.counter, acc_z)

        self.mag_plot.add_data(0, self.counter, mag_x)
        self.mag_plot.add_data(1, self.counter, mag_y)
        self.mag_plot.add_data(2, self.counter, mag_z)

        self.tem_plot.add_data(0, self.counter, tem)

        self.counter += 0.1

    def acceleration_update(self, x, y, z):
        x_str = "%g" % x
        y_str = "%g" % y
        z_str = "%g" % z
        self.acc_y_label.setText(y_str)
        self.acc_x_label.setText(x_str)
        self.acc_z_label.setText(z_str)

    def magnetometer_update(self, x, y, z):
        # Earth magnetic field. 0.5 Gauss
        x_str = "%g" % x
        y_str = "%g" % y
        z_str = "%g" % z
        self.mag_x_label.setText(x_str)
        self.mag_y_label.setText(y_str)
        self.mag_z_label.setText(z_str)

    def gyroscope_update(self, x, y, z):
        x_str = "%g" % int(x)
        y_str = "%g" % int(y)
        z_str = "%g" % int(z)
        self.gyr_x_label.setText(x_str)
        self.gyr_y_label.setText(y_str)
        self.gyr_z_label.setText(z_str)

    def orientation_update(self, r, p, y):
        r_str = "%g" % r
        p_str = "%g" % p
        y_str = "%g" % y
        self.roll_label.setText(r_str)
        self.pitch_label.setText(p_str)
        self.yaw_label.setText(y_str)

    def temperature_update(self, t):
        t_str = "%.2f" % t
        self.tem_label.setText(t_str)