diff --git a/giro_leo_prova.py b/giro_leo_prova.py index d94c661..de11afa 100644 --- a/giro_leo_prova.py +++ b/giro_leo_prova.py @@ -5,8 +5,9 @@ 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 cbrobot as robot +from robots import leorobot as robot def main(robot: LazyRobot): @@ -21,23 +22,77 @@ def main(robot: LazyRobot): # if angolo < gradi: # db.turn(gradi - angolo) - def gira_precisa(gradi: int): + def giro_preciso(gradi: int): + destinazione = gradi aumento_v = 30 posizione_iniziale = db.angle() gradi_attuali = round(db.angle(), 1) - while (gradi - gradi_attuali) > 0.2 or (gradi - gradi_attuali) < -0.2: - gradi_mancanti = gradi - gradi_attuali + 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 = gradi_attuali - destinazione 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) + if curva_normale: + 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) @@ -60,12 +115,12 @@ def main(robot: LazyRobot): # gira_precisa(360) # print(db.angle()) - gira_precisa(120) + giro_preciso(-120) print(db.angle()) - gira_precisa(60) - print(db.angle()) - gira_precisa(-60) + giro_preciso(120) print(db.angle()) + # gira_precisa(-60) + # print(db.angle()) # gira_fino_a_quando(120) # print(db.angle()) @@ -79,6 +134,6 @@ def main(robot: LazyRobot): if __name__ == "__main__": import batteria - from robots import leorobot as robot + from robots import cbrobot as robot main(robot)