Adding Joystick for ESP32 STage Manager, Updating ESPSerialCamera

Author: beniroquai

imswitch/imcommon/view/guitools/__init__.py

@@ -1,4 +1,5 @@
 from .BetterPushButton import BetterPushButton
+from .joystick import Joystick
 from .BetterSlider import BetterSlider
 from .CheckableComboBox import CheckableComboBox
 from .FloatSlider import FloatSlider

imswitch/imcommon/view/guitools/joystick.py

@@ -0,0 +1,167 @@
+import sys
+import math
+
+from PyQt5.QtWidgets import QApplication, QWidget
+from PyQt5.QtWidgets import QLabel
+from PyQt5.QtGui import QPainter, QBrush, QPen
+from PyQt5.QtCore import Qt
+from qtpy import QtCore
+
+class Joystick(QWidget):
+    floatValueChanged = QtCore.Signal(float)
+    ''' based on https://github.com/bsiyoung/PyQt5-Joystick/ '''
+
+    def __init__(self, window_min_size = [200, 200], callbackFct=None):
+        super().__init__()
+
+        self.window_title = 'Joystick'
+        self.window_min_size = window_min_size 
+        self.wnd_fit_size = 400
+        self.window_size = [self.wnd_fit_size, self.wnd_fit_size]
+
+        self.circle_margin_ratio = 0.1
+        self.circle_diameter = int(self.window_size[0] * (1 - self.circle_margin_ratio * 2))
+
+        self.stick_diameter_ratio = 0.1
+        self.stick_diameter = int(self.circle_diameter * self.stick_diameter_ratio)
+        self.is_mouse_down = False
+        self.stick_pos = [0, 0]
+        self.strength = 0
+
+        self.stat_label_margin = 10
+        self.stat_label = QLabel(self)
+        
+        self.callbackFct = callbackFct
+        
+        self.init_ui()
+
+    def init_ui(self):
+        self.setWindowTitle(self.window_title)
+
+        self.setMinimumSize(self.window_min_size[0], self.window_min_size[1])
+        self.resize(self.window_size[0], self.window_size[1])
+
+        self.stat_label.setAlignment(Qt.AlignLeft)
+        self.stat_label.setGeometry(self.stat_label_margin, self.stat_label_margin,
+                                    self.window_min_size[0] - self.stat_label_margin * 2,
+                                    self.window_min_size[0] - self.stat_label_margin * 2)
+        font = self.stat_label.font()
+        font.setPointSize(10)
+
+        self.setMouseTracking(True)
+
+        self.show()
+
+    def resizeEvent(self, event):
+        self.wnd_fit_size = min(self.width(), self.height())
+
+        self.circle_diameter = int(self.wnd_fit_size * (1 - self.circle_margin_ratio * 2))
+        self.stick_diameter = int(self.circle_diameter * self.stick_diameter_ratio)
+
+    def _draw_outer_circle(self, painter):
+        painter.setPen(QPen(Qt.black, 2, Qt.SolidLine))
+
+        circle_margin = int(self.wnd_fit_size * self.circle_margin_ratio)
+        painter.drawEllipse(circle_margin, circle_margin,
+                            self.circle_diameter, self.circle_diameter)
+
+    def _draw_sub_lines(self, painter):
+        painter.setRenderHint(QPainter.Antialiasing)
+        painter.setPen(QPen(Qt.lightGray, 1, Qt.DashLine))
+
+        num_sub_line = 6
+        for i in range(num_sub_line):
+            theta = math.pi / 2 - math.pi * i / num_sub_line
+            x0 = int(self.wnd_fit_size / 2 - self.circle_diameter / 2 * math.cos(theta))
+            y0 = int(self.wnd_fit_size / 2 - self.circle_diameter / 2 * math.sin(theta))
+            x1 = int(self.wnd_fit_size / 2 - self.circle_diameter / 2 * math.cos(theta + math.pi))
+            y1 = int(self.wnd_fit_size / 2 - self.circle_diameter / 2 * math.sin(theta + math.pi))
+            painter.drawLine(x0, y0, x1, y1)
+
+    def _draw_sub_circles(self, painter):
+        painter.setPen(QPen(Qt.lightGray, 1, Qt.DashLine))
+
+        num_sub_circle = 4
+        for i in range(num_sub_circle):
+            sub_radius = int(self.circle_diameter / 2 * (i + 1) / (num_sub_circle + 1))
+            sub_margin = int(self.wnd_fit_size / 2 - sub_radius)
+            painter.drawEllipse(sub_margin, sub_margin, sub_radius * 2, sub_radius * 2)
+
+        # Draw Inner(Joystick) Circle
+        painter.setBrush(QBrush(Qt.black, Qt.SolidPattern))
+        stick_margin = [int(self.wnd_fit_size / 2 + self.stick_pos[0] - self.stick_diameter / 2),
+                        int(self.wnd_fit_size / 2 - self.stick_pos[1] - self.stick_diameter / 2)]
+        painter.drawEllipse(stick_margin[0], stick_margin[1], self.stick_diameter, self.stick_diameter)
+
+    def paintEvent(self, event):
+        painter = QPainter(self)
+
+        # Draw Outer(Main) Circle
+        self._draw_outer_circle(painter)
+
+        # Draw Sub Lines
+        self._draw_sub_lines(painter)
+
+        # Draw Sub Circles
+        self._draw_sub_circles(painter)
+
+        # Change Status Label Text (Angle In Degree)
+        strength = self.get_strength()
+        angle = self.get_angle(in_deg=True)
+        if angle < 0:
+            angle += 360
+        #self.stat_label.setText('Strength : {:.2f} \nDirection : {:.2f}°'.format(strength, angle))
+
+    def mouseMoveEvent(self, event):
+        # Move Stick Only When Mouse Left Button Pressed
+        if self.is_mouse_down is False:
+            return
+
+        # Window Coordinate To Cartesian Coordinate
+        pos = event.pos()
+        stick_pos_buf = [pos.x() - self.wnd_fit_size / 2, self.wnd_fit_size / 2 - pos.y()]
+
+        # If Cursor Is Not In Available Range, Correct It
+        if self._get_strength(stick_pos_buf) > 1.0:
+            theta = math.atan2(stick_pos_buf[1], stick_pos_buf[0])
+            radius = (self.circle_diameter - self.stick_diameter) / 2
+            stick_pos_buf[0] = radius * math.cos(theta)
+            stick_pos_buf[1] = radius * math.sin(theta)
+        
+        # Emit signal #TODO: Not sure if this is the right way to do it
+        if self.callbackFct is not None:
+            self.callbackFct(stick_pos_buf[0], stick_pos_buf[1])
+
+        self.stick_pos = stick_pos_buf
+        self.repaint()
+
+    def mousePressEvent(self, event):
+        if event.button() == Qt.LeftButton:
+            self.is_mouse_down = True
+
+    def mouseReleaseEvent(self, event):
+        if event.button() == Qt.LeftButton:
+            self.is_mouse_down = False
+            self.stick_pos = [0, 0]
+            self.repaint()
+
+    # Get Strength With Argument
+    def _get_strength(self, stick_pos):
+        max_distance = (self.circle_diameter - self.stick_diameter) / 2
+        distance = math.sqrt(stick_pos[0] * stick_pos[0] + stick_pos[1] * stick_pos[1])
+
+        return distance / max_distance
+
+    # Get Strength With Current Stick Position
+    def get_strength(self):
+        max_distance = (self.circle_diameter - self.stick_diameter) / 2
+        distance = math.sqrt(self.stick_pos[0] * self.stick_pos[0] + self.stick_pos[1] * self.stick_pos[1])
+        
+        return distance / max_distance
+
+    def get_angle(self, in_deg=False):
+        angle = math.atan2(self.stick_pos[1], self.stick_pos[0])
+        if in_deg is True:
+            angle = angle * 180 / math.pi
+
+        return angle

