senpai/FLL
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

robot.py funzionante

Browse source
This commit is contained in:
senpai 2025-10-12 15:55:53 +02:00
parent e291375c25
commit f5048c4413
6 changed files with 255 additions and 100 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

146
batteria.py
View file

@ -1,4 +1,5 @@
from pybricks.hubs import PrimeHub from pybricks.hubs import PrimeHub
from pybricks.tools import wait
hub = PrimeHub() hub = PrimeHub()
@ -14,40 +15,125 @@ class Colors:
NC: str = "\033[0m" NC: str = "\033[0m"
def calcola_carica(v_mv: float) -> str: # ======= SINGLE SOURCE OF TRUTH =======
v: float = v_mv / 1000 # 1) Curva OCV (Volt pacco 2S Li-ion -> percentuale SOC)
carica = "" # Punti ordinati per tensione, usati per un'interpolazione lineare a tratti.
if v >= 8.4: OCV_POINTS: tuple[tuple[float, int], ...] = (
carica = f"{Colors.BLU}CARICA" (6.00, 0), # ~3.0 V/cella: fine scarica tipica
elif v >= 7.4: (6.40, 5),
carica = f"{Colors.GRE}MEDIA" (6.60, 12),
elif v > 6.8: (6.80, 20),
carica = f"{Colors.YEL}QUASI SCARICA" (7.00, 30),
else: (7.20, 45),
carica = f"{Colors.RED}SCARICA" (7.40, 60),
return carica + Colors.NC (7.60, 72),
(7.80, 82),
(8.00, 90),
(8.19, 97), # ~soglia "full" (LED verde) osservata su Pybricks
(8.30, 100), # piena carica "reale"
(8.40, 100), # tetto CV
)
# 2) Stati nominali in funzione della percentuale calcolata dalla curva OCV.
# (min_percent_incluso, Nome, Colore)
BATTERY_STATES: tuple[tuple[int, str, str], ...] = (
(90, "PIENA", Colors.BLU),
(60, "ALTA", Colors.GRE),
(30, "MEDIA", Colors.CYA),
(15, "BASSA", Colors.YEL),
(0, "CRITICA", Colors.RED),
)
# ======================================
def calcola_perc(v_mv: float) -> float: def _clamp(x: float, lo: float, hi: float) -> float:
v: float = v_mv / 1000.0 return hi if x > hi else lo if x < lo else x
v_min: float = 6.8
v_max: float = 8.5
perc: float = (v - v_min) / (v_max - v_min) * 100.0
if perc < 0.0:
return 0.0
if perc > 100.0:
return 100.0
return perc
def print_carica(): def _interp_soc_from_voltage(v_pack: float) -> int:
v_mv = hub.battery.voltage() # millivolt """Interpolazione lineare a tratti sulla curva OCV."""
i_ma = hub.battery.current() # milliampere (corrente assorbita) v = _clamp(v_pack, OCV_POINTS[0][0], OCV_POINTS[-1][0])
print(f"Voltage: {v_mv} mV | Current: {i_ma} mA")
# print(f"Caricatore: {}") # nodo esatto?
print(f"Batteria: {calcola_carica(v_mv)} ({calcola_perc(v_mv)}%)") for vp, sp in OCV_POINTS:
if abs(v - vp) < 1e-6:
return int(sp)
# trova il segmento [i, i+1] e interpola
for i in range(len(OCV_POINTS) - 1):
v0, s0 = OCV_POINTS[i]
v1, s1 = OCV_POINTS[i + 1]
if v0 <= v <= v1:
t = (v - v0) / (v1 - v0)
s = s0 + t * (s1 - s0)
return int(round(s))
return 0 # fallback (non dovrebbe accadere)
print_carica() def _avg_voltage_mv(samples: int = 5, delay_ms: int = 40) -> int:
"""Media semplice per mitigare il sag sotto carico motori."""
total = 0
n = max(1, samples)
for _ in range(n):
total += hub.battery.voltage() # mV
wait(delay_ms)
return total // n
def _state_from_percent(percent: int) -> tuple[str, str]:
"""Mappa la % calcolata agli stati nominali (nome+colore)."""
p = max(0, min(100, percent))
for p_min, name, color in BATTERY_STATES:
if p >= p_min:
return name, color
# fallback al peggior stato
return BATTERY_STATES[-1][1], BATTERY_STATES[-1][2]
def calcola_perc(v_mv: float, consider_charger: bool = True) -> int:
"""Restituisce %SOC intera da 0..100 usando la curva OCV."""
v = v_mv / 1000.0
if consider_charger:
# Se il caricatore dice "full", forza 100%
try:
st = hub.charger.status() # 0=off, 1=rosso, 2=verde, 3=giallo
if st == 2: # verde
return 100
except Exception:
pass
return _interp_soc_from_voltage(v)
def calcola_stato(percent: int) -> str:
"""Restituisce stringa colorata con il nome dello stato."""
name, color = _state_from_percent(percent)
return f"{color}{name}{Colors.NC}"
def print_carica(samples: int = 6) -> None:
v_mv = _avg_voltage_mv(samples=samples)
i_ma = hub.battery.current()
perc = calcola_perc(v_mv, consider_charger=True)
stato = calcola_stato(perc)
# Info caricatore (se presente)
charging_flag = ""
try:
st = hub.charger.status()
if st == 1:
charging_flag = " | In carica"
elif st == 2:
charging_flag = " | Carica completata"
except Exception:
pass
print(f"Voltage: {v_mv} mV | Current: {i_ma} mA{charging_flag}")
print(f"Batteria: {stato} ({perc}%)")
print_carica(samples=6)
# if __name__ == "__main__": # if __name__ == "__main__":
# print_carica() # print_carica(samples=6)

