"""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 lo100: 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.ligar([(channel,prazo) for channel in pulses]) 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) alvo=[float(a) for a in pose] if all(abs(a-t)<.01 for a,t in zip(self.atual,alvo)): # Já no alvo: só renova o prazo (e reenergiza canais que tenham desligado). self.alvo=None self.escrever(pulses,prazo) return dict(ok=True,pulsos=pulses,movendo=False) self.alvo=alvo; 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.ligar([(ch,time.monotonic()+2) for ch in outputs]) 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