imswitch/imcontrol/controller/controllers/JoystickController.py

@@ -0,0 +1,60 @@
+
+from imswitch.imcommon.framework import Signal, Thread, Worker, Mutex
+from imswitch.imcontrol.view import guitools
+from imswitch.imcommon.model import initLogger
+from ..basecontrollers import LiveUpdatedController
+
+
+class JoystickController(LiveUpdatedController):
+    """ Linked to JoystickWidget."""
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        
+        # scaler
+        self.scaler = 1
+        
+        # initialize the positioner
+        self.positioner_name = self._master.positionersManager.getAllDeviceNames()[0]
+        self.positioner = self._master.positionersManager[self.positioner_name]
+        
+        self._widget.sigJoystickXY.connect(self.moveXY)
+        self._widget.sigJoystickZA.connect(self.moveZA)
+        
+    def moveXY(self, x, y):
+        if abs(x)>0 or abs(y) >0:
+            self.positioner.moveForever(speed=(0, x, y, 0), is_stop=False)
+        else:
+            self.stop_x()
+            self.stop_y()
+        return x, y
+    
+    def moveZA(self, a, z):
+        if abs(a)>0 or abs(z) >0:
+            self.positioner.moveForever(speed=(a, 0, 0, z), is_stop=False)
+        else:
+            self.stop_a()
+            self.stop_z()
+        return a, z
+        
+        
+        
+        
+
+
+
+# Copyright (C) 2020-2021 ImSwitch developers
+# This file is part of ImSwitch.
+#
+# ImSwitch is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# ImSwitch is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <https://www.gnu.org/licenses/>.

