117 lines
6.4 KiB
Python
117 lines
6.4 KiB
Python
"""Sete articulações do FABRI_CREATOR; calibração independente por saída."""
|
|
import copy
|
|
import json
|
|
import math
|
|
from pathlib import Path
|
|
import time
|
|
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]
|
|
|
|
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
|
|
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,
|
|
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')
|
|
tmp=self.path.with_suffix('.tmp'); tmp.write_text(json.dumps(clean,ensure_ascii=False,indent=2),encoding='utf-8'); tmp.replace(self.path)
|
|
self.config=clean
|
|
return self.status()
|
|
def move(self,pose):
|
|
with self.servos.lock:
|
|
pulses=self.validate_pose(pose,self.config['calibracao'])
|
|
if self.servos.hw is None: raise OSError('PCA9685 indisponível')
|
|
try:
|
|
for channel,pulse in pulses.items():
|
|
self.servos.hw.pulse(channel,pulse)
|
|
self.servos.active[channel]=time.monotonic()+2
|
|
except OSError:
|
|
self.servos.stop()
|
|
raise
|
|
return dict(ok=True,pulsos=pulses)
|
|
@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')
|
|
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':
|
|
origin=h.headers.get('Origin')
|
|
if origin and origin not in ('http://'+h.headers.get('Host',''),'https://'+h.headers.get('Host','')): raise ValueError('Origem inválida')
|
|
if h.headers.get('Content-Type','').split(';')[0]!='application/json': raise ValueError('Use JSON')
|
|
size=int(h.headers.get('Content-Length','0'))
|
|
if not 0<size<=65536: raise ValueError('Tamanho inválido')
|
|
data=json.loads(h.rfile.read(size))
|
|
if not isinstance(data,dict): raise ValueError('Objeto obrigatório')
|
|
if route=='/braco/config': result=self.save(data)
|
|
elif route=='/braco/mover': result=self.move(data.get('angulos'))
|
|
elif route=='/braco/calibrar': result=self.calibrate(data.get('canal'),data.get('pulso'))
|
|
elif route=='/braco/parar': 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 (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
|