""" Driver PCA9685 pour contrôle PWM des servomoteurs """ import time from typing import Dict, Optional, List from .exceptions import I2CError from .utils import setup_json_logger try: import smbus2 I2C_AVAILABLE = True except ImportError: I2C_AVAILABLE = False # Registres PCA9685 PCA9685_MODE1 = 0x00 PCA9685_MODE2 = 0x01 PCA9685_PRESCALE = 0xFE PCA9685_LED0_ON_L = 0x06 PCA9685_ALLLED_ON_L = 0xFA # Bits MODE1 MODE1_RESTART = 0x80 MODE1_SLEEP = 0x10 MODE1_ALLCALL = 0x01 MODE1_AI = 0x20 class PCA9685Driver: """Driver pour contrôleur PWM PCA9685""" def __init__(self, address: int = 0x40, frequency: int = 50, bus_num: int = 1): self.address = address self.frequency = frequency self.bus_num = bus_num self._bus: Optional[smbus2.SMBus] = None self._is_initialized = False self._channel_values: Dict[int, int] = {} self.logger = setup_json_logger("/opt/Skull/logs/motion.log") if not I2C_AVAILABLE: self.logger.warning("smbus2 non disponible, mode simulation") self._mock_mode = True else: self._mock_mode = False def initialize(self) -> bool: """Initialise le PCA9685""" try: if self._mock_mode: self.logger.info( f"PCA9685 simulé initialisé @{hex(self.address)}, {self.frequency}Hz" ) self._is_initialized = True return True self._bus = smbus2.SMBus(self.bus_num) # Reset et configuration self._write_reg(PCA9685_MODE1, MODE1_RESTART) time.sleep(0.01) # Configuration MODE1: auto-increment activé mode1 = MODE1_AI | MODE1_ALLCALL self._write_reg(PCA9685_MODE1, mode1) # Configuration MODE2: totem pole self._write_reg(PCA9685_MODE2, 0x04) # Configuration fréquence self._set_frequency(self.frequency) # Wake up mode1 = self._read_reg(PCA9685_MODE1) mode1 &= ~MODE1_SLEEP self._write_reg(PCA9685_MODE1, mode1) time.sleep(0.005) self._is_initialized = True self.logger.info( f"PCA9685 initialisé @{hex(self.address)}, {self.frequency}Hz" ) return True except Exception as e: self.logger.error(f"Erreur initialisation PCA9685: {e}") self._is_initialized = False return False def _write_reg(self, reg: int, value: int) -> None: """Écrit un registre""" if self._mock_mode: return if not self._bus: raise I2CError("Bus I2C non initialisé") try: self._bus.write_byte_data(self.address, reg, value) except Exception as e: raise I2CError(f"Erreur écriture reg {hex(reg)}: {e}") def _read_reg(self, reg: int) -> int: """Lit un registre""" if self._mock_mode: return 0 if not self._bus: raise I2CError("Bus I2C non initialisé") try: return self._bus.read_byte_data(self.address, reg) except Exception as e: raise I2CError(f"Erreur lecture reg {hex(reg)}: {e}") def _set_frequency(self, freq: int) -> None: """Configure la fréquence PWM""" # Formule datasheet: prescale = round(25MHz / (4096 * freq)) - 1 prescale = int(round(25000000.0 / (4096.0 * freq)) - 1) prescale = max(3, min(255, prescale)) # Limites datasheet if not self._mock_mode: # Mise en sleep pour changer prescale old_mode = self._read_reg(PCA9685_MODE1) self._write_reg(PCA9685_MODE1, (old_mode & 0x7F) | MODE1_SLEEP) self._write_reg(PCA9685_PRESCALE, prescale) self._write_reg(PCA9685_MODE1, old_mode) time.sleep(0.005) actual_freq = 25000000.0 / (4096.0 * (prescale + 1)) self.logger.info(f"Fréquence PWM: {actual_freq:.1f}Hz (prescale={prescale})") def set_channel(self, channel: int, ticks: int) -> bool: """Configure un canal PWM""" if not self._is_initialized: self.logger.error("PCA9685 non initialisé") return False if not (0 <= channel <= 15): self.logger.error(f"Canal invalide: {channel}") return False if not (0 <= ticks <= 4095): self.logger.error(f"Valeur ticks invalide: {ticks}") return False try: if self._mock_mode: self._channel_values[channel] = ticks self.logger.debug(f"Canal {channel} → {ticks} ticks (simulé)") return True # Calcul des registres reg_base = PCA9685_LED0_ON_L + 4 * channel if ticks == 0: # PWM OFF (full low) on_low, on_high = 0, 0 off_low, off_high = 0, 0x10 # Bit 12 = full OFF elif ticks >= 4095: # PWM ON (full high) on_low, on_high = 0, 0x10 # Bit 12 = full ON off_low, off_high = 0, 0 else: # PWM normal on_low, on_high = 0, 0 # Start à 0 off_low = ticks & 0xFF off_high = (ticks >> 8) & 0x0F # Écriture en bloc (4 bytes) data = [on_low, on_high, off_low, off_high] self._bus.write_i2c_block_data(self.address, reg_base, data) self._channel_values[channel] = ticks self.logger.debug(f"Canal {channel} → {ticks} ticks") return True except Exception as e: self.logger.error(f"Erreur écriture canal {channel}: {e}") return False def set_multiple_channels(self, channels: Dict[int, int]) -> bool: """Configure plusieurs canaux en une fois""" success = True for channel, ticks in channels.items(): if not self.set_channel(channel, ticks): success = False return success def get_channel_value(self, channel: int) -> Optional[int]: """Retourne la dernière valeur écrite sur un canal""" return self._channel_values.get(channel) def stop_all_channels(self) -> bool: """Arrête tous les canaux (PWM = 0)""" try: if self._mock_mode: self._channel_values.clear() self.logger.info("Tous canaux arrêtés (simulé)") return True # Écriture ALL LED OFF self._bus.write_i2c_block_data( self.address, PCA9685_ALLLED_ON_L, [0, 0, 0, 0x10] # Full OFF ) self._channel_values.clear() self.logger.info("Tous canaux PWM arrêtés") return True except Exception as e: self.logger.error(f"Erreur arrêt canaux: {e}") return False def is_initialized(self) -> bool: """Vérifie si le driver est initialisé""" return self._is_initialized def cleanup(self) -> None: """Nettoyage des ressources""" if self._bus: try: self.stop_all_channels() self._bus.close() except: pass self._bus = None self._is_initialized = False self.logger.info("PCA9685 driver fermé")