imswitch/imcontrol/controller/controllers/__init__.py

@@ -8,6 +8,7 @@
 from .EtSTEDController import EtSTEDController
 from .FFTController import FFTController
 from .HoloController import HoloController
+from .JoystickController import JoystickController
 from .HistogrammController import HistogrammController
 from .STORMReconController import STORMReconController
 from .HoliSheetController import HoliSheetController

imswitch/imcontrol/model/interfaces/CameraESP32CamSerial.py

@@ -6,6 +6,7 @@
 import base64
 from PIL import Image
 import io
+import math
 
 class CameraESP32CamSerial:
     def __init__(self, port=None):
@@ -36,6 +37,9 @@ def __init__(self, port=None):
         self.newCommand = ""
         self.exposureTime = -1
         self.gain = -1
+        
+        self.waitForNextFrame = True
+
 
         self.serialdevice = self.connect_to_usb_device()
 
@@ -48,19 +52,34 @@ def connect_to_usb_device(self):
             except:
                 pass
 
-
         ports = serial.tools.list_ports.comports()
         for port in ports:
             if port.manufacturer == self.manufacturer or port.manufacturer=="Microsoft":
                 try:
                     ser = serial.Serial(port.device, baudrate=2000000, timeout=1)
+                    ser.write_timeout = 1
                     print(f"Connected to device: {port.description}")
                     return ser
                 except serial.SerialException:
                     print(f"Failed to connect to device: {port.description}")
         print("No matching USB device found.")
         return None
 
+    def calculate_base64_length(self, width, height):
+        """Calculate the length of a base64 string for an image of given dimensions."""
+        num_bytes = width * height
+        base64_length = math.ceil((num_bytes * 4) / 3)
+        # ensure length is multiple of 4
+        base64_length = base64_length + (4 - base64_length % 4) % 4
+        return base64_length
+
+    def initCam(self):
+        """Initialize the camera."""
+        # adjust exposure time
+        self.serialdevice.write(('t10\n').encode())
+        while(self.serialdevice.read()):
+            pass
+
     def put_frame(self, frame):
         self.frame = frame
         return frame