10
guida_funzionante.py
View file

@ -6,6 +6,10 @@ from pybricks.tools import wait
import batteria import batteria
from assi import A from assi import A
from robots import leorobot as robot
hub = robot.hub
db = robot.db
def gira_fino_a_quando(gradi: int): def gira_fino_a_quando(gradi: int):
@ -17,12 +21,6 @@ def gira_fino_a_quando(gradi: int):
# db.turn(gradi - angolo) # db.turn(gradi - angolo)
left = Motor(Port.B, Direction.COUNTERCLOCKWISE)
right = Motor(Port.A)
hub = PrimeHub(top_side=A.UP, front_side=A.BACKWARD)
db = DriveBase(left, right, wheel_diameter=56, axle_track=105)
db.use_gyro(True) # abilitiamo il giroscopio db.use_gyro(True) # abilitiamo il giroscopio
# (mm/s, mm/s², deg/s, deg/s²) # (mm/s, mm/s², deg/s, deg/s²)
db.settings(100, 100, 90, 50) db.settings(100, 100, 90, 50)

30
guida_funzionante_2.py
View file

@ -1,30 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.parameters import Axis, Direction, Port
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase
from pybricks.tools import wait
from robot import CBHUB as HUB
# import batteria
hub = HUB.hub
db = HUB.db
db.use_gyro(True)
# (mm/s, mm/s², deg/s, deg/s²)
db.settings(100, 100, 90, 50)
wait(300)
db.reset(angle=0)
db.straight(100)
db.turn(90)
db.straight(100)
db.turn(90)
db.straight(100)
db.turn(90)
db.straight(100)
db.turn(90)
print(f"drived {db.distance()}")

53
old/batteria.py Normal file
View file

@ -0,0 +1,53 @@
from pybricks.hubs import PrimeHub
hub = PrimeHub()
class Colors:
RED: str = "\033[0;31m"
GRE: str = "\033[0;32m"
YEL: str = "\033[0;33m"
BLU: str = "\033[0;34m"
MAG: str = "\033[0;35m"
CYA: str = "\033[0;36m"
WHI: str = "\033[0;37m"
NC: str = "\033[0m"
def calcola_carica(v_mv: float) -> str:
v: float = v_mv / 1000
carica = ""
if v >= 8.4:
carica = f"{Colors.BLU}CARICA"
elif v >= 7.4:
carica = f"{Colors.GRE}MEDIA"
elif v > 6.8:
carica = f"{Colors.YEL}QUASI SCARICA"
else:
carica = f"{Colors.RED}SCARICA"
return carica + Colors.NC
def calcola_perc(v_mv: float) -> float:
v: float = v_mv / 1000.0
v_min: float = 6.8
v_max: float = 8.5
perc: float = (v - v_min) / (v_max - v_min) * 100.0
if perc < 0.0:
return 0.0
if perc > 100.0:
return 100.0
return perc
def print_carica():
v_mv = hub.battery.voltage() # millivolt
i_ma = hub.battery.current() # milliampere (corrente assorbita)
print(f"Voltage: {v_mv} mV | Current: {i_ma} mA")
# print(f"Caricatore: {}")
print(f"Batteria: {calcola_carica(v_mv)} ({calcola_perc(v_mv)}%)")
print_carica()
# if __name__ == "__main__":
# print_carica()

61
robot.py → robot_class.py
View file

@ -93,38 +93,31 @@ class Robot:
self.db.reset() self.db.reset()
LEOHUB: Robot = Robot( class LazyRobot:
name="leohub", """Proxy che istanzia Robot solo al primo accesso a un attributo.
top_side=A.UP, Compatibile con MicroPython Pybricks (niente 'typing' e niente 'property').
front_side=A.BACKWARD, """
left_port=Port.B,
right_port=Port.A,
left_direction=Direction.COUNTERCLOCKWISE,
right_direction=Direction.CLOCKWISE,
wheel_diameter=56,
axle_track=105,
use_gyro=True,
straight_speed=DEFAULT_STRAIGHT_SPEED,
straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION,
turn_rate=DEFAULT_TURN_RATE,
turn_acceleration=DEFAULT_TURN_ACCELERATION,
settle_ms=DEFAULT_GYRO_SETTLE_MS,
)
CBHUB: Robot = Robot( # --- DICHIARAZIONI TIPI PER IL TYPE CHECKER (statiche, nessun costo runtime) ---
name="cbhub", name: "str"
top_side=A.UP, hub: "PrimeHub"
front_side=A.RIGHT, left: "Motor"
left_port=Port.B, right: "Motor"
right_port=Port.A, db: "DriveBase"
left_direction=Direction.COUNTERCLOCKWISE,
right_direction=Direction.CLOCKWISE, # stato interno del proxy
wheel_diameter=49.5, _params: "dict[str, object]"
axle_track=88, _robot: "Robot | None"
use_gyro=True,
straight_speed=DEFAULT_STRAIGHT_SPEED, def __init__(self, **params) -> None:
straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION, self._params = params
turn_rate=DEFAULT_TURN_RATE, self._robot = None
turn_acceleration=DEFAULT_TURN_ACCELERATION,
settle_ms=DEFAULT_GYRO_SETTLE_MS, def _ensure(self) -> "Robot":
) if self._robot is None:
self._robot = Robot(**self._params)
return self._robot
def __getattr__(self, name: "str") -> "object":
# Delega qualsiasi attributo (hub, db, left, right, name, …)
return getattr(self._ensure(), name)

55
robots.py Normal file
View file

@ -0,0 +1,55 @@
from pybricks.hubs import PrimeHub
from pybricks.parameters import Axis, Direction, Port, Stop
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase
from pybricks.tools import wait
from assi import A
from robot_class import (
DEFAULT_GYRO_SETTLE_MS,
DEFAULT_STRAIGHT_ACCELERATION,
DEFAULT_STRAIGHT_SPEED,
DEFAULT_TURN_ACCELERATION,
DEFAULT_TURN_RATE,
LazyRobot,
Robot,
)
# robot.py (estratto rilevante)
leorobot = LazyRobot(
name="leohub",
top_side=A.UP,
front_side=A.BACKWARD,
left_port=Port.B,
right_port=Port.A,
left_direction=Direction.COUNTERCLOCKWISE,
right_direction=Direction.CLOCKWISE,
wheel_diameter=56,
axle_track=105,
use_gyro=True,
straight_speed=DEFAULT_STRAIGHT_SPEED,
straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION,
turn_rate=DEFAULT_TURN_RATE,
turn_acceleration=DEFAULT_TURN_ACCELERATION,
settle_ms=DEFAULT_GYRO_SETTLE_MS,
)
cbrobot = LazyRobot(
name="cbhub",
top_side=A.UP,
front_side=A.RIGHT,
left_port=Port.B,
right_port=Port.A,
left_direction=Direction.COUNTERCLOCKWISE,
right_direction=Direction.CLOCKWISE,
wheel_diameter=49.5,
axle_track=88,
use_gyro=True,
straight_speed=DEFAULT_STRAIGHT_SPEED,
straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION,
turn_rate=DEFAULT_TURN_RATE,
turn_acceleration=DEFAULT_TURN_ACCELERATION,
settle_ms=DEFAULT_GYRO_SETTLE_MS,
)