carinha-control-panel/braco.py

"""Sete articulações do FABRI_CREATOR; calibração independente por saída."""
import copy
import json
import math
from pathlib import Path
import threading
import time
from comum import Conflito, gravar_atomico, ler_json
from servos import integer

NAMES=['Garra','Punho 1','Punho 2','Braço superior','Braço inferior','Antebraço','Base']
CHANNELS=[0,1,2,3,4,5,7]
LEASE_PADRAO=2000   # ms; o painel novo manda prazos curtos e renova enquanto o botão está pressionado
PASSO=.02           # 50 Hz de interpolação no servidor

def velocidade_valida(v):
    if v is None: return None
    if isinstance(v,bool) or not isinstance(v,(int,float)) or not math.isfinite(v) or not 5<=v<=360:
        raise ValueError('Velocidade deve ficar entre 5 e 360 graus/s')
    return float(v)

def prazo_valido(ms):
    if ms is None: return LEASE_PADRAO
    return integer(ms,100,2000)

class Braco:
    def __init__(self,path,servos):
        self.path=Path(path); self.servos=servos
        self.config=dict(calibracao=[dict(canal=i,minimo=1400,maximo=1600) for i in range(8)],posturas=[])
        try: self.config=self.validate(json.loads(self.path.read_text(encoding='utf-8')))
        except (OSError,ValueError,TypeError): pass
        self.atual=None; self.alvo=None; self.velocidade=None; self.lease=0
        threading.Thread(target=self.loop,daemon=True).start()
    def validate_pose(self,pose,calibration):
        if not isinstance(pose,list) or len(pose)!=7: raise ValueError('Esperava sete ângulos')
        pulses={}
        for i,angle in enumerate(pose):
            if isinstance(angle,bool) or not isinstance(angle,(int,float)) or not math.isfinite(angle) or not 0<=angle<=180:
                raise ValueError('Ângulo inválido')
            channel=CHANNELS[i]
            pulses[channel]=self.angle_pulse(angle,calibration[channel].get('centro',1500))
        pulses[6]=self.angle_pulse(180-pose[5],calibration[6].get('centro',1500))
        for channel,pulse in pulses.items():
            row=calibration[channel]
            if not row['minimo']<=pulse<=row['maximo']:
                raise ValueError(f'Canal {channel}: pulso {pulse} µs fora dos limites calibrados')
        return pulses
    def validate(self,data):
        if not isinstance(data,dict): raise ValueError('Objeto obrigatório')
        rows=data.get('calibracao')
        if not isinstance(rows,list) or len(rows)!=8: raise ValueError('Calibre os oito canais')
        calibration=[]
        for i,row in enumerate(rows):
            if not isinstance(row,dict) or row.get('canal')!=i: raise ValueError('Ordem dos canais inválida')
            lo=integer(row.get('minimo'),500,2500); hi=integer(row.get('maximo'),500,2500)
            center=integer(row.get('centro',1500),501,2499)
            if not lo<center<hi: raise ValueError('O centro deve ficar entre mínimo e máximo')
            calibration.append(dict(canal=i,minimo=lo,maximo=hi,centro=center))
        poses=data.get('posturas')
        if not isinstance(poses,list) or len(poses)>100: raise ValueError('Máximo 100 posturas')
        clean=[]
        for pose in poses:
            if not isinstance(pose,dict) or not isinstance(pose.get('nome'),str) or not 1<=len(pose['nome'].strip())<=40:
                raise ValueError('Nome de postura inválido')
            angles=pose.get('angulos'); self.validate_pose(angles,calibration)
            clean.append(dict(nome=pose['nome'].strip(),angulos=angles[:]))
        return dict(calibracao=calibration,posturas=clean)
    def status(self):
        with self.servos.lock:
            result=copy.deepcopy(self.config)
            result.update(disponivel=self.servos.hw is not None,erro=self.servos.error,
                          dono=self.servos.dono,travado=self.servos.travado,atual=self.atual,
                          movendo=self.alvo is not None,
                          articulacoes=[dict(nome=name,canal=ch) for name,ch in zip(NAMES,CHANNELS)])
            return result
    def save(self,data):
        clean=self.validate(data)
        with self.servos.lock:
            if self.servos.active: raise ValueError('Pare os motores antes de salvar')
            gravar_atomico(self.path,clean)
            self.config=clean
        return self.status()
    def escrever(self,pulses,prazo):
        try:
            for channel,pulse in pulses.items():
                self.servos.hw.pulse(channel,pulse)
                self.servos.active[channel]=prazo
        except OSError:
            self.alvo=None; self.servos.stop()
            raise
    def move(self,pose,velocidade=None,ttl_ms=None):
        """Sem velocidade: vai direto (compatível). Com velocidade: o servidor interpola até o alvo.
        Em ambos os casos os motores desligam sozinhos quando o prazo (ttl_ms) vence sem renovação."""
        velocidade=velocidade_valida(velocidade); ttl=prazo_valido(ttl_ms)
        with self.servos.lock:
            pulses=self.validate_pose(pose,self.config['calibracao'])
            if self.servos.hw is None: raise OSError('PCA9685 indisponível')
            self.servos.reservar('braco')
            prazo=time.monotonic()+ttl/1000
            if velocidade is None or self.atual is None:
                self.alvo=None
                self.escrever(pulses,prazo)
                self.atual=[float(a) for a in pose]
                return dict(ok=True,pulsos=pulses,movendo=False)
            self.alvo=[float(a) for a in pose]; self.velocidade=velocidade; self.lease=prazo
            for channel in list(self.servos.active): self.servos.active[channel]=prazo
        return dict(ok=True,pulsos=pulses,movendo=True)
    def cancelar(self):
        with self.servos.lock: self.alvo=None
    def passo(self,agora,dt):
        with self.servos.lock:
            if self.alvo is None: return
            if agora>=self.lease or self.servos.travado or (self.servos.dono!='braco' and self.servos.active):
                self.alvo=None; return
            delta=self.velocidade*dt
            nova=[a+max(-delta,min(delta,t-a)) for a,t in zip(self.atual,self.alvo)]
            try: pulses=self.validate_pose(nova,self.config['calibracao'])
            except ValueError:
                self.alvo=None; return
            self.escrever(pulses,self.lease)
            self.atual=nova
            if nova==self.alvo: self.alvo=None
    def loop(self):
        anterior=time.monotonic()
        while not self.servos.closed.wait(PASSO):
            agora=time.monotonic()
            try: self.passo(agora,agora-anterior)
            except OSError as e: self.servos.error=str(e)
            anterior=agora
    @staticmethod
    def angle_pulse(angle,center):
        return round(500+angle*(center-500)/90 if angle<=90 else center+(angle-90)*(2500-center)/90)
    def calibrate(self,channel,pulse):
        channel=integer(channel,0,7);pulse=integer(pulse,500,2500)
        outputs={channel:pulse}
        if channel in (5,6):
            other=11-channel
            center_sum=self.config['calibracao'][5].get('centro',1500)+self.config['calibracao'][6].get('centro',1500)
            outputs[other]=integer(center_sum-pulse,500,2500)
        with self.servos.lock:
            if self.servos.hw is None: raise OSError('PCA9685 indisponível')
            self.servos.reservar('braco'); self.alvo=None; self.atual=None
            for ch,us in outputs.items():
                self.servos.hw.pulse(ch,us)
                self.servos.active[ch]=time.monotonic()+2
        return dict(ok=True,pulsos=outputs)
    def rotas(self,h,method):
        route=h.path.split('?',1)[0]
        if route not in ('/braco/status','/braco/config','/braco/mover','/braco/parar','/braco/calibrar'): return False
        try:
            if method=='GET' and route=='/braco/status': result=self.status()
            elif method=='POST':
                data=ler_json(h,65536)
                if route=='/braco/config': result=self.save(data)
                elif route=='/braco/mover': result=self.move(data.get('angulos'),data.get('velocidade'),data.get('ttl_ms'))
                elif route=='/braco/calibrar': result=self.calibrate(data.get('canal'),data.get('pulso'))
                elif route=='/braco/parar': self.cancelar(); result=self.servos.stop()
                else: raise ValueError('Rota inválida')
            else:
                h.enviar(json.dumps(dict(erro='Método inválido')),codigo=405); return True
            h.enviar(json.dumps(result,ensure_ascii=False))
        except Conflito as e: h.enviar(json.dumps(dict(erro=str(e)),ensure_ascii=False),codigo=409)
        except (ValueError,TypeError) as e: h.enviar(json.dumps(dict(erro=str(e))),codigo=400)
        except OSError as e: h.enviar(json.dumps(dict(erro=str(e))),codigo=503)
        return True