@@ -126,6 +145,10 @@ def startStreamingThread(self):
             return
         nFrame = 0
         nTrial = 0
+        # Calculate the length of the base64 string
+        base64_length = self.calculate_base64_length(self.SensorWidth, self.SensorHeight)
+        lineBreakLength = 2
+
         while self.isRunning:
             try:
 
@@ -141,18 +164,38 @@ def startStreamingThread(self):
                 time.sleep(.05)
                 # readline of camera
                 frame_size = 320 * 240
+        
+                # Read the base64 string from the serial port
+                lineBreakLength = 2
+                base64_image_string  = self.serialdevice.read(base64_length+lineBreakLength)
+
+                # Decode the base64 string into a 1D numpy array
+                image_bytes = base64.b64decode(base64_image_string)
+                image_1d = np.frombuffer(image_bytes, dtype=np.uint8)
+
+                # Reshape the 1D array into a 2D image
+                frame = image_1d.reshape(self.SensorHeight, self.SensorWidth)
+                                
                 frame_bytes = self.serialdevice.read(frame_size)
                 frame_flat = np.frombuffer(frame_bytes, dtype=np.uint8)
-                self.frame = frame_flat.reshape((240, 320))
 
+                # Display the image
+                if waitForNextFrame:
+                    waitForNextFrame = False
+
+                else:
+                    # publish frame frame
+                    self.frame = frame_flat.reshape((240, 320))
+                
+                '''
                 # find 0,1,0,1... pattern to sync
                 pattern = (0,1,0,1,0,1,0,1,0,1)
                 window_size = len(pattern)
                 for i in range(len(frame_flat) - window_size + 1):
                     # Check if the elements in the current window match the pattern
                     if np.array_equal(frame_flat[i:i+window_size], pattern):
                         break
-                
+                '''                
 
                 nFrame += 1
 
@@ -161,11 +204,15 @@ def startStreamingThread(self):
                 #print(e) # most of the time "incorrect padding of the bytes "
                 nFrame = 0
                 nTrial+=1
-                try:
-                    self.serialdevice.flushInput()
-                    self.serialdevice.flushOutput()
-                except:
+
+                # Clear the serial buffer
+                while(self.serialdevice.read()):
                     pass
+                # Re-initialize the camera
+                self.initCam()
+                waitForNextFrame = True
+                
+                # Attempt a hard reset every 20 errors
                 if nTrial > 10 and type(e)==serial.serialutil.SerialException:
                     try:
                         # close the device - similar to hard reset

imswitch/imcontrol/model/managers/positioners/ESP32StageManager.py

@@ -157,7 +157,7 @@ def setupPIDcontroller(self, PIDactive=1, Kp=100, Ki=10, Kd=1, target=500, PID_u
         return self._motor.set_pidcontroller(PIDactive=PIDactive, Kp=Kp, Ki=Ki, Kd=Kd, target=target,
                                              PID_updaterate=PID_updaterate)
 
-    def moveForever(self, speed=(0, 0, 0), is_stop=False):
+    def moveForever(self, speed=(0, 0, 0, 0), is_stop=False):
         self._motor.move_forever(speed=speed, is_stop=is_stop)
 
     def setEnabled(self, is_enabled):

imswitch/imcontrol/view/ImConMainView.py

@@ -95,6 +95,7 @@ def __init__(self, options, viewSetupInfo, *args, **kwargs):
             'ULenses': _DockInfo(name='uLenses Tool', yPosition=3),
             'FFT': _DockInfo(name='FFT Tool', yPosition=3),
             'Holo': _DockInfo(name='Holo Tool', yPosition=3),
+            'Joystick': _DockInfo(name='Joystick Tool', yPosition=3),
             'Histogramm': _DockInfo(name='Histogramm Tool', yPosition=3),
             'STORMRecon': _DockInfo(name='STORM Recon Tool', yPosition=2),
             'HoliSheet': _DockInfo(name='HoliSheet Tool', yPosition=3),