vguz_v2/i2c_old/i2c_polling.py

# File: components/i2c/i2c_polling.py

from PyQt6.QtWidgets import QWidget, QVBoxLayout, QLabel, QPushButton, QHBoxLayout
from PyQt6.QtCore import QTimer, Qt
from PyQt6.QtGui import QPainter, QColor, QPen
from components.console.console_registry import log_main_console
from smbus2 import SMBus
import config.config as config
import math

AS5048_ADDR = 0x40
REG_ANGLE_MSB = 0xFE
REG_ANGLE_LSB = 0xFF


class AngleGauge(QWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        self.setMinimumSize(200, 200)
        self.angle = 0  # degrees

    def set_angle(self, angle):
        self.angle = max(0.0, min(90.0, angle))
        self.update()

    def paintEvent(self, event):
        painter = QPainter(self)
        painter.setRenderHint(QPainter.RenderHint.Antialiasing)

        rect = self.rect()
        width = rect.width()
        height = rect.height()
        center_x = width // 2
        bottom_y = height - 10
        length = height - 40

        # Draw vertical Y axis
        pen = QPen(QColor(180, 180, 180), 2)
        painter.setPen(pen)
        painter.drawLine(center_x, bottom_y, center_x, bottom_y - length)

        # Draw needle
        radians = math.radians(self.angle)
        end_x = int(center_x + length * math.sin(radians))
        end_y = int(bottom_y - length * math.cos(radians))

        pen = QPen(QColor(0, 100, 200), 4)
        painter.setPen(pen)
        painter.drawLine(center_x, bottom_y, end_x, end_y)

        # Draw angle value
        painter.setPen(Qt.GlobalColor.black)
        painter.drawText(
            rect,
            int(Qt.AlignmentFlag.AlignTop | Qt.AlignmentFlag.AlignHCenter),
            f"{self.angle:.1f}°",
        )


class I2CPollingWidget(QWidget):
    def __init__(self, bus_id=1, parent=None):
        super().__init__(parent)

        self.label = QLabel("AS5048A Angle Sensor")
        self.gauge = AngleGauge()
        self.zero_raw = None

        self.start_button = QPushButton("Start")
        self.stop_button = QPushButton("Stop")
        self.set_zero_button = QPushButton("Set Zero")

        self.start_button.clicked.connect(self.start_polling)
        self.stop_button.clicked.connect(self.stop_polling)
        self.set_zero_button.clicked.connect(self.set_zero)

        button_layout = QHBoxLayout()
        button_layout.addWidget(self.start_button)
        button_layout.addWidget(self.stop_button)
        button_layout.addWidget(self.set_zero_button)

        layout = QVBoxLayout()
        layout.addWidget(self.label)
        layout.addWidget(self.gauge)
        layout.addLayout(button_layout)
        self.setLayout(layout)

        self.bus_id = bus_id
        self.bus = None
        self.timer = QTimer(self)
        self.timer.timeout.connect(self.read_angle)

        if not config.DEBUG_MODE:
            try:
                self.bus = SMBus(self.bus_id)
                log_main_console("success", f"AS5048A ready on bus {self.bus_id}")
            except Exception as e:
                log_main_console("error", f"Failed to open I2C bus {self.bus_id}: {e}")
        else:
            log_main_console("info", "DEBUG_MODE: fake polling setup complete")

    def start_polling(self):
        self.timer.start(50)
        log_main_console("info", "Started I2C angle polling")

    def stop_polling(self):
        self.timer.stop()
        log_main_console("info", "Stopped I2C angle polling")

    def set_zero(self):
        raw = self._read_raw_angle()
        if raw is not None:
            self.zero_raw = raw
            log_main_console("info", f"Zero set to raw value: {self.zero_raw}")

    def _read_raw_angle(self):
        if config.DEBUG_MODE:
            import random

            return random.randint(0, 16383)
        try:
            msb = self.bus.read_byte_data(AS5048_ADDR, REG_ANGLE_MSB)
            lsb = self.bus.read_byte_data(AS5048_ADDR, REG_ANGLE_LSB)
            raw = ((msb << 6) | (lsb & 0x3F)) & 0x3FFF
            return raw
        except Exception as e:
            log_main_console("error", f"I2C read failed: {e}")
            return None

    def read_angle(self):
        current_raw = self._read_raw_angle()
        if current_raw is None or self.zero_raw is None:
            return

        # Calculate signed delta
        diff = (current_raw - self.zero_raw + 8192) % 16384 - 8192
        angle_deg = -((diff * 360.0) / 16384.0)

        # Clamp to 0–90° range for display
        angle_clamped = max(0.0, min(90.0, angle_deg))

        log_main_console(
            "info",
            f"Read raw: {current_raw}, Δ: {diff}, Degrees: {angle_deg:.2f}° → Gauge: {angle_clamped:.2f}°",
        )
        self.gauge.set_angle(angle_clamped)

    def closeEvent(self, event):
        self.timer.stop()
        if self.bus:
            self.bus.close()
        super().closeEvent(event)