139 lines
4.8 KiB
Python
139 lines
4.8 KiB
Python
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 assi import A
|
|
|
|
# from leo import giro
|
|
from robot_class import LazyRobot, Robot
|
|
from robots import leorobot as robot
|
|
|
|
|
|
def main(robot: LazyRobot):
|
|
hub = robot.hub
|
|
db = robot.db
|
|
|
|
def gira_fino_a_quando(gradi: int):
|
|
posizione_iniziale = db.angle()
|
|
gradi_mancanti = gradi - posizione_iniziale
|
|
db.turn(gradi_mancanti)
|
|
# dangolo = db.angle()
|
|
# if angolo < gradi:
|
|
# db.turn(gradi - angolo)
|
|
|
|
def giro_preciso(gradi: int):
|
|
destinazione = gradi
|
|
aumento_v = 30
|
|
posizione_iniziale = db.angle()
|
|
gradi_attuali = round(db.angle(), 1)
|
|
gradi_mancanti_iniziali = gradi - posizione_iniziale
|
|
if gradi_mancanti_iniziali <= 180 and gradi_mancanti_iniziali >= -180:
|
|
curva_normale = True
|
|
else:
|
|
curva_normale = False
|
|
if gradi_mancanti_iniziali > 0:
|
|
gradi_da_percorerre = gradi_mancanti_iniziali - 360
|
|
destinazione = posizione_iniziale + gradi_da_percorerre
|
|
else:
|
|
gradi_da_percorerre = gradi_mancanti_iniziali + 360
|
|
destinazione = posizione_iniziale + gradi_da_percorerre
|
|
print(
|
|
f"curva precisa a {gradi} per andare da {posizione_iniziale} a {destinazione}"
|
|
)
|
|
while (destinazione - gradi_attuali) > 0.2 or (
|
|
destinazione - gradi_attuali
|
|
) < -0.2:
|
|
gradi_mancanti = destinazione - gradi_attuali
|
|
velocita = abs(gradi_mancanti)
|
|
print("Gradi manc: ", gradi_mancanti)
|
|
if gradi_mancanti > 0:
|
|
robot.left.run(velocita + aumento_v)
|
|
robot.right.run(-velocita - aumento_v)
|
|
hub.display.char(">")
|
|
elif gradi_mancanti < 0:
|
|
robot.right.run(velocita + aumento_v)
|
|
robot.left.run(-velocita - aumento_v)
|
|
hub.display.char("<")
|
|
gradi_attuali = round(db.angle(), 1)
|
|
# else:
|
|
# if gradi > 0:
|
|
# if gradi_di_arrivo_calcolati == false:
|
|
# gradi_di_arrivo = 360 - gradi_mancanti
|
|
# gradi_mancanti = gradi_di_arrivo - gradi
|
|
# print("Gradi manc magg: ", gradi_mancanti)
|
|
# if gradi_mancanti < 0:
|
|
# robot.left.run(velocita + aumento_v)
|
|
# robot.right.run(-velocita - aumento_v)
|
|
# hub.display.char(">")
|
|
# elif gradi_mancanti > 0:
|
|
# robot.right.run(velocita + aumento_v)
|
|
# robot.left.run(-velocita - aumento_v)
|
|
# hub.display.char("<")
|
|
# gradi_attuali = round(db.angle(), 1)
|
|
# elif gradi < 0:
|
|
# gradi_di_arrivo = 360 - gradi_mancanti * -1
|
|
# gradi_mancanti = gradi_di_arrivo - gradi
|
|
# print("Gradi manc magg: ", gradi_mancanti)
|
|
# if gradi_mancanti < 0:
|
|
# robot.left.run(velocita + aumento_v)
|
|
# robot.right.run(-velocita - aumento_v)
|
|
# elif gradi_mancanti > 0:
|
|
# robot.right.run(velocita + aumento_v)
|
|
# robot.left.run(-velocita - aumento_v)
|
|
# hub.display.char("<")
|
|
# gradi_attuali = round(db.angle(), 1)
|
|
# if gradi_mancanti > 0:
|
|
# robot.right.run(velocita + aumento_v)
|
|
# robot.left.run(-velocita - aumento_v)
|
|
# hub.display.char(">")
|
|
# elif gradi_mancanti < 0:
|
|
# robot.left.run(velocita + aumento_v)
|
|
# robot.right.run(-velocita - aumento_v)
|
|
# hub.display.char("<")
|
|
# gradi_attuali = round(db.angle(), 1)
|
|
|
|
print("Gradi att: ", gradi_attuali)
|
|
|
|
db.use_gyro(True) # abilitiamo il giroscopio
|
|
# (mm/s, mm/s², deg/s, deg/s²)
|
|
db.settings(100, 100, 90, 50)
|
|
wait(300)
|
|
db.reset(angle=0)
|
|
|
|
# db.straight(100)
|
|
# gira_precisa(90)
|
|
# print(db.angle())
|
|
# db.straight(100)
|
|
# gira_precisa(180)
|
|
# print(db.angle())
|
|
# db.straight(100)
|
|
# gira_precisa(270)
|
|
# print(db.angle())
|
|
# db.straight(100)
|
|
# gira_precisa(360)
|
|
# print(db.angle())
|
|
|
|
giro_preciso(-120)
|
|
print(db.angle())
|
|
giro_preciso(120)
|
|
print(db.angle())
|
|
# gira_precisa(-60)
|
|
# print(db.angle())
|
|
|
|
# gira_fino_a_quando(120)
|
|
# print(db.angle())
|
|
# gira_fino_a_quando(60)
|
|
# print(db.angle())
|
|
# gira_fino_a_quando(-60)
|
|
# print(db.angle())
|
|
|
|
print(f"drived {db.distance()}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
import batteria
|
|
from robots import cbrobot as robot
|
|
|
|
main(robot)
|