From f5048c4413c30d06c5703b5b3db14711224e8074 Mon Sep 17 00:00:00 2001 From: senpai Date: Sun, 12 Oct 2025 15:55:53 +0200 Subject: [PATCH] robot.py funzionante --- batteria.py | 146 +++++++++++++++++++++++++++++-------- guida_funzionante.py | 10 +-- guida_funzionante_2.py | 30 -------- old/batteria.py | 53 ++++++++++++++ robot.py => robot_class.py | 61 +++++++--------- robots.py | 55 ++++++++++++++ 6 files changed, 255 insertions(+), 100 deletions(-) delete mode 100644 guida_funzionante_2.py create mode 100644 old/batteria.py rename robot.py => robot_class.py (75%) create mode 100644 robots.py diff --git a/batteria.py b/batteria.py index 1602b42..e993c6b 100644 --- a/batteria.py +++ b/batteria.py @@ -1,4 +1,5 @@ from pybricks.hubs import PrimeHub +from pybricks.tools import wait hub = PrimeHub() @@ -14,40 +15,125 @@ class Colors: 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 +# ======= SINGLE SOURCE OF TRUTH ======= +# 1) Curva OCV (Volt pacco 2S Li-ion -> percentuale SOC) +# Punti ordinati per tensione, usati per un'interpolazione lineare a tratti. +OCV_POINTS: tuple[tuple[float, int], ...] = ( + (6.00, 0), # ~3.0 V/cella: fine scarica tipica + (6.40, 5), + (6.60, 12), + (6.80, 20), + (7.00, 30), + (7.20, 45), + (7.40, 60), + (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: - 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 _clamp(x: float, lo: float, hi: float) -> float: + return hi if x > hi else lo if x < lo else x -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)}%)") +def _interp_soc_from_voltage(v_pack: float) -> int: + """Interpolazione lineare a tratti sulla curva OCV.""" + v = _clamp(v_pack, OCV_POINTS[0][0], OCV_POINTS[-1][0]) + + # nodo esatto? + 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__": -# print_carica() +# print_carica(samples=6) diff --git a/guida_funzionante.py b/guida_funzionante.py index 4262386..be6753f 100644 --- a/guida_funzionante.py +++ b/guida_funzionante.py @@ -6,6 +6,10 @@ from pybricks.tools import wait import batteria from assi import A +from robots import leorobot as robot + +hub = robot.hub +db = robot.db def gira_fino_a_quando(gradi: int): @@ -17,12 +21,6 @@ def gira_fino_a_quando(gradi: int): # 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 # (mm/s, mm/s², deg/s, deg/s²) db.settings(100, 100, 90, 50) diff --git a/guida_funzionante_2.py b/guida_funzionante_2.py deleted file mode 100644 index 2f7544f..0000000 --- a/guida_funzionante_2.py +++ /dev/null @@ -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()}") diff --git a/old/batteria.py b/old/batteria.py new file mode 100644 index 0000000..1602b42 --- /dev/null +++ b/old/batteria.py @@ -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() diff --git a/robot.py b/robot_class.py similarity index 75% rename from robot.py rename to robot_class.py index c781201..6d7b689 100644 --- a/robot.py +++ b/robot_class.py @@ -93,38 +93,31 @@ class Robot: self.db.reset() -LEOHUB: Robot = Robot( - 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, -) +class LazyRobot: + """Proxy che istanzia Robot solo al primo accesso a un attributo. + Compatibile con MicroPython Pybricks (niente 'typing' e niente 'property'). + """ -CBHUB: Robot = Robot( - 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, -) + # --- DICHIARAZIONI TIPI PER IL TYPE CHECKER (statiche, nessun costo runtime) --- + name: "str" + hub: "PrimeHub" + left: "Motor" + right: "Motor" + db: "DriveBase" + + # stato interno del proxy + _params: "dict[str, object]" + _robot: "Robot | None" + + def __init__(self, **params) -> None: + self._params = params + self._robot = None + + 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) diff --git a/robots.py b/robots.py new file mode 100644 index 0000000..d2ef2ab --- /dev/null +++ b/robots.py @@ -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, +)