carinha-control-panel/servos.py

"""Testes individuais do PCA9685. Inicialização sem movimento; pulso com prazo."""
import json
import os
from pathlib import Path
import threading
import time
from comum import Conflito, gravar_atomico, ler_json

def integer(value, lo, hi):
    if type(value) is not int or not lo <= value <= hi:
        raise ValueError(f'Esperava inteiro entre {lo} e {hi}')
    return value

class PCA:
    def __init__(self, bus):
        import fcntl
        self.fd = os.open(bus, os.O_RDWR)
        fcntl.ioctl(self.fd, 0x0703, 0x40)
        self.ready = False
    def write(self, reg, values):
        os.write(self.fd, bytes([reg, *values]))
    def read(self, reg):
        os.write(self.fd, bytes([reg]))
        return os.read(self.fd, 1)[0]
    def init(self):
        # All outputs off before changing global frequency; no position at startup.
        self.write(0xFD, [0x10])
        for channel in range(16): self.off(channel)
        old = self.read(0)
        self.write(0, [(old & 0x7F) | 0x10])
        self.write(0xFE, [121])  # 25 MHz / (4096 * (121+1)) ≈ 50 Hz
        self.write(0, [(old & ~0x10) | 0x20])
        time.sleep(.005)
        self.write(0, [(old & ~0x10) | 0xA0])
        self.ready = True
    def pulse(self, channel, us):
        if not self.ready: self.init()
        ticks = round(us * 25 / 122)
        self.write(6 + 4 * channel, [0, 0, ticks & 255, ticks >> 8])
    def off(self, channel):
        self.write(9 + 4 * channel, [0x10])

class ControleServos:
    def __init__(self, arquivo, hardware=None):
        self.path = Path(arquivo)
        self.lock = threading.RLock()
        self.hw = hardware
        self.error = ''
        self.active = {}
        self.dono = None          # 'braco' ou 'diagnostico': quem está com os motores
        self.travado = False      # E-Stop: nada se move até rearmar
        self.publicar = lambda tipo, dados: None
        self.closed = threading.Event()
        self.config = [dict(nome=f'Servo {i+1}', canal=i, minimo=1400, maximo=1600, centro=1500) for i in range(6)]
        try:
            self.config = self.validate(json.loads(self.path.read_text(encoding='utf-8')))
        except (OSError, ValueError): pass
        if self.hw is None and os.name == 'posix':
            bus = os.environ.get('CARINHA_I2C')
            if bus:
                try: self.hw = PCA(bus)
                except OSError as e: self.error = str(e)
        self.worker = threading.Thread(target=self.watch, daemon=True)
        self.worker.start()
    def validate(self, rows):
        if not isinstance(rows, list) or not 1 <= len(rows) <= 16:
            raise ValueError('Use entre 1 e 16 servos')
        result, channels = [], set()
        for row in rows:
            if not isinstance(row, dict): raise ValueError('Servo inválido')
            channel = integer(row.get('canal'), 0, 15)
            if channel in channels: raise ValueError('Canais repetidos')
            channels.add(channel)
            name = row.get('nome')
            if not isinstance(name, str) or not 1 <= len(name.strip()) <= 40: raise ValueError('Nome inválido')
            lo = integer(row.get('minimo'), 1000, 2000)
            hi = integer(row.get('maximo'), 1000, 2000)
            center = integer(row.get('centro'), lo, hi)
            if lo >= hi: raise ValueError('Mínimo deve ser menor que máximo')
            result.append(dict(nome=name.strip(), canal=channel, minimo=lo, maximo=hi, centro=center))
        return result
    def status(self):
        with self.lock:
            return dict(disponivel=self.hw is not None, erro=self.error,
                        servos=[dict(row) for row in self.config], ativos=list(self.active), prazo=2,
                        dono=self.dono, travado=self.travado)
    def motores(self):
        with self.lock:
            return dict(disponivel=self.hw is not None, erro=self.error, ativos=sorted(self.active),
                        dono=self.dono, travado=self.travado)
    def reservar(self, nome):
        """Braço e diagnóstico usam o mesmo PCA9685 com limites diferentes: um de cada vez."""
        with self.lock:
            if self.travado: raise Conflito('E-Stop ativo: rearme antes de mover')
            if self.dono not in (None, nome) and self.active:
                raise Conflito(f'Motores em uso por {self.dono}; pare-os primeiro')
            self.dono = nome
    def save(self, rows):
        rows = self.validate(rows)
        with self.lock:
            if self.active: raise ValueError('Desligue os sinais antes de salvar')
            gravar_atomico(self.path, rows)
            self.config = rows
        return self.status()
    def move(self, index, pulse):
        with self.lock:
            index = integer(index, 0, len(self.config)-1)
            row = self.config[index]
            pulse = integer(pulse, row['minimo'], row['maximo'])
            if self.hw is None: raise OSError('PCA9685 indisponível neste servidor')
            self.reservar('diagnostico')
            self.hw.pulse(row['canal'], pulse)
            self.active[row['canal']] = time.monotonic() + 2
        return self.status()
    def expire(self, now):
        with self.lock:
            liberou = False
            for channel, deadline in list(self.active.items()):
                if now >= deadline:
                    self.hw.off(channel)
                    del self.active[channel]
                    liberou = True
            if liberou and not self.active:
                self.dono = None
                self.publicar('motores', self.motores())
    def stop(self):
        with self.lock:
            if self.hw:
                for channel in range(16): self.hw.off(channel)
            self.active.clear()
            self.dono = None
            self.publicar('motores', self.motores())
        return self.status()
    def watch(self):
        while not self.closed.wait(.1):
            try: self.expire(time.monotonic())
            except OSError as e: self.error = str(e)
    def close(self):
        self.closed.set()
        self.worker.join(timeout=1)
    def rotas(self, h, method):
        route = h.path.split('?', 1)[0]
        if route not in ('/servos/status', '/servos/config', '/servos/mover', '/servos/parar'): return False
        try:
            if method == 'GET' and route == '/servos/status': result = self.status()
            elif method == 'POST':
                data = ler_json(h, 8192)
                if route == '/servos/config': result = self.save(data.get('servos'))
                elif route == '/servos/mover': result = self.move(data.get('indice'), data.get('pulso'))
                elif route == '/servos/parar': result = self.stop()
                else: raise ValueError('Rota inválida')
            else:
                h.enviar(json.dumps({'erro':'Método não permitido'}), codigo=405)
                return True
            h.enviar(json.dumps(result, ensure_ascii=False))
        except Conflito as e:
            h.enviar(json.dumps({'erro':str(e)}, ensure_ascii=False), codigo=409)
        except (ValueError, TypeError) as e:
            h.enviar(json.dumps({'erro':str(e)}), codigo=400)
        except OSError as e:
            h.enviar(json.dumps({'erro':str(e)}), codigo=503)
        return True