robot.py funzionante
This commit is contained in:
parent
e291375c25
commit
f5048c4413
6 changed files with 255 additions and 100 deletions
146
batteria.py
146
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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
53
old/batteria.py
Normal 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()
|
||||
|
|
@ -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)
|
||||
55
robots.py
Normal file
55
robots.py
Normal 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,
|
||||
)
|
||||
Loading…
Add table
Reference in a new issue