From 2685f39dc0410fecd0ccaeb8b00d45cce9da2d1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Tue, 9 May 2023 15:12:56 -0300 Subject: [PATCH 01/88] Suporte para testes de controle --- ALP-Winners/src/loop.py | 6 ++++++ ALP-Winners/src/world/__init__.py | 9 +++++++-- MainSystem/controller/states/debugHLC.py | 11 ++++++----- MainSystem/controller/vision/__init__.py | 5 ++++- MainSystem/controller/world/__init__.py | 4 +++- 5 files changed, 26 insertions(+), 9 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index a37597d..4d7244f 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -69,6 +69,7 @@ def loop(self): # Executa o controle # control_output = [robot.entity.control.actuateNoControl(robot) for robot in self.world.team if robot.entity is not None] + control_output = [] for robot in self.world.team: # if robot.entity.__class__.__name__ == "GoalKeeper": @@ -77,6 +78,11 @@ def loop(self): # print('th_raw:', robot.th_raw, '. th:', self.world.field.side * robot.th_raw) # print('w_raw:', robot.w_raw, '. w:', self.world.field.side * robot.w_raw) + if self.world.enableManualControl: + print("Controle manual") + print("V:", self.world.manualControlSpeedV) + print("W:", self.world.manualControlSpeedW) + if robot.entity is not None: if(robot.entity.__class__.__name__ == "GoalKeeper" or robot.entity.__class__.__name__ == "Defender"): control_output.append(robot.entity.control.actuateNoControl(robot)) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index d32b1e5..357d9b3 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -58,7 +58,9 @@ def __init__(self, n_robots=5, side=1, vss=None, team_yellow=False, immediate_st self.allyGoals = 0 self.enemyGoals = 0 self.updateCount = 0 - self.checkBatteries = False + self.enableManualControl = False + self.manualControlSpeedV = 0 + self.manualControlSpeedW = 0 def update(self, message): if self.team_yellow: @@ -77,7 +79,10 @@ def update(self, message): robot_id+=1 self.ball.update(message[0], message[1], message[2], message[3]) - self.checkBatteries = message[24] + self.enableManualControl = message[24] + self.manualControlSpeedV = message[25] + self.manualControlSpeedW = message[26] + self.updateCount += 1 diff --git a/MainSystem/controller/states/debugHLC.py b/MainSystem/controller/states/debugHLC.py index 53228de..516db1b 100644 --- a/MainSystem/controller/states/debugHLC.py +++ b/MainSystem/controller/states/debugHLC.py @@ -22,6 +22,8 @@ def __init__(self, controller): "manualControlSpeedV": 0, "manualControlSpeedW": 0, "enableManualControl": False, + "manualControlSpeedV" : 0, + "manualControlSpeedW" : 0, "selectedField": "UVF", "selectableFinalPoint": False, "runVision": True, @@ -162,11 +164,6 @@ def update(self): dt = time.time()-self.t self.t = time.time() - #Atualiza o mundo com a nova funcionalidade de enableManualControl, checar baterias - if self.getParam("enableManualControl"): - self.world.checkBatteries = True - else: - self.world.checkBatteries = False # Atualiza o mundo com a visão if self.getParam("runVision"): @@ -183,11 +180,15 @@ def update(self): # Controle manual if self.getParam("enableManualControl"): + self.world.enableManualControl = True + self.world.manualControlSpeedV = self.getParam("manualControlSpeedV") + self.world.manualControlSpeedW = self.getParam("manualControlSpeedW") manualSpeed = SpeedPair(self.getParam("manualControlSpeedV"), self.getParam("manualControlSpeedW")) speeds = [manualSpeed, manualSpeed, manualSpeed] # Controle de alto nível else: + self.world.enableManualControl = False speeds = [r.actuate() for r in self.robots] # Obtém o target instantâneo diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index efd118e..2656ff4 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -61,7 +61,10 @@ def update(self): message.append(self._world.robots[i].inst_w) message.append(self._world.running) - message.append(self._world.checkBatteries) + message.append(self._world.enableManualControl) + message.append(self._world.manualControlSpeedV) + message.append(self._world.manualControlSpeedW) + self.server_pickle.send(message) diff --git a/MainSystem/controller/world/__init__.py b/MainSystem/controller/world/__init__.py index ef7f810..c14f3bd 100644 --- a/MainSystem/controller/world/__init__.py +++ b/MainSystem/controller/world/__init__.py @@ -44,7 +44,9 @@ def __init__(self, n_robots): self.n_robots = n_robots self.fieldSide = Field.RIGHT self.running = False - self.checkBatteries = False + self.enableManualControl = False + self.manualControlSpeedV = 0 + self.manualControlSpeedW = 0 self.mus = [0.07, 0.07, 0.12, 0.07, 0.07] self.robots = [Robot(self, i, self.mus[i]) for i in range(self.n_robots)] self.enemyRobots = [] From 3c0559d522952996c4eb875047ea637509fd4f0e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 16 Jun 2023 19:45:06 -0300 Subject: [PATCH 02/88] envia velocidades do controle manual --- ALP-Winners/src/control/__init__.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index f3b64b1..d187b9c 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -41,9 +41,11 @@ def actuate(self, robot): if not robot.on: return (0, 0) - v, w = self.output(robot) - # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) - # w = 0 + if self.world.enableManualControl: + v = self.world.manualControlSpeedV + w = self.world.manualControlSpeedW + else: + v, w = self.output(robot) robot.lastControlLinVel = v vr, vl = speeds2motors(v, w) From a4eb6da5fd6b6997cc6d651a088693bb9f6c0a1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 24 Jun 2023 16:44:39 -0300 Subject: [PATCH 03/88] cria entidade de teste de controle --- .../src/strategy/entity/controlTest.py | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 ALP-Winners/src/strategy/entity/controlTest.py diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py new file mode 100644 index 0000000..e0e6d8d --- /dev/null +++ b/ALP-Winners/src/strategy/entity/controlTest.py @@ -0,0 +1,49 @@ +from ..entity import Entity +from strategy.field.UVF import UVF +from strategy.field.DirectionalField import DirectionalField +from strategy.field.goalKeeper import GoalKeeperField +from strategy.field.attractive import AttractiveField +from strategy.movements import goalkeep, spinGoalKeeper +from tools import angError, howFrontBall, howPerpBall, ang, norml, norm, angl +from tools.interval import Interval +from control.goalKeeper import GoalKeeperControl +from control.UFC import UFC_Simple +import numpy as np +import math +import time + +class ControlTester(Entity): + def __init__(self, world, robot, side=1): + super().__init__(world, robot) + + self._control = UFC_Simple(self.world) + + @property + def control(self): + return self._control + + def onExit(self): + pass + + def directionDecider(self): + if self.robot.field is not None: + ref_th = self.robot.field.F(self.robot.pose) + rob_th = self.robot.th + + if abs(angError(ref_th, rob_th)) > 90 * np.pi / 180: + self.robot.direction *= -1 + self.lastChat = time.time() + + # Inverter a direção se o robô ficar preso em algo + elif not self.robot.isAlive() and self.robot.spin == 0: + if time.time()-self.lastChat > .3: + self.lastChat = time.time() + self.robot.direction *= -1 + + def fieldDecider(self): + #ir pro ponto 1 + #ir pro ponto 2 + #ir pro ponto 3 + #ir pro ponto 4 + pass + From 3f46bee11dc6474992eb0b7787a68a0506687aa9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Tue, 27 Jun 2023 21:40:24 -0300 Subject: [PATCH 04/88] adiciona def de movimento de teste de controle --- ALP-Winners/src/strategy/movements.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ALP-Winners/src/strategy/movements.py b/ALP-Winners/src/strategy/movements.py index 40aa534..b5b4225 100644 --- a/ALP-Winners/src/strategy/movements.py +++ b/ALP-Winners/src/strategy/movements.py @@ -194,3 +194,11 @@ def intercept(rr, rb, direction, rg, vb, vrref=0.5, arref=1.4): # else: # return False + +def doSquareTest(point1, point2, point3, point4): + #ir para ponto 1 + #ir para ponto 2 + #ir para ponto 3 + #ir para ponto 4 + + return \ No newline at end of file From eaaf0971254c687f3022826e6190ebd6dbe1a4fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Tue, 27 Jun 2023 21:42:40 -0300 Subject: [PATCH 05/88] implementa fieldDecider para entidade teste --- ALP-Winners/src/strategy/entity/controlTest.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py index e0e6d8d..0d13306 100644 --- a/ALP-Winners/src/strategy/entity/controlTest.py +++ b/ALP-Winners/src/strategy/entity/controlTest.py @@ -4,7 +4,7 @@ from strategy.field.goalKeeper import GoalKeeperField from strategy.field.attractive import AttractiveField from strategy.movements import goalkeep, spinGoalKeeper -from tools import angError, howFrontBall, howPerpBall, ang, norml, norm, angl +from tools import angError, howFrontBall, howPerpBall, ang, norml, norm, angl, doSquareTest from tools.interval import Interval from control.goalKeeper import GoalKeeperControl from control.UFC import UFC_Simple @@ -41,9 +41,7 @@ def directionDecider(self): self.robot.direction *= -1 def fieldDecider(self): - #ir pro ponto 1 - #ir pro ponto 2 - #ir pro ponto 3 - #ir pro ponto 4 - pass + Pb = doSquareTest(0,0,0,0) + self.robot.field = UVF(Pb) + self._control = UFC_Simple(self.world) From e4a18b4381aafcc335a245d6b44a6d5fcfaabfd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Tue, 27 Jun 2023 21:43:50 -0300 Subject: [PATCH 06/88] remove isAlive da entidade teste --- ALP-Winners/src/strategy/entity/controlTest.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py index 0d13306..98f42c4 100644 --- a/ALP-Winners/src/strategy/entity/controlTest.py +++ b/ALP-Winners/src/strategy/entity/controlTest.py @@ -33,12 +33,6 @@ def directionDecider(self): if abs(angError(ref_th, rob_th)) > 90 * np.pi / 180: self.robot.direction *= -1 self.lastChat = time.time() - - # Inverter a direção se o robô ficar preso em algo - elif not self.robot.isAlive() and self.robot.spin == 0: - if time.time()-self.lastChat > .3: - self.lastChat = time.time() - self.robot.direction *= -1 def fieldDecider(self): Pb = doSquareTest(0,0,0,0) From bf372066f58e6f330475c9615ad153fdfd4bf284 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 1 Jul 2023 18:41:06 -0300 Subject: [PATCH 07/88] implementa ir p direita e esquerda --- .../src/strategy/entity/controlTest.py | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py index 98f42c4..773cfab 100644 --- a/ALP-Winners/src/strategy/entity/controlTest.py +++ b/ALP-Winners/src/strategy/entity/controlTest.py @@ -4,7 +4,7 @@ from strategy.field.goalKeeper import GoalKeeperField from strategy.field.attractive import AttractiveField from strategy.movements import goalkeep, spinGoalKeeper -from tools import angError, howFrontBall, howPerpBall, ang, norml, norm, angl, doSquareTest +from tools import angError, howFrontBall, howPerpBall, ang, norml, norm, angl from tools.interval import Interval from control.goalKeeper import GoalKeeperControl from control.UFC import UFC_Simple @@ -17,10 +17,14 @@ def __init__(self, world, robot, side=1): super().__init__(world, robot) self._control = UFC_Simple(self.world) + self.lastChat = 0 @property def control(self): return self._control + + def equalsTo(self, otherTester): + return True def onExit(self): pass @@ -34,8 +38,16 @@ def directionDecider(self): self.robot.direction *= -1 self.lastChat = time.time() + # Inverter a direção se o robô ficar preso em algo + elif not self.robot.isAlive() and self.robot.spin == 0: + self.lastChat = time.time() + self.robot.direction *= -1 + def fieldDecider(self): - Pb = doSquareTest(0,0,0,0) - self.robot.field = UVF(Pb) - self._control = UFC_Simple(self.world) + rr = np.array(self.robot.pos) + + if rr[0] > 0.4: + self.robot.field = DirectionalField(np.pi) + if rr[0] < -0.4: + self.robot.field = DirectionalField(0) From edc6b2d9051b0c3cd0e983053922094eb9623cc8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 1 Jul 2023 18:41:55 -0300 Subject: [PATCH 08/88] aplica entidade teste com controle man --- ALP-Winners/src/strategy/__init__.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ALP-Winners/src/strategy/__init__.py b/ALP-Winners/src/strategy/__init__.py index c1fdac5..c28bce8 100644 --- a/ALP-Winners/src/strategy/__init__.py +++ b/ALP-Winners/src/strategy/__init__.py @@ -3,6 +3,7 @@ from .entity.goalKeeper import GoalKeeper from .entity.defender import Defender from .entity.midfielder import Midfielder +from .entity.controlTest import ControlTester #from client.protobuf.vssref_common_pb2 import Foul from client.referee import RefereeCommands from tools import sats, norml, unit, angl, angError, projectLine, howFrontBall, norm, bestWithHyst @@ -140,6 +141,11 @@ def update(self): self.world.team[1].updateEntity(Attacker) self.world.team[0].updateEntity(GoalKeeper) + elif self.world.enableManualControl: + self.world.team[2].updateEntity(Attacker) + self.world.team[1].updateEntity(Attacker) + self.world.team[0].updateEntity(ControlTester) + else: formation = self.formationDecider() toDecide = self.availableRobotIndexes() From 11ac89be84fb94f12325a8a31c76262ba7bcf0c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 24 Jun 2023 16:19:44 -0300 Subject: [PATCH 09/88] =?UTF-8?q?controle=20antigo=20agora=20=C3=A9=20actu?= =?UTF-8?q?ateOldControl?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/control/__init__.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index ec52570..f6981f6 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -23,6 +23,20 @@ def actuate(self, robot): vr, vl = speeds2motors(v, w) + vr = int(deadzone(sat(vr, 255), 32, -32)) + vl = int(deadzone(sat(vl, 255), 32, -32)) + + return vr, vl + + def actuateOldControl(self, robot): + if not robot.on: + return (0, 0) + + v, w = self.output(robot) + robot.lastControlLinVel = v + + vr, vl = speeds2motors(v, w) + vr = int(deadzone(sat(vr, 255), 32, -32)) vl = int(deadzone(sat(vl, 255), 32, -32)) From c0b634fa53fbb0844731654bbd3a59fac61c2211 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Tue, 27 Jun 2023 18:02:05 -0300 Subject: [PATCH 10/88] remove controle antigo --- ALP-Winners/src/control/__init__.py | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index f6981f6..7a40785 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -23,20 +23,6 @@ def actuate(self, robot): vr, vl = speeds2motors(v, w) - vr = int(deadzone(sat(vr, 255), 32, -32)) - vl = int(deadzone(sat(vl, 255), 32, -32)) - - return vr, vl - - def actuateOldControl(self, robot): - if not robot.on: - return (0, 0) - - v, w = self.output(robot) - robot.lastControlLinVel = v - - vr, vl = speeds2motors(v, w) - vr = int(deadzone(sat(vr, 255), 32, -32)) vl = int(deadzone(sat(vl, 255), 32, -32)) @@ -52,4 +38,3 @@ def actuateOldControl(self, robot): return vr, vl - From 957e6a262a2b1dffdd8bea1c836fa848784cb9b3 Mon Sep 17 00:00:00 2001 From: adornelas Date: Sat, 1 Jul 2023 19:37:53 -0300 Subject: [PATCH 11/88] =?UTF-8?q?Revert=20"21=20minimizar=20prints=20duran?= =?UTF-8?q?te=20execu=C3=A7=C3=A3o=20do=20malp"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/loop.py | 5 +---- ALP-Winners/src/main.py | 4 +--- MainSystem/controller/__init__.py | 6 ++---- MainSystem/controller/vision/__init__.py | 4 ++-- MainSystem/controller/vision/mainVision/__init__.py | 4 ++-- MainSystem/controller/vision/server_pickle.py | 7 ++----- MainSystem/main.py | 3 +-- MainSystem/view/vision/mainVision/visaoAltoNivel.py | 1 + README.md | 5 +---- 9 files changed, 13 insertions(+), 26 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 41129b3..0ffdf7e 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -24,7 +24,6 @@ def __init__( static_entities=False, port=5001, n_robots=3, - debug=False ): # Instancia interface com o simulador #self.vss = VSS(team_yellow=team_yellow) @@ -45,7 +44,6 @@ def __init__( self.radio = SerialRadio() self.execute = False self.t0 = time.time() - self.debug= debug # Interface gráfica para mostrar campos self.draw_uvf = draw_uvf @@ -60,8 +58,7 @@ def __init__( def loop(self): if self.world.updateCount == self.lastupdatecount: return - if(self.debug): print((time.time()-self.t0)*1000) - + print((time.time()-self.t0)*1000) self.t0 = time.time() self.lastupdatecount = self.world.updateCount diff --git a/ALP-Winners/src/main.py b/ALP-Winners/src/main.py index 36d39d2..23e2e79 100644 --- a/ALP-Winners/src/main.py +++ b/ALP-Winners/src/main.py @@ -14,7 +14,6 @@ parser.add_argument('--disable-alp-gui', dest='disable_alp_gui', action='store_const', const=True, default=False, help='If set, no communciation with ALP-GUI overhead will be added.') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') -parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() if args.disable_alp_gui: client.gui.disabled = True @@ -27,8 +26,7 @@ immediate_start=True, static_entities=False, port=args.port, - n_robots=3, - debug=args.debug, + n_robots=3 ) loop.run() \ No newline at end of file diff --git a/MainSystem/controller/__init__.py b/MainSystem/controller/__init__.py index 9d44d31..923b2b7 100644 --- a/MainSystem/controller/__init__.py +++ b/MainSystem/controller/__init__.py @@ -15,7 +15,7 @@ class Controller: """Classe que declara a thread do backend e define o estado do sistema""" - def __init__(self, port, n_robots, debug): + def __init__(self, port, n_robots): self.__thread = Thread(target=self.loop) """Thread que executa o backend do sistema""" @@ -31,15 +31,13 @@ def __init__(self, port, n_robots, debug): self.world = World(n_robots=n_robots) """Essa é uma instância do mundo. O mundo contém informações sobre estado do campo como posição de robôs, velocidades, posição de bola e limites do campo.""" - self.visionSystem = MainVision(self.world, port, debug) + self.visionSystem = MainVision(self.world, port) """Instância do sistema de visão""" self.communicationSystems = Mux([SerialRadio(self.world)]) """Instância do sistema que se comunica com o rádio""" self.__thread.start() - - self.__debug = debug def addEvent(self, method, *args, run_when_done_with_glib=None): """Adiciona um evento a fila de eventos agendados para serem executados no início do próximo loop do backend. Se `run_when_done_with_glib` estiver definido como a tupla `(method,args)` o método dessa tupla será executado depois que o evento for executado.""" diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index e16d828..2656ff4 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -8,7 +8,7 @@ class Vision(ABC): """Classe que define as interfaces que qualquer sistema de visão deve ter no sistema.""" - def __init__(self, world, port, debug): + def __init__(self, world, port): super().__init__() self.cameraHandler = CameraHandler() @@ -20,7 +20,7 @@ def __init__(self, world, port, debug): self.usePastPositions = False self.lastCandidateUse = 0 - self.server_pickle = ServerPickle(port, debug) + self.server_pickle = ServerPickle(port) @abstractmethod def process(self, frame): diff --git a/MainSystem/controller/vision/mainVision/__init__.py b/MainSystem/controller/vision/mainVision/__init__.py index 314635a..a01d1b5 100644 --- a/MainSystem/controller/vision/mainVision/__init__.py +++ b/MainSystem/controller/vision/mainVision/__init__.py @@ -10,8 +10,8 @@ class MainVision(Vision): """Classe que implementa a visão principal da UnBall, que utiliza segmentação por única cor e faz a identificação por forma.""" - def __init__(self, world, port, debug): - super().__init__(world=world, port=port, debug=debug) + def __init__(self, world, port): + super().__init__(world=world, port=port) self.__model = MainVisionModel() """Modelo `MainSystem.model.vision.mainVisionModel.MainVisionModel` que mantém as variáveis da visão""" diff --git a/MainSystem/controller/vision/server_pickle.py b/MainSystem/controller/vision/server_pickle.py index 1c906fe..0eee132 100644 --- a/MainSystem/controller/vision/server_pickle.py +++ b/MainSystem/controller/vision/server_pickle.py @@ -3,7 +3,7 @@ import socket class ServerPickle: - def __init__(self, port,debug): + def __init__(self, port): self.host = socket.gethostname() self.port = port @@ -13,14 +13,11 @@ def __init__(self, port,debug): psocket.listen(2) self.conn, self.address = psocket.accept() print("Connection from: " + str(self.address)) - - self.__debug = debug self.t0 = time.time() def send(self, data): - - if(self.debug): print(1000*(time.time()-self.t0)) + print(1000*(time.time()-self.t0)) self.t0 = time.time() message = pickle.dumps(data,-1) diff --git a/MainSystem/main.py b/MainSystem/main.py index 3b5ff6a..a07cf34 100755 --- a/MainSystem/main.py +++ b/MainSystem/main.py @@ -11,12 +11,11 @@ parser = argparse.ArgumentParser(description='UnBall Main System') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') -parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() def main(): """Função principal que instancia os componentes base, abre a interface gráfica e faz o flush dos dados em memória permanente ao terminar""" - controller = Controller(port=args.port, n_robots=3, debug=args.debug) + controller = Controller(port=args.port, n_robots=3) view = View(controller) view.run() diff --git a/MainSystem/view/vision/mainVision/visaoAltoNivel.py b/MainSystem/view/vision/mainVision/visaoAltoNivel.py index b8638af..dd88fa5 100644 --- a/MainSystem/view/vision/mainVision/visaoAltoNivel.py +++ b/MainSystem/view/vision/mainVision/visaoAltoNivel.py @@ -38,6 +38,7 @@ def getFrame(self): img_filtered = cv2.bitwise_and(img_warpped, img_warpped, mask=fgMask) t0 = time.time() message = self.__visionSystem.process(frame) + print((time.time()-t0)*1000) GLib.idle_add(self.updateRobotsInfo, message) Drawing.draw_field(self.__world, img_filtered) diff --git a/README.md b/README.md index ac3bb88..ecd6088 100644 --- a/README.md +++ b/README.md @@ -21,12 +21,9 @@ pip install -r requirements.txt 4. No terminal 2, mover para o diretório ALP-Winners e executar com: ``` -python3 src/main.py --port 5001 +python3 src/main.py --team-color blue --team-side left --immediate-start --port 5001 --n_robots 3 ``` -## Para debugg - -Para debugar, basta incluir como argumento ```--debug``` tanto quando for executar o MainSystem quanto o ALP-Winners. Por enquanto, as unicas informações que o sistema entrega no modo de debug é o tempo do loop. > ### Importante!! > From a4fe5a04ca0e2ea248cb2d5fbf633281a7811263 Mon Sep 17 00:00:00 2001 From: ayssag Date: Wed, 19 Jul 2023 16:08:34 -0500 Subject: [PATCH 12/88] Revert "Merge branch 'main' into dev-controle" This reverts commit 5cfe367993d6dab9815c591d146f0c6fcddad89e, reversing changes made to 5063dab1fce2c71de18daf8f4d7c8665e2493f90. --- ALP-Winners/src/loop.py | 5 +- ALP-Winners/src/main.py | 4 +- ALP-Winners/src/strategy/__init__.py | 6 --- .../src/strategy/entity/controlTest.py | 53 ------------------- ALP-Winners/src/strategy/movements.py | 8 --- ALP-Winners/src/world/__init__.py | 4 +- MainSystem/controller/__init__.py | 6 ++- .../controller/communication/server_pickle.py | 3 +- MainSystem/controller/states/debugHLC.py | 11 ++-- MainSystem/controller/vision/__init__.py | 4 +- .../controller/vision/mainVision/__init__.py | 4 +- MainSystem/controller/world/__init__.py | 4 +- MainSystem/main.py | 3 +- .../view/vision/mainVision/visaoAltoNivel.py | 1 - README.md | 2 +- 15 files changed, 27 insertions(+), 91 deletions(-) delete mode 100644 ALP-Winners/src/strategy/entity/controlTest.py diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 4fb9694..215cf7b 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -24,6 +24,7 @@ def __init__( static_entities=False, port=5001, n_robots=3, + debug=False ): # Instancia interface com o simulador #self.vss = VSS(team_yellow=team_yellow) @@ -44,6 +45,7 @@ def __init__( self.radio = SerialRadio() self.execute = False self.t0 = time.time() + self.debug= debug # Interface gráfica para mostrar campos self.draw_uvf = draw_uvf @@ -58,7 +60,8 @@ def __init__( def loop(self): if self.world.updateCount == self.lastupdatecount: return - print((time.time()-self.t0)*1000) + if(self.debug): print((time.time()-self.t0)*1000) + self.t0 = time.time() self.lastupdatecount = self.world.updateCount diff --git a/ALP-Winners/src/main.py b/ALP-Winners/src/main.py index 23e2e79..36d39d2 100644 --- a/ALP-Winners/src/main.py +++ b/ALP-Winners/src/main.py @@ -14,6 +14,7 @@ parser.add_argument('--disable-alp-gui', dest='disable_alp_gui', action='store_const', const=True, default=False, help='If set, no communciation with ALP-GUI overhead will be added.') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') +parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() if args.disable_alp_gui: client.gui.disabled = True @@ -26,7 +27,8 @@ immediate_start=True, static_entities=False, port=args.port, - n_robots=3 + n_robots=3, + debug=args.debug, ) loop.run() \ No newline at end of file diff --git a/ALP-Winners/src/strategy/__init__.py b/ALP-Winners/src/strategy/__init__.py index c28bce8..c1fdac5 100644 --- a/ALP-Winners/src/strategy/__init__.py +++ b/ALP-Winners/src/strategy/__init__.py @@ -3,7 +3,6 @@ from .entity.goalKeeper import GoalKeeper from .entity.defender import Defender from .entity.midfielder import Midfielder -from .entity.controlTest import ControlTester #from client.protobuf.vssref_common_pb2 import Foul from client.referee import RefereeCommands from tools import sats, norml, unit, angl, angError, projectLine, howFrontBall, norm, bestWithHyst @@ -141,11 +140,6 @@ def update(self): self.world.team[1].updateEntity(Attacker) self.world.team[0].updateEntity(GoalKeeper) - elif self.world.enableManualControl: - self.world.team[2].updateEntity(Attacker) - self.world.team[1].updateEntity(Attacker) - self.world.team[0].updateEntity(ControlTester) - else: formation = self.formationDecider() toDecide = self.availableRobotIndexes() diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py deleted file mode 100644 index 773cfab..0000000 --- a/ALP-Winners/src/strategy/entity/controlTest.py +++ /dev/null @@ -1,53 +0,0 @@ -from ..entity import Entity -from strategy.field.UVF import UVF -from strategy.field.DirectionalField import DirectionalField -from strategy.field.goalKeeper import GoalKeeperField -from strategy.field.attractive import AttractiveField -from strategy.movements import goalkeep, spinGoalKeeper -from tools import angError, howFrontBall, howPerpBall, ang, norml, norm, angl -from tools.interval import Interval -from control.goalKeeper import GoalKeeperControl -from control.UFC import UFC_Simple -import numpy as np -import math -import time - -class ControlTester(Entity): - def __init__(self, world, robot, side=1): - super().__init__(world, robot) - - self._control = UFC_Simple(self.world) - self.lastChat = 0 - - @property - def control(self): - return self._control - - def equalsTo(self, otherTester): - return True - - def onExit(self): - pass - - def directionDecider(self): - if self.robot.field is not None: - ref_th = self.robot.field.F(self.robot.pose) - rob_th = self.robot.th - - if abs(angError(ref_th, rob_th)) > 90 * np.pi / 180: - self.robot.direction *= -1 - self.lastChat = time.time() - - # Inverter a direção se o robô ficar preso em algo - elif not self.robot.isAlive() and self.robot.spin == 0: - self.lastChat = time.time() - self.robot.direction *= -1 - - def fieldDecider(self): - rr = np.array(self.robot.pos) - - if rr[0] > 0.4: - self.robot.field = DirectionalField(np.pi) - if rr[0] < -0.4: - self.robot.field = DirectionalField(0) - diff --git a/ALP-Winners/src/strategy/movements.py b/ALP-Winners/src/strategy/movements.py index b5b4225..40aa534 100644 --- a/ALP-Winners/src/strategy/movements.py +++ b/ALP-Winners/src/strategy/movements.py @@ -194,11 +194,3 @@ def intercept(rr, rb, direction, rg, vb, vrref=0.5, arref=1.4): # else: # return False - -def doSquareTest(point1, point2, point3, point4): - #ir para ponto 1 - #ir para ponto 2 - #ir para ponto 3 - #ir para ponto 4 - - return \ No newline at end of file diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index 8093993..e8b3354 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -58,9 +58,7 @@ def __init__(self, n_robots=5, side=1, vss=None, team_yellow=False, immediate_st self.allyGoals = 0 self.enemyGoals = 0 self.updateCount = 0 - self.enableManualControl = False - self.manualControlSpeedV = 0 - self.manualControlSpeedW = 0 + self.checkBatteries = False def update(self, message): if self.team_yellow: diff --git a/MainSystem/controller/__init__.py b/MainSystem/controller/__init__.py index 923b2b7..9d44d31 100644 --- a/MainSystem/controller/__init__.py +++ b/MainSystem/controller/__init__.py @@ -15,7 +15,7 @@ class Controller: """Classe que declara a thread do backend e define o estado do sistema""" - def __init__(self, port, n_robots): + def __init__(self, port, n_robots, debug): self.__thread = Thread(target=self.loop) """Thread que executa o backend do sistema""" @@ -31,13 +31,15 @@ def __init__(self, port, n_robots): self.world = World(n_robots=n_robots) """Essa é uma instância do mundo. O mundo contém informações sobre estado do campo como posição de robôs, velocidades, posição de bola e limites do campo.""" - self.visionSystem = MainVision(self.world, port) + self.visionSystem = MainVision(self.world, port, debug) """Instância do sistema de visão""" self.communicationSystems = Mux([SerialRadio(self.world)]) """Instância do sistema que se comunica com o rádio""" self.__thread.start() + + self.__debug = debug def addEvent(self, method, *args, run_when_done_with_glib=None): """Adiciona um evento a fila de eventos agendados para serem executados no início do próximo loop do backend. Se `run_when_done_with_glib` estiver definido como a tupla `(method,args)` o método dessa tupla será executado depois que o evento for executado.""" diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/communication/server_pickle.py index 427e212..0b9d090 100644 --- a/MainSystem/controller/communication/server_pickle.py +++ b/MainSystem/controller/communication/server_pickle.py @@ -19,7 +19,8 @@ def __init__(self, port, debug): self.t0 = time.time() def send(self, data): - print(1000*(time.time()-self.t0)) + + if(self.debug): print(1000*(time.time()-self.t0)) self.t0 = time.time() message = pickle.dumps(data,-1) diff --git a/MainSystem/controller/states/debugHLC.py b/MainSystem/controller/states/debugHLC.py index 516db1b..53228de 100644 --- a/MainSystem/controller/states/debugHLC.py +++ b/MainSystem/controller/states/debugHLC.py @@ -22,8 +22,6 @@ def __init__(self, controller): "manualControlSpeedV": 0, "manualControlSpeedW": 0, "enableManualControl": False, - "manualControlSpeedV" : 0, - "manualControlSpeedW" : 0, "selectedField": "UVF", "selectableFinalPoint": False, "runVision": True, @@ -164,6 +162,11 @@ def update(self): dt = time.time()-self.t self.t = time.time() + #Atualiza o mundo com a nova funcionalidade de enableManualControl, checar baterias + if self.getParam("enableManualControl"): + self.world.checkBatteries = True + else: + self.world.checkBatteries = False # Atualiza o mundo com a visão if self.getParam("runVision"): @@ -180,15 +183,11 @@ def update(self): # Controle manual if self.getParam("enableManualControl"): - self.world.enableManualControl = True - self.world.manualControlSpeedV = self.getParam("manualControlSpeedV") - self.world.manualControlSpeedW = self.getParam("manualControlSpeedW") manualSpeed = SpeedPair(self.getParam("manualControlSpeedV"), self.getParam("manualControlSpeedW")) speeds = [manualSpeed, manualSpeed, manualSpeed] # Controle de alto nível else: - self.world.enableManualControl = False speeds = [r.actuate() for r in self.robots] # Obtém o target instantâneo diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 1238d4d..4545a40 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -8,7 +8,7 @@ class Vision(ABC): """Classe que define as interfaces que qualquer sistema de visão deve ter no sistema.""" - def __init__(self, world, port): + def __init__(self, world, port, debug): super().__init__() self.cameraHandler = CameraHandler() @@ -20,7 +20,7 @@ def __init__(self, world, port): self.usePastPositions = False self.lastCandidateUse = 0 - self.server_pickle = ServerPickle(port) + self.server_pickle = ServerPickle(port, debug) @abstractmethod def process(self, frame): diff --git a/MainSystem/controller/vision/mainVision/__init__.py b/MainSystem/controller/vision/mainVision/__init__.py index a01d1b5..314635a 100644 --- a/MainSystem/controller/vision/mainVision/__init__.py +++ b/MainSystem/controller/vision/mainVision/__init__.py @@ -10,8 +10,8 @@ class MainVision(Vision): """Classe que implementa a visão principal da UnBall, que utiliza segmentação por única cor e faz a identificação por forma.""" - def __init__(self, world, port): - super().__init__(world=world, port=port) + def __init__(self, world, port, debug): + super().__init__(world=world, port=port, debug=debug) self.__model = MainVisionModel() """Modelo `MainSystem.model.vision.mainVisionModel.MainVisionModel` que mantém as variáveis da visão""" diff --git a/MainSystem/controller/world/__init__.py b/MainSystem/controller/world/__init__.py index c14f3bd..ef7f810 100644 --- a/MainSystem/controller/world/__init__.py +++ b/MainSystem/controller/world/__init__.py @@ -44,9 +44,7 @@ def __init__(self, n_robots): self.n_robots = n_robots self.fieldSide = Field.RIGHT self.running = False - self.enableManualControl = False - self.manualControlSpeedV = 0 - self.manualControlSpeedW = 0 + self.checkBatteries = False self.mus = [0.07, 0.07, 0.12, 0.07, 0.07] self.robots = [Robot(self, i, self.mus[i]) for i in range(self.n_robots)] self.enemyRobots = [] diff --git a/MainSystem/main.py b/MainSystem/main.py index a07cf34..3b5ff6a 100755 --- a/MainSystem/main.py +++ b/MainSystem/main.py @@ -11,11 +11,12 @@ parser = argparse.ArgumentParser(description='UnBall Main System') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') +parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() def main(): """Função principal que instancia os componentes base, abre a interface gráfica e faz o flush dos dados em memória permanente ao terminar""" - controller = Controller(port=args.port, n_robots=3) + controller = Controller(port=args.port, n_robots=3, debug=args.debug) view = View(controller) view.run() diff --git a/MainSystem/view/vision/mainVision/visaoAltoNivel.py b/MainSystem/view/vision/mainVision/visaoAltoNivel.py index dd88fa5..b8638af 100644 --- a/MainSystem/view/vision/mainVision/visaoAltoNivel.py +++ b/MainSystem/view/vision/mainVision/visaoAltoNivel.py @@ -38,7 +38,6 @@ def getFrame(self): img_filtered = cv2.bitwise_and(img_warpped, img_warpped, mask=fgMask) t0 = time.time() message = self.__visionSystem.process(frame) - print((time.time()-t0)*1000) GLib.idle_add(self.updateRobotsInfo, message) Drawing.draw_field(self.__world, img_filtered) diff --git a/README.md b/README.md index 5a16440..6741e4d 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ pip install -r requirements.txt 4. No terminal 2, mover para o diretório ALP-Winners e executar com: ``` -python3 src/main.py --team-color blue --team-side left --immediate-start --port 5001 --n_robots 3 +python3 src/main.py --port 5001 ``` ## Para debug From 883a9d5b37795f16564fa351405c372f7a8df638 Mon Sep 17 00:00:00 2001 From: ayssag Date: Wed, 19 Jul 2023 16:33:10 -0500 Subject: [PATCH 13/88] Envio das constantes do controle sentido MS -> ALP --- ALP-Winners/src/loop.py | 1 + MainSystem/controller/vision/__init__.py | 13 ++++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 215cf7b..738b068 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -101,6 +101,7 @@ def loop(self): def busyLoop(self): message = self.pclient.receive() self.execute = message["running"] + print(message) if message is not None: self.world.update(message) # command = self.rc.receive() diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 4545a40..60f1317 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -65,7 +65,18 @@ def update(self): for i in range(self._world.n_robots) }, "running": self._world.running, - "check_batteries": self._world.checkBatteries + "check_batteries": self._world.checkBatteries, + "control_params":{ + "kw": self._world.robots[0].controlSystem.getParam("kw"), + "kp": self._world.robots[0].controlSystem.getParam("kp"), + "L": self._world.robots[0].controlSystem.getParam("L"), + "amax": .12*self._world.robots[0].controlSystem.g, + "vmax": self._world.robots[0].controlSystem.getParam("vmax"), + "motorangaccelmax": self._world.robots[0].controlSystem.getParam("motorangaccelmax"), + "r": self._world.robots[0].controlSystem.getParam("r"), + "maxangerror": self._world.robots[0].controlSystem.getParam("maxangerror"), + "tau": self._world.robots[0].controlSystem.getParam("tau") + } } self.server_pickle.send(message) From c241f57cde8c3a98d9b84872585ec3cee528083d Mon Sep 17 00:00:00 2001 From: ayssag Date: Wed, 19 Jul 2023 18:09:47 -0500 Subject: [PATCH 14/88] Revert "Envio das constantes do controle sentido MS -> ALP" This reverts commit 883a9d5b37795f16564fa351405c372f7a8df638. --- ALP-Winners/src/loop.py | 1 - MainSystem/controller/vision/__init__.py | 13 +------------ 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 738b068..215cf7b 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -101,7 +101,6 @@ def loop(self): def busyLoop(self): message = self.pclient.receive() self.execute = message["running"] - print(message) if message is not None: self.world.update(message) # command = self.rc.receive() diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 60f1317..4545a40 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -65,18 +65,7 @@ def update(self): for i in range(self._world.n_robots) }, "running": self._world.running, - "check_batteries": self._world.checkBatteries, - "control_params":{ - "kw": self._world.robots[0].controlSystem.getParam("kw"), - "kp": self._world.robots[0].controlSystem.getParam("kp"), - "L": self._world.robots[0].controlSystem.getParam("L"), - "amax": .12*self._world.robots[0].controlSystem.g, - "vmax": self._world.robots[0].controlSystem.getParam("vmax"), - "motorangaccelmax": self._world.robots[0].controlSystem.getParam("motorangaccelmax"), - "r": self._world.robots[0].controlSystem.getParam("r"), - "maxangerror": self._world.robots[0].controlSystem.getParam("maxangerror"), - "tau": self._world.robots[0].controlSystem.getParam("tau") - } + "check_batteries": self._world.checkBatteries } self.server_pickle.send(message) From afb2f5c0bdaf33714bfa84e9716a52b35fcba7a8 Mon Sep 17 00:00:00 2001 From: ayssag Date: Wed, 19 Jul 2023 18:10:33 -0500 Subject: [PATCH 15/88] Revert "Merge branch 'main' into dev-controle" This reverts commit cfed2a569b51f732e71b90331767ec561b65a380, reversing changes made to cb560aabcc10b01cc71649263b33118f96c5c4fc. --- MainSystem/controller/communication/server_pickle.py | 4 +--- README.md | 3 --- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/communication/server_pickle.py index 0b9d090..c5c84b5 100644 --- a/MainSystem/controller/communication/server_pickle.py +++ b/MainSystem/controller/communication/server_pickle.py @@ -3,7 +3,7 @@ import socket class ServerPickle: - def __init__(self, port, debug): + def __init__(self, port): self.host = socket.gethostname() self.port = port @@ -13,8 +13,6 @@ def __init__(self, port, debug): psocket.listen(2) self.conn, self.address = psocket.accept() print("Connection from: " + str(self.address)) - - self.debug = debug self.t0 = time.time() diff --git a/README.md b/README.md index 6741e4d..c02a0d6 100644 --- a/README.md +++ b/README.md @@ -24,9 +24,6 @@ pip install -r requirements.txt python3 src/main.py --port 5001 ``` -## Para debug - -Para debugar, basta incluir como argumento ```--debug``` tanto quando for executar o MainSystem quanto o ALP-Winners. Por enquanto, as unicas informações que o sistema entrega no modo de debug é o tempo do loop. > ### Importante!! > From 99d5c41353828315d1430cb5d633703cca18a867 Mon Sep 17 00:00:00 2001 From: ayssag Date: Wed, 19 Jul 2023 21:27:30 -0500 Subject: [PATCH 16/88] Revert "Merge branch 'main' into dev-controle" This reverts commit 5cfe367993d6dab9815c591d146f0c6fcddad89e, reversing changes made to cfed2a569b51f732e71b90331767ec561b65a380. --- ALP-Winners/src/control/UFC.py | 216 ++---------------- ALP-Winners/src/control/__init__.py | 70 ++---- ALP-Winners/src/control/math.md | 29 +++ ALP-Winners/src/control/motorControl.py | 64 ------ ALP-Winners/src/loop.py | 16 +- ALP-Winners/src/tools/__init__.py | 20 +- ALP-Winners/src/world/__init__.py | 27 +-- MainSystem/controller/vision/__init__.py | 44 ++-- .../server_pickle.py | 0 requirements.txt | 72 ------ 10 files changed, 102 insertions(+), 456 deletions(-) create mode 100644 ALP-Winners/src/control/math.md delete mode 100644 ALP-Winners/src/control/motorControl.py rename MainSystem/controller/{communication => vision}/server_pickle.py (100%) delete mode 100644 requirements.txt diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index e5a8c50..abe99ff 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -5,14 +5,6 @@ import math import time -PLOT_CONTROL = False -if PLOT_CONTROL: - import matplotlib.pyplot as plt - -def close_event(): - plt.close() - - class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=False): @@ -26,6 +18,9 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vmax = vmax self.L = L self.kv = 10 + self.ki = 10 + self.kd = 1 + self.integral = 0 self.vbias = 0.2 self.sd_min = 1e-4 @@ -44,53 +39,31 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vPb = np.array([0,0]) self.eth = 0 - self.plots = {"ref":[], "out": [], "eth":[], "vref": [], "v": [], "wref": [], "w": [], "sd": [], "injection": [], "dth": []} @property def error(self): return self.eth - def abs_path_dth(self, initial_pose, error, field, step = 0.01, n = 10): - pos = np.array(initial_pose[:2]) - thlist = np.array([]) - - count = 0 - - for i in range(n): - th = field.F(pos) - thlist = np.append(thlist, th) - pos = pos + step * unit(th) - count += 1 - - aes = np.sum(np.abs(angError(thlist[1:], thlist[:-1]))) / n - - return aes + 0.05 * abs(error) - - def controlLine(self, x, xmax, xmin): - return sats((xmax - x) / (xmax - xmin), 0, 1) - - def output(self, robot): if robot.field is None: return 0,0 # Ângulo de referência - #th = (time.time()/1) % (2*np.pi) - np.pi#np.pi/2 * np.sign(time.time() % 3 - 1.5)#robot.field.F(robot.pose) th = robot.field.F(robot.pose) + # Erro angular eth = angError(th, robot.th) # Tempo desde a última atuação dt = self.interval.getInterval() - # Derivada da referência - dth = filt(0.5 * (th - self.lastth[-1]) / dt + 0.2 * (th - self.lastth[-2]) / (2*dt) + 0.2 * (th - self.lastth[-3]) / (3*dt) + 0.1 * (th - self.lastth[-4]) / (4*dt), 10) - #dth = filt((th - self.lastth) / dt, 10) + # Calcula a integral e satura (descobrir o quanto saturar, tirei esse 64 rad/s) + self.integral = sat(self.integral + eth * dt, 64) - # Erro de velocidade angular - ew = self.lastwref - robot.w + # Calcula a derivada + # self.dth = eth/dt + # print("dth: ", self.dth) - # Lei de controle da velocidade angular - w = dth + self.kw * np.sqrt(abs(eth)) * np.sign(eth) #* (robot.velmod + 1) - #w = self.kw * eth + 0.3 * ew + # Velocidade angular com controle PID + w = self.kp * eth + self.ki*self.integral# + self.kd*self.dth # Velocidade limite de deslizamento v1 = self.amax / np.abs(w) @@ -101,171 +74,28 @@ def output(self, robot): # Velocidade limite de aproximação v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - # v4 = self.kv / abs(eth) + self.vbias - - # Velocidade linear é menor de todas - sd = self.abs_path_dth(robot.pose, eth, robot.field) - #if robot.id == 0: print(sd) - Pb = np.array(robot.field.Pb[:2]) - - if self.enableInjection: - currentnorm = norm(robot.pos, robot.field.Pb) - #print(self.integrateinjection) - # if self.integrateinjection > 10: - # injection = 0 - # else: - # injection = 0.0015 / (abs(currentnorm - self.lastnorm) + 1e-3) - # self.integrateinjection = max(self.integrateinjection + injection - 0.5, 0) - # if abs(currentnorm - self.lastnorm) < 0.001: - # self.loadedInjection = 0.90 * self.loadedInjection + 10 * 0.10 - # else: - # self.loadedInjection = 0.90 * self.loadedInjection - #injection = 0.0010 / (abs(currentnorm - self.lastnorm) + 1e-3) - #injection = 2 * norml(self.vPb) * (np.dot(self.vPb, unit(robot.field.Pb[2])) < 0) * 0.10 / norm(robot.pos, robot.field.Pb[:2]) - - # Only apply injection near - d = norm(robot.pos, robot.field.Pb[:2]) - r1 = 0.10 - r2 = 0.50 - eta = sats((r2 - d) / (r2 - r1), 0, 1) - - # Injection if ball runs in oposite direction - if np.dot(self.vPb, unit(robot.field.Pb[2])) < 0: - raw_injection = max(-np.dot(self.vPb, unit(robot.field.Pb[2])), 0) - else: - raw_injection = 0.5 * max(np.dot(self.vPb, unit(robot.field.Pb[2])), 0) - - injection = raw_injection * eta - else: - currentnorm = 0 - injection = 0 - - v5 = self.vbias + (self.vmax-self.vbias) * self.controlLine(np.log(sd), np.log(self.sd_max), np.log(self.sd_min)) - v = min(v5, v3) + sat(injection, 1) - #print(vtarget) - #v = max(min(self.vbias + (self.vmax-self.vbias) * np.exp(-self.kapd * sd), v3), self.loadedInjection * vtarget)#max(min(v1, v2, v3, v4), 0) - #ev = self.lastvref - robot.velmod - #v = 1 * norm(robot.pos, robot.field.Pb) + 0.1 * ev + 0.2 - # v = 0.25#0.5*np.sin(2*time.time())+0.5 - # w = 0#2*np.sin(5*time.time()) + v = min(v1, v2, v3) + if self.world.enableManualControl: + v = self.world.manualControlSpeedV + # w = self.world.manualControlSpeedW + + if robot.id == 0: + print(f"ref(th): {(th * 180 / np.pi):.0f}º") + print(f"erro(th): {(eth * 180 / np.pi):.0f}º") + print(f"vref: {v:.2f}", end='') + print(f", wref: {w:.2f}") + print(f"v: {robot.velmod:.2f}", end='') + print(f", w: {robot.w:.2f}") # Atualiza a última referência self.lastth = self.lastth[1:] + [th] - self.lastnorm = currentnorm robot.lastControlLinVel = v # Atualiza variáveis de estado self.eth = eth - self.lastdth = dth self.lastwref = w self.lastvref = v - self.vPb = 0.90 * self.vPb + 0.10 * (Pb - self.lastPb) / dt - self.lastPb = Pb - - if PLOT_CONTROL: - self.plots["eth"].append(eth * 180 / np.pi) - self.plots["ref"].append(th * 180 / np.pi) - self.plots["out"].append(robot.th * 180 / np.pi) - self.plots["vref"].append(abs(v)) - self.plots["wref"].append(w) - self.plots["v"].append(robot.velmod) - self.plots["w"].append(robot.w) - self.plots["sd"].append(sd) - self.plots["injection"].append(injection) - self.plots["dth"].append(dth) - if len(self.plots["eth"]) >= 300 and robot.id == 0: - t = np.linspace(0, 300 * 0.016, 300) - fig = plt.figure() - #timer = fig.canvas.new_timer(interval = 5000) - #timer.add_callback(close_event) - plt.subplot(7,1,1) - plt.plot(t, self.plots["eth"], label='eth') - plt.plot(t, np.zeros_like(t), '--') - plt.legend() - plt.subplot(7,1,2) - plt.plot(t, self.plots["ref"], '--', label='th_ref') - plt.plot(t, self.plots["out"], label='th') - plt.legend() - plt.subplot(7,1,3) - plt.plot(t, self.plots["vref"], '--', label='vref') - plt.plot(t, self.plots["v"], label='v') - plt.legend() - plt.subplot(7,1,4) - plt.plot(t, self.plots["wref"], '--', label='wref') - plt.plot(t, self.plots["w"], label='w') - plt.legend() - plt.subplot(7,1,5) - plt.plot(t, self.plots["sd"], label='sd') - plt.legend() - plt.subplot(7,1,6) - plt.plot(t, self.plots["injection"], label='injection') - plt.legend() - plt.subplot(7,1,7) - plt.plot(t, self.plots["dth"], label='dth') - plt.legend() - #timer.start() - robot.stop() - plt.show() - #timer.stop() - for plot in self.plots.keys(): self.plots[plot] = [] - - #return (0,0) if robot.spin == 0: return (v * robot.direction, w) else: return (0, 60 * robot.spin) -# class UFC(): -# """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" -# def __init__(self): -# self.g = 9.8 -# self.kw = 5.5 -# self.kp = 10 -# self.mu = 0.7 -# self.amax = self.mu * self.g -# self.vmax = 2 -# self.L = 0.075 - -# self.lastth = 0 -# self.interval = Interval(filter=True, initial_dt=0.016) - -# def actuate(self, robot): -# if robot.field is None: return 0,0 -# # Ângulo de referência -# th = robot.field.F(robot.pose) -# # Erro angular -# eth = angError(th, robot.th) -# # Tempo desde a última atuação -# dt = self.interval.getInterval() -# # Derivada da referência -# dth = sat(angError(th, self.lastth) / dt, 15) -# # Computa phi -# phi = robot.field.phi(robot.pose) -# # Computa gamma -# gamma = robot.field.gamma(dth, robot.velmod, phi) -# #print("eth: {:.4f}\tphi: {:.4f}\tth: {:.4f}\trth: {:.4f}\t".format(eth*180/np.pi, phi, th*180/np.pi, robot.th*180/np.pi)) - -# # Computa omega -# omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) + gamma - -# # Velocidade limite de deslizamento -# if phi != 0: v1 = (-np.abs(omega) + np.sqrt(omega**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi)) -# if phi == 0: v1 = self.amax / np.abs(omega) - -# # Velocidade limite das rodas -# v2 = (2*self.vmax - self.L * np.abs(omega)) / (2 + self.L * np.abs(phi)) - -# # Velocidade limite de aproximação -# v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - -# # Velocidade linear é menor de todas -# v = max(min(v1, v2, v3), 0) - -# # Lei de controle da velocidade angular -# w = v * phi + omega - -# # Atualiza a última referência -# self.lastth = th -# robot.lastControlLinVel = v - -# if robot.spin == 0: return speeds2motors(v * robot.direction, w) -# else: return speeds2motors(0, 60 * robot.spin) \ No newline at end of file diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index f3b64b1..7a40785 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -1,6 +1,5 @@ from abc import ABC, abstractmethod from tools import speeds2motors, deadzone, sat -from control.motorControl import MotorControl import numpy as np import time @@ -10,75 +9,32 @@ def __init__(self, world): ABC.__init__(self) self.world = world - self.motor_vr_control = MotorControl(3, 0.2 , 0.05, 32) - self.motor_vl_control = MotorControl(3, 0.2, 0.05, 32) - # self.motor_vr_control = MotorControl(1, 1, 3, 32) - # self.motor_vl_control = MotorControl(1, 1, 3, 32) - - self.last_w = 0 @abstractmethod def output(self, robot): pass - def actuateNoControl(self, robot): - if not robot.on: - return (0, 0) - - v, w = self.output(robot) - # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) - # w = 0 - robot.lastControlLinVel = v - - vr, vl = speeds2motors(v, w) - - vr = 3*int(sat(vr * 127 / 110, 127)) - vl = 3*int(sat(vl * 127 / 110, 127)) - - return vr, vl - def actuate(self, robot): if not robot.on: return (0, 0) v, w = self.output(robot) - # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) - # w = 0 robot.lastControlLinVel = v vr, vl = speeds2motors(v, w) - vision_w = self.last_w + sat(robot.angvel-self.last_w, 0.1) - vision_vr, vision_vl = speeds2motors(robot.v_signed, vision_w) - - ur = self.motor_vr_control.actuate(vr, vr - vision_vr) - ul = self.motor_vl_control.actuate(vl, vl - vision_vl) - - if robot.id == 0: - pass - # print( - # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # print( - # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") - # print( - # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") - # print( - # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") - - # vr, vl = speeds2motors(uv, uw) - - self.last_w = vision_w - return ur, ul - # vr, vl = speeds2motors(v, self.world.field.side * w) - - # # return (vr, vl) - - # vision_vr, vision_vl = speeds2motors(robot.velmod, robot.angvel) - - # ur = self.motor_r_control.actuate(vr - vision_vr) - # ul = self.motor_l_control.actuate(vl - vision_vl) + vr = int(deadzone(sat(vr, 255), 32, -32)) + vl = int(deadzone(sat(vl, 255), 32, -32)) + + # print( + # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") + # print( + # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") + # print( + # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") + # print( + # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") + + return vr, vl - # if robot.id == 0: - # print(f"ur: {ur}, e: {vr-vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # return (int(ur), int(ul)) diff --git a/ALP-Winners/src/control/math.md b/ALP-Winners/src/control/math.md new file mode 100644 index 0000000..5377069 --- /dev/null +++ b/ALP-Winners/src/control/math.md @@ -0,0 +1,29 @@ +ALP-Winners + +Lei de controle da velocidade angular: + +Valor lido: $\theta_a$ (ângulo atual do robô) + +Referência: $\theta_r$ (ângulo calculado pelo UVF) + +Erro: $e_{th}$ ($\theta_r$ -$\theta_a$ ) + +Saída: w (velocidade angular) + +$$ w = \dot{\theta_r} + k_w\cdot \sqrt{e_{th}} +$$ +------ +Main System + +$$ v = \min(v_1,v_2,v_3)\\ +v_1 = \frac{-K_{\omega} \sqrt{|\theta_e|} + \\sqrt{K_{\omega}^2 |\theta_e| + 4 |\phi| A_m}}{2|\phi|}\\\\ + + +v_2 = \frac{2 \cdot V_m - L \cdot K_{\omega} \sqrt{|\theta_e|} }{2+L \cdot |\phi|}\\\\ + +v_3 = K_p ||\vec{r}-\vec{P}(1)||\\\\ + +\phi = \frac{\partial \theta_d}{\partial x} \cos(\theta) + \frac{\partial \theta_d}{\partial y} \sin(\theta)\\\\ + +\omega = v \cdot \phi + K_{\omega} \cdot \text{sign}(\theta_e) \sqrt{|\theta_e|} +$$ \ No newline at end of file diff --git a/ALP-Winners/src/control/motorControl.py b/ALP-Winners/src/control/motorControl.py deleted file mode 100644 index 2fcef9e..0000000 --- a/ALP-Winners/src/control/motorControl.py +++ /dev/null @@ -1,64 +0,0 @@ -import time -from tools import sat, deadzone - -class MotorControl: - def __init__(self, refk, kp, ki, deadzone): - self.kp = kp - self.ki = ki - self.refk = refk - self.deadzone = deadzone - self.old_err = 0 - self.old_out = 0 - - self.int = 0 - self.last_time = -1 - - def actuate(self, ref, err): - now = time.time() - dt = now - self.last_time if self.last_time > 0 else 0.040 - - self.int = sat(self.int + err * dt, 64) - out = self.refk * ref + self.kp * err + self.ki * self.int - - - return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) - - # def actuate(self, ref, err): - # ki = self.ki - # kp = self.kp - # T = 0.04 - # # now = time.time() - # # T = now - self.last_time if self.last_time > 0 else 0.040 - - # c0 = -kp + T/2 * ki - # c1 = kp + T/2 * ki - # A = 1 / T - - # out = A * (c1 * err + c0 * self.old_err) + self.old_out - # self.old_err = err - # self.old_out = out if abs(out) <= 127 else 0 - - # return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) - - # double PImotorA(double err){ - # static double old_err; - # static double old_out; - # double out = ( 1.6 * (err - 0.91 * old_err) + old_out); - # old_err = err; //- (saturation(out)-out); - # // 1.6 * (1-0.91 * z^-1)/(1-z^-1) - # // 1.6 * (z-0.91)/(z-1) Projetado a partir do LGR pro motor (identificação o motor) - # // - # // 1 - Identificar novo motor (com carga/com roda e no chão) - # // -MATLAB identificação de sistemas - # // -sintonização de constantes - # // 2 - Projetar um controlador (contínuo) - # // 3 - Discretizar controlador (mapeamento direto com transformada z) - # // -comando MATLAB: c2d(tf,T,"matched") - # // 4 - Expandir função de transferencia e isolar U(z) - # // 5 - Transformada inversa de z - # // - # // z = e^(sT) - - # old_out = (abs(out) < 255)? out : 0; // anti-windup (evita que o erro de saturação seja considerado como erro) - # return out; - # } diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 215cf7b..41129b3 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -70,23 +70,13 @@ def loop(self): self.strategy.update() # Executa o controle - # control_output = [robot.entity.control.actuateNoControl(robot) for robot in self.world.team if robot.entity is not None] - - control_output = [] - for robot in self.world.team: + control_output = [robot.entity.control.actuate(robot) for robot in self.world.team if robot.entity is not None] + # if robot.entity.__class__.__name__ == "GoalKeeper": # print('x_raw:', robot.x_raw, '. x:', self.world.field.side * robot.x_raw) # print('vx_raw:', robot.vx_raw, '. vx:', self.world.field.side * robot.vx_raw) # print('th_raw:', robot.th_raw, '. th:', self.world.field.side * robot.th_raw) # print('w_raw:', robot.w_raw, '. w:', self.world.field.side * robot.w_raw) - - if robot.entity is not None: - if(robot.entity.__class__.__name__ == "GoalKeeper" or robot.entity.__class__.__name__ == "Defender"): - control_output.append(robot.entity.control.actuateNoControl(robot)) - else: - control_output.append(robot.entity.control.actuate(robot)) - - # print('controle:', control_output) if self.execute: for robot in self.world.raw_team: robot.turnOn() @@ -100,7 +90,7 @@ def loop(self): def busyLoop(self): message = self.pclient.receive() - self.execute = message["running"] + self.execute = message[23] if message is not None: self.world.update(message) # command = self.rc.receive() diff --git a/ALP-Winners/src/tools/__init__.py b/ALP-Winners/src/tools/__init__.py index 78dc3fc..36f0b3c 100644 --- a/ALP-Winners/src/tools/__init__.py +++ b/ALP-Winners/src/tools/__init__.py @@ -1,15 +1,9 @@ import numpy as np # Constantes físicas do robô -wheel_reduction = 5 - -# Supostamente devia ser -#r = 0.0325 -#L = 0.075 - -# O que realmente é r = 0.016 -L = 0.052 +L = 0.075 +wheel_reduction = 1 wheel_w_max = 110 conversion = 127 / wheel_w_max @@ -18,16 +12,12 @@ def deadzone(vin, up, down): return vin+up if (vin > 0) else vin-abs(down) return 0 -def speeds2motors(v: float, w: float) -> (int, int): +def speeds2motors(v: float, w: float): """Recebe velocidade linear e angular e retorna velocidades para as duas rodas""" # Computa a velocidade angular de rotação de cada roda - vr = (v + (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction - vl = (v - (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction - - #if fabs(vr) > max_motor_speed or fabs(vl) > max_motor_speed: - # vr = max_motor_speed * vr / max(vr, vl) - # vl = max_motor_speed * vl / max(vr, vl) + vr = (v + (L/2)*w) / r + vl = (v - (L/2)*w) / r # vr = 3*int(sat(vr * conversion, 127)) # vl = 3*int(sat(vl * conversion, 127)) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index e8b3354..83bf41f 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -69,29 +69,18 @@ def update(self, message): blue = self.team robot_id = 0 - for robot in range(message["n_robots"]): + for robot in range(message[4]): if self.team_yellow: - yellow[robot_id].update( - message["robots"][robot_id]["pos_x"], - message["robots"][robot_id]["pos_y"], - message["robots"][robot_id]["th"], - message["robots"][robot_id]["vel_x"], - message["robots"][robot_id]["vel_y"], - message["robots"][robot_id]["w"] - ) + yellow[robot_id].update(message[5+6*robot], message[6+6*robot], message[7+6*robot], message[8+6*robot], message[9+6*robot], message[10+6*robot]) else: - blue[robot_id].update( - message["robots"][robot_id]["pos_x"], - message["robots"][robot_id]["pos_y"], - message["robots"][robot_id]["th"], - message["robots"][robot_id]["vel_x"], - message["robots"][robot_id]["vel_y"], - message["robots"][robot_id]["w"] - ) + blue[robot_id].update(message[5+6*robot], message[6+6*robot], message[7+6*robot], message[8+6*robot], message[9+6*robot], message[10+6*robot]) robot_id+=1 - self.ball.update(message["ball"]["pos_x"], message["ball"]["pos_y"], message["ball"]["vel_x"], message["ball"]["vel_y"]) - self.checkBatteries = message["check_batteries"] + self.ball.update(message[0], message[1], message[2], message[3]) + self.enableManualControl = message[24] + self.manualControlSpeedV = message[25] + self.manualControlSpeedW = message[26] + self.updateCount += 1 diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 4545a40..e16d828 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -1,7 +1,7 @@ from abc import ABC, abstractmethod import time import numpy as np -from controller.communication.server_pickle import ServerPickle +from controller.vision.server_pickle import ServerPickle from controller.vision.cameras import CameraHandler from controller.vision.visionMessage import VisionMessage @@ -45,28 +45,26 @@ def update(self): data = self.process(frame) self._world.update(data) - message = { - "ball":{ - "pos_x": self._world.ball.pos[0], - "pos_y": self._world.ball.pos[1], - "vel_x": self._world.ball.vel[0], - "vel_y": self._world.ball.vel[1] - }, - "n_robots": self._world.n_robots, - "robots":{ - i: { - "pos_x": self._world.robots[i].inst_x, - "pos_y": self._world.robots[i].inst_y, - "th": self._world.robots[i].raw_th, - "vel_x": self._world.robots[i].inst_vx, - "vel_y": self._world.robots[i].inst_vy, - "w": self._world.robots[i].inst_w - } - for i in range(self._world.n_robots) - }, - "running": self._world.running, - "check_batteries": self._world.checkBatteries - } + message = [] + message.append(self._world.ball.pos[0]) + message.append(self._world.ball.pos[1]) + message.append(self._world.ball.vel[0]) + message.append(self._world.ball.vel[1]) + message.append(self._world.n_robots) + + for i in range(self._world.n_robots): + message.append(self._world.robots[i].inst_x) + message.append(self._world.robots[i].inst_y) + message.append(self._world.robots[i].raw_th) + message.append(self._world.robots[i].inst_vx) + message.append(self._world.robots[i].inst_vy) + message.append(self._world.robots[i].inst_w) + + message.append(self._world.running) + message.append(self._world.enableManualControl) + message.append(self._world.manualControlSpeedV) + message.append(self._world.manualControlSpeedW) + self.server_pickle.send(message) diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/vision/server_pickle.py similarity index 100% rename from MainSystem/controller/communication/server_pickle.py rename to MainSystem/controller/vision/server_pickle.py diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index 56953ea..0000000 --- a/requirements.txt +++ /dev/null @@ -1,72 +0,0 @@ -application-utility==1.3.2 -attrs==22.2.0 -autocommand==2.2.2 -btrfsutil==6.2.2 -certifi==2022.12.7 -cffi==1.15.1 -chardet==5.1.0 -contourpy==1.0.7 -cryptography==40.0.1 -cupshelpers==1.0 -cycler==0.11.0 -docopt==0.6.2 -fastjsonschema==2.16.3 -fonttools==4.39.4 -future==0.18.3 -idna==3.4 -inflect==6.0.4 -iso8601==1.1.0 -jaraco.context==4.3.0 -jaraco.functools==3.6.0 -jaraco.text==3.11.1 -keyutils==0.6 -kiwisolver==1.4.4 -lensfun==0.3.3 -lit==15.0.7.dev0 -manjaro-sdk==0.1 -matplotlib==3.7.1 -more-itertools==9.1.0 -netsnmp-python==1.0a1 -nftables==0.1 -npyscreen==4.10.5 -numpy==1.24.3 -opencv-python==4.7.0.72 -ordered-set==4.1.0 -packaging==23.0 -pacman-mirrors==4.23.2 -Pillow==9.4.0 -platformdirs==3.2.0 -ply==3.11 -protobuf==4.23.2 -pyCAIR==0.1.13 -pycairo==1.23.0 -pycparser==2.21 -pycryptodomex==3.12.0 -pycups==2.0.1 -pycurl==7.45.2 -pydantic==1.10.7 -pygame==2.4.0 -PyGObject==3.44.1 -pyparsing==3.0.9 -PyQt5==5.15.9 -PyQt5-sip==12.12.1 -PySide6==6.5.0 -pysmbc==1.0.23 -python-dateutil==2.8.2 -PyYAML==6.0 -reportlab==3.6.12 -requests==2.28.2 -scipy==1.10.1 -serial==0.0.97 -shiboken6==6.5.0 -shiboken6-generator==6.5.0 -simplejson==3.18.0 -six==1.16.0 -systemd-python==235 -team==1.0 -tomli==2.0.1 -trove-classifiers==2023.4.20 -typing_extensions==4.5.0 -udiskie==2.4.2 -urllib3==1.26.13 -validate-pyproject==0.12.2.post1.dev0+g2940279.d20230328 From a9ed703fb23c6751f86ff116725c545363e6853f Mon Sep 17 00:00:00 2001 From: ayssag Date: Wed, 19 Jul 2023 21:29:43 -0500 Subject: [PATCH 17/88] Revert "Revert "Merge branch 'main' into dev-controle"" This reverts commit 99d5c41353828315d1430cb5d633703cca18a867. --- ALP-Winners/src/control/UFC.py | 216 ++++++++++++++++-- ALP-Winners/src/control/__init__.py | 70 ++++-- ALP-Winners/src/control/math.md | 29 --- ALP-Winners/src/control/motorControl.py | 64 ++++++ ALP-Winners/src/loop.py | 16 +- ALP-Winners/src/tools/__init__.py | 20 +- ALP-Winners/src/world/__init__.py | 27 ++- .../server_pickle.py | 0 MainSystem/controller/vision/__init__.py | 44 ++-- requirements.txt | 72 ++++++ 10 files changed, 456 insertions(+), 102 deletions(-) delete mode 100644 ALP-Winners/src/control/math.md create mode 100644 ALP-Winners/src/control/motorControl.py rename MainSystem/controller/{vision => communication}/server_pickle.py (100%) create mode 100644 requirements.txt diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index abe99ff..e5a8c50 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -5,6 +5,14 @@ import math import time +PLOT_CONTROL = False +if PLOT_CONTROL: + import matplotlib.pyplot as plt + +def close_event(): + plt.close() + + class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=False): @@ -18,9 +26,6 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vmax = vmax self.L = L self.kv = 10 - self.ki = 10 - self.kd = 1 - self.integral = 0 self.vbias = 0.2 self.sd_min = 1e-4 @@ -39,31 +44,53 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vPb = np.array([0,0]) self.eth = 0 + self.plots = {"ref":[], "out": [], "eth":[], "vref": [], "v": [], "wref": [], "w": [], "sd": [], "injection": [], "dth": []} @property def error(self): return self.eth + def abs_path_dth(self, initial_pose, error, field, step = 0.01, n = 10): + pos = np.array(initial_pose[:2]) + thlist = np.array([]) + + count = 0 + + for i in range(n): + th = field.F(pos) + thlist = np.append(thlist, th) + pos = pos + step * unit(th) + count += 1 + + aes = np.sum(np.abs(angError(thlist[1:], thlist[:-1]))) / n + + return aes + 0.05 * abs(error) + + def controlLine(self, x, xmax, xmin): + return sats((xmax - x) / (xmax - xmin), 0, 1) + + def output(self, robot): if robot.field is None: return 0,0 # Ângulo de referência + #th = (time.time()/1) % (2*np.pi) - np.pi#np.pi/2 * np.sign(time.time() % 3 - 1.5)#robot.field.F(robot.pose) th = robot.field.F(robot.pose) - # Erro angular eth = angError(th, robot.th) # Tempo desde a última atuação dt = self.interval.getInterval() - # Calcula a integral e satura (descobrir o quanto saturar, tirei esse 64 rad/s) - self.integral = sat(self.integral + eth * dt, 64) + # Derivada da referência + dth = filt(0.5 * (th - self.lastth[-1]) / dt + 0.2 * (th - self.lastth[-2]) / (2*dt) + 0.2 * (th - self.lastth[-3]) / (3*dt) + 0.1 * (th - self.lastth[-4]) / (4*dt), 10) + #dth = filt((th - self.lastth) / dt, 10) - # Calcula a derivada - # self.dth = eth/dt - # print("dth: ", self.dth) + # Erro de velocidade angular + ew = self.lastwref - robot.w - # Velocidade angular com controle PID - w = self.kp * eth + self.ki*self.integral# + self.kd*self.dth + # Lei de controle da velocidade angular + w = dth + self.kw * np.sqrt(abs(eth)) * np.sign(eth) #* (robot.velmod + 1) + #w = self.kw * eth + 0.3 * ew # Velocidade limite de deslizamento v1 = self.amax / np.abs(w) @@ -74,28 +101,171 @@ def output(self, robot): # Velocidade limite de aproximação v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - v = min(v1, v2, v3) - if self.world.enableManualControl: - v = self.world.manualControlSpeedV - # w = self.world.manualControlSpeedW - - if robot.id == 0: - print(f"ref(th): {(th * 180 / np.pi):.0f}º") - print(f"erro(th): {(eth * 180 / np.pi):.0f}º") - print(f"vref: {v:.2f}", end='') - print(f", wref: {w:.2f}") - print(f"v: {robot.velmod:.2f}", end='') - print(f", w: {robot.w:.2f}") + # v4 = self.kv / abs(eth) + self.vbias + + # Velocidade linear é menor de todas + sd = self.abs_path_dth(robot.pose, eth, robot.field) + #if robot.id == 0: print(sd) + Pb = np.array(robot.field.Pb[:2]) + + if self.enableInjection: + currentnorm = norm(robot.pos, robot.field.Pb) + #print(self.integrateinjection) + # if self.integrateinjection > 10: + # injection = 0 + # else: + # injection = 0.0015 / (abs(currentnorm - self.lastnorm) + 1e-3) + # self.integrateinjection = max(self.integrateinjection + injection - 0.5, 0) + # if abs(currentnorm - self.lastnorm) < 0.001: + # self.loadedInjection = 0.90 * self.loadedInjection + 10 * 0.10 + # else: + # self.loadedInjection = 0.90 * self.loadedInjection + #injection = 0.0010 / (abs(currentnorm - self.lastnorm) + 1e-3) + #injection = 2 * norml(self.vPb) * (np.dot(self.vPb, unit(robot.field.Pb[2])) < 0) * 0.10 / norm(robot.pos, robot.field.Pb[:2]) + + # Only apply injection near + d = norm(robot.pos, robot.field.Pb[:2]) + r1 = 0.10 + r2 = 0.50 + eta = sats((r2 - d) / (r2 - r1), 0, 1) + + # Injection if ball runs in oposite direction + if np.dot(self.vPb, unit(robot.field.Pb[2])) < 0: + raw_injection = max(-np.dot(self.vPb, unit(robot.field.Pb[2])), 0) + else: + raw_injection = 0.5 * max(np.dot(self.vPb, unit(robot.field.Pb[2])), 0) + + injection = raw_injection * eta + else: + currentnorm = 0 + injection = 0 + + v5 = self.vbias + (self.vmax-self.vbias) * self.controlLine(np.log(sd), np.log(self.sd_max), np.log(self.sd_min)) + v = min(v5, v3) + sat(injection, 1) + #print(vtarget) + #v = max(min(self.vbias + (self.vmax-self.vbias) * np.exp(-self.kapd * sd), v3), self.loadedInjection * vtarget)#max(min(v1, v2, v3, v4), 0) + #ev = self.lastvref - robot.velmod + #v = 1 * norm(robot.pos, robot.field.Pb) + 0.1 * ev + 0.2 + # v = 0.25#0.5*np.sin(2*time.time())+0.5 + # w = 0#2*np.sin(5*time.time()) # Atualiza a última referência self.lastth = self.lastth[1:] + [th] + self.lastnorm = currentnorm robot.lastControlLinVel = v # Atualiza variáveis de estado self.eth = eth + self.lastdth = dth self.lastwref = w self.lastvref = v + self.vPb = 0.90 * self.vPb + 0.10 * (Pb - self.lastPb) / dt + self.lastPb = Pb + + if PLOT_CONTROL: + self.plots["eth"].append(eth * 180 / np.pi) + self.plots["ref"].append(th * 180 / np.pi) + self.plots["out"].append(robot.th * 180 / np.pi) + self.plots["vref"].append(abs(v)) + self.plots["wref"].append(w) + self.plots["v"].append(robot.velmod) + self.plots["w"].append(robot.w) + self.plots["sd"].append(sd) + self.plots["injection"].append(injection) + self.plots["dth"].append(dth) + if len(self.plots["eth"]) >= 300 and robot.id == 0: + t = np.linspace(0, 300 * 0.016, 300) + fig = plt.figure() + #timer = fig.canvas.new_timer(interval = 5000) + #timer.add_callback(close_event) + plt.subplot(7,1,1) + plt.plot(t, self.plots["eth"], label='eth') + plt.plot(t, np.zeros_like(t), '--') + plt.legend() + plt.subplot(7,1,2) + plt.plot(t, self.plots["ref"], '--', label='th_ref') + plt.plot(t, self.plots["out"], label='th') + plt.legend() + plt.subplot(7,1,3) + plt.plot(t, self.plots["vref"], '--', label='vref') + plt.plot(t, self.plots["v"], label='v') + plt.legend() + plt.subplot(7,1,4) + plt.plot(t, self.plots["wref"], '--', label='wref') + plt.plot(t, self.plots["w"], label='w') + plt.legend() + plt.subplot(7,1,5) + plt.plot(t, self.plots["sd"], label='sd') + plt.legend() + plt.subplot(7,1,6) + plt.plot(t, self.plots["injection"], label='injection') + plt.legend() + plt.subplot(7,1,7) + plt.plot(t, self.plots["dth"], label='dth') + plt.legend() + #timer.start() + robot.stop() + plt.show() + #timer.stop() + for plot in self.plots.keys(): self.plots[plot] = [] + + #return (0,0) if robot.spin == 0: return (v * robot.direction, w) else: return (0, 60 * robot.spin) +# class UFC(): +# """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" +# def __init__(self): +# self.g = 9.8 +# self.kw = 5.5 +# self.kp = 10 +# self.mu = 0.7 +# self.amax = self.mu * self.g +# self.vmax = 2 +# self.L = 0.075 + +# self.lastth = 0 +# self.interval = Interval(filter=True, initial_dt=0.016) + +# def actuate(self, robot): +# if robot.field is None: return 0,0 +# # Ângulo de referência +# th = robot.field.F(robot.pose) +# # Erro angular +# eth = angError(th, robot.th) +# # Tempo desde a última atuação +# dt = self.interval.getInterval() +# # Derivada da referência +# dth = sat(angError(th, self.lastth) / dt, 15) +# # Computa phi +# phi = robot.field.phi(robot.pose) +# # Computa gamma +# gamma = robot.field.gamma(dth, robot.velmod, phi) +# #print("eth: {:.4f}\tphi: {:.4f}\tth: {:.4f}\trth: {:.4f}\t".format(eth*180/np.pi, phi, th*180/np.pi, robot.th*180/np.pi)) + +# # Computa omega +# omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) + gamma + +# # Velocidade limite de deslizamento +# if phi != 0: v1 = (-np.abs(omega) + np.sqrt(omega**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi)) +# if phi == 0: v1 = self.amax / np.abs(omega) + +# # Velocidade limite das rodas +# v2 = (2*self.vmax - self.L * np.abs(omega)) / (2 + self.L * np.abs(phi)) + +# # Velocidade limite de aproximação +# v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref + +# # Velocidade linear é menor de todas +# v = max(min(v1, v2, v3), 0) + +# # Lei de controle da velocidade angular +# w = v * phi + omega + +# # Atualiza a última referência +# self.lastth = th +# robot.lastControlLinVel = v + +# if robot.spin == 0: return speeds2motors(v * robot.direction, w) +# else: return speeds2motors(0, 60 * robot.spin) \ No newline at end of file diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index 7a40785..f3b64b1 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -1,5 +1,6 @@ from abc import ABC, abstractmethod from tools import speeds2motors, deadzone, sat +from control.motorControl import MotorControl import numpy as np import time @@ -9,32 +10,75 @@ def __init__(self, world): ABC.__init__(self) self.world = world + self.motor_vr_control = MotorControl(3, 0.2 , 0.05, 32) + self.motor_vl_control = MotorControl(3, 0.2, 0.05, 32) + # self.motor_vr_control = MotorControl(1, 1, 3, 32) + # self.motor_vl_control = MotorControl(1, 1, 3, 32) + + self.last_w = 0 @abstractmethod def output(self, robot): pass - def actuate(self, robot): + def actuateNoControl(self, robot): if not robot.on: return (0, 0) v, w = self.output(robot) + # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) + # w = 0 robot.lastControlLinVel = v vr, vl = speeds2motors(v, w) - - vr = int(deadzone(sat(vr, 255), 32, -32)) - vl = int(deadzone(sat(vl, 255), 32, -32)) - - # print( - # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # print( - # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") - # print( - # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") - # print( - # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") + + vr = 3*int(sat(vr * 127 / 110, 127)) + vl = 3*int(sat(vl * 127 / 110, 127)) return vr, vl + def actuate(self, robot): + if not robot.on: + return (0, 0) + + v, w = self.output(robot) + # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) + # w = 0 + robot.lastControlLinVel = v + + vr, vl = speeds2motors(v, w) + vision_w = self.last_w + sat(robot.angvel-self.last_w, 0.1) + vision_vr, vision_vl = speeds2motors(robot.v_signed, vision_w) + + ur = self.motor_vr_control.actuate(vr, vr - vision_vr) + ul = self.motor_vl_control.actuate(vl, vl - vision_vl) + + if robot.id == 0: + pass + # print( + # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") + # print( + # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") + # print( + # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") + # print( + # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") + + # vr, vl = speeds2motors(uv, uw) + + self.last_w = vision_w + return ur, ul + + # vr, vl = speeds2motors(v, self.world.field.side * w) + + # # return (vr, vl) + + # vision_vr, vision_vl = speeds2motors(robot.velmod, robot.angvel) + + # ur = self.motor_r_control.actuate(vr - vision_vr) + # ul = self.motor_l_control.actuate(vl - vision_vl) + + # if robot.id == 0: + # print(f"ur: {ur}, e: {vr-vision_vr}, vr: {vr}, vision_vr: {vision_vr}") + # return (int(ur), int(ul)) diff --git a/ALP-Winners/src/control/math.md b/ALP-Winners/src/control/math.md deleted file mode 100644 index 5377069..0000000 --- a/ALP-Winners/src/control/math.md +++ /dev/null @@ -1,29 +0,0 @@ -ALP-Winners - -Lei de controle da velocidade angular: - -Valor lido: $\theta_a$ (ângulo atual do robô) - -Referência: $\theta_r$ (ângulo calculado pelo UVF) - -Erro: $e_{th}$ ($\theta_r$ -$\theta_a$ ) - -Saída: w (velocidade angular) - -$$ w = \dot{\theta_r} + k_w\cdot \sqrt{e_{th}} -$$ ------- -Main System - -$$ v = \min(v_1,v_2,v_3)\\ -v_1 = \frac{-K_{\omega} \sqrt{|\theta_e|} + \\sqrt{K_{\omega}^2 |\theta_e| + 4 |\phi| A_m}}{2|\phi|}\\\\ - - -v_2 = \frac{2 \cdot V_m - L \cdot K_{\omega} \sqrt{|\theta_e|} }{2+L \cdot |\phi|}\\\\ - -v_3 = K_p ||\vec{r}-\vec{P}(1)||\\\\ - -\phi = \frac{\partial \theta_d}{\partial x} \cos(\theta) + \frac{\partial \theta_d}{\partial y} \sin(\theta)\\\\ - -\omega = v \cdot \phi + K_{\omega} \cdot \text{sign}(\theta_e) \sqrt{|\theta_e|} -$$ \ No newline at end of file diff --git a/ALP-Winners/src/control/motorControl.py b/ALP-Winners/src/control/motorControl.py new file mode 100644 index 0000000..2fcef9e --- /dev/null +++ b/ALP-Winners/src/control/motorControl.py @@ -0,0 +1,64 @@ +import time +from tools import sat, deadzone + +class MotorControl: + def __init__(self, refk, kp, ki, deadzone): + self.kp = kp + self.ki = ki + self.refk = refk + self.deadzone = deadzone + self.old_err = 0 + self.old_out = 0 + + self.int = 0 + self.last_time = -1 + + def actuate(self, ref, err): + now = time.time() + dt = now - self.last_time if self.last_time > 0 else 0.040 + + self.int = sat(self.int + err * dt, 64) + out = self.refk * ref + self.kp * err + self.ki * self.int + + + return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) + + # def actuate(self, ref, err): + # ki = self.ki + # kp = self.kp + # T = 0.04 + # # now = time.time() + # # T = now - self.last_time if self.last_time > 0 else 0.040 + + # c0 = -kp + T/2 * ki + # c1 = kp + T/2 * ki + # A = 1 / T + + # out = A * (c1 * err + c0 * self.old_err) + self.old_out + # self.old_err = err + # self.old_out = out if abs(out) <= 127 else 0 + + # return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) + + # double PImotorA(double err){ + # static double old_err; + # static double old_out; + # double out = ( 1.6 * (err - 0.91 * old_err) + old_out); + # old_err = err; //- (saturation(out)-out); + # // 1.6 * (1-0.91 * z^-1)/(1-z^-1) + # // 1.6 * (z-0.91)/(z-1) Projetado a partir do LGR pro motor (identificação o motor) + # // + # // 1 - Identificar novo motor (com carga/com roda e no chão) + # // -MATLAB identificação de sistemas + # // -sintonização de constantes + # // 2 - Projetar um controlador (contínuo) + # // 3 - Discretizar controlador (mapeamento direto com transformada z) + # // -comando MATLAB: c2d(tf,T,"matched") + # // 4 - Expandir função de transferencia e isolar U(z) + # // 5 - Transformada inversa de z + # // + # // z = e^(sT) + + # old_out = (abs(out) < 255)? out : 0; // anti-windup (evita que o erro de saturação seja considerado como erro) + # return out; + # } diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 41129b3..215cf7b 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -70,13 +70,23 @@ def loop(self): self.strategy.update() # Executa o controle - control_output = [robot.entity.control.actuate(robot) for robot in self.world.team if robot.entity is not None] - + # control_output = [robot.entity.control.actuateNoControl(robot) for robot in self.world.team if robot.entity is not None] + + control_output = [] + for robot in self.world.team: # if robot.entity.__class__.__name__ == "GoalKeeper": # print('x_raw:', robot.x_raw, '. x:', self.world.field.side * robot.x_raw) # print('vx_raw:', robot.vx_raw, '. vx:', self.world.field.side * robot.vx_raw) # print('th_raw:', robot.th_raw, '. th:', self.world.field.side * robot.th_raw) # print('w_raw:', robot.w_raw, '. w:', self.world.field.side * robot.w_raw) + + if robot.entity is not None: + if(robot.entity.__class__.__name__ == "GoalKeeper" or robot.entity.__class__.__name__ == "Defender"): + control_output.append(robot.entity.control.actuateNoControl(robot)) + else: + control_output.append(robot.entity.control.actuate(robot)) + + # print('controle:', control_output) if self.execute: for robot in self.world.raw_team: robot.turnOn() @@ -90,7 +100,7 @@ def loop(self): def busyLoop(self): message = self.pclient.receive() - self.execute = message[23] + self.execute = message["running"] if message is not None: self.world.update(message) # command = self.rc.receive() diff --git a/ALP-Winners/src/tools/__init__.py b/ALP-Winners/src/tools/__init__.py index 36f0b3c..78dc3fc 100644 --- a/ALP-Winners/src/tools/__init__.py +++ b/ALP-Winners/src/tools/__init__.py @@ -1,9 +1,15 @@ import numpy as np # Constantes físicas do robô +wheel_reduction = 5 + +# Supostamente devia ser +#r = 0.0325 +#L = 0.075 + +# O que realmente é r = 0.016 -L = 0.075 -wheel_reduction = 1 +L = 0.052 wheel_w_max = 110 conversion = 127 / wheel_w_max @@ -12,12 +18,16 @@ def deadzone(vin, up, down): return vin+up if (vin > 0) else vin-abs(down) return 0 -def speeds2motors(v: float, w: float): +def speeds2motors(v: float, w: float) -> (int, int): """Recebe velocidade linear e angular e retorna velocidades para as duas rodas""" # Computa a velocidade angular de rotação de cada roda - vr = (v + (L/2)*w) / r - vl = (v - (L/2)*w) / r + vr = (v + (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction + vl = (v - (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction + + #if fabs(vr) > max_motor_speed or fabs(vl) > max_motor_speed: + # vr = max_motor_speed * vr / max(vr, vl) + # vl = max_motor_speed * vl / max(vr, vl) # vr = 3*int(sat(vr * conversion, 127)) # vl = 3*int(sat(vl * conversion, 127)) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index 83bf41f..e8b3354 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -69,18 +69,29 @@ def update(self, message): blue = self.team robot_id = 0 - for robot in range(message[4]): + for robot in range(message["n_robots"]): if self.team_yellow: - yellow[robot_id].update(message[5+6*robot], message[6+6*robot], message[7+6*robot], message[8+6*robot], message[9+6*robot], message[10+6*robot]) + yellow[robot_id].update( + message["robots"][robot_id]["pos_x"], + message["robots"][robot_id]["pos_y"], + message["robots"][robot_id]["th"], + message["robots"][robot_id]["vel_x"], + message["robots"][robot_id]["vel_y"], + message["robots"][robot_id]["w"] + ) else: - blue[robot_id].update(message[5+6*robot], message[6+6*robot], message[7+6*robot], message[8+6*robot], message[9+6*robot], message[10+6*robot]) + blue[robot_id].update( + message["robots"][robot_id]["pos_x"], + message["robots"][robot_id]["pos_y"], + message["robots"][robot_id]["th"], + message["robots"][robot_id]["vel_x"], + message["robots"][robot_id]["vel_y"], + message["robots"][robot_id]["w"] + ) robot_id+=1 - self.ball.update(message[0], message[1], message[2], message[3]) - self.enableManualControl = message[24] - self.manualControlSpeedV = message[25] - self.manualControlSpeedW = message[26] - + self.ball.update(message["ball"]["pos_x"], message["ball"]["pos_y"], message["ball"]["vel_x"], message["ball"]["vel_y"]) + self.checkBatteries = message["check_batteries"] self.updateCount += 1 diff --git a/MainSystem/controller/vision/server_pickle.py b/MainSystem/controller/communication/server_pickle.py similarity index 100% rename from MainSystem/controller/vision/server_pickle.py rename to MainSystem/controller/communication/server_pickle.py diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index e16d828..4545a40 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -1,7 +1,7 @@ from abc import ABC, abstractmethod import time import numpy as np -from controller.vision.server_pickle import ServerPickle +from controller.communication.server_pickle import ServerPickle from controller.vision.cameras import CameraHandler from controller.vision.visionMessage import VisionMessage @@ -45,26 +45,28 @@ def update(self): data = self.process(frame) self._world.update(data) - message = [] - message.append(self._world.ball.pos[0]) - message.append(self._world.ball.pos[1]) - message.append(self._world.ball.vel[0]) - message.append(self._world.ball.vel[1]) - message.append(self._world.n_robots) - - for i in range(self._world.n_robots): - message.append(self._world.robots[i].inst_x) - message.append(self._world.robots[i].inst_y) - message.append(self._world.robots[i].raw_th) - message.append(self._world.robots[i].inst_vx) - message.append(self._world.robots[i].inst_vy) - message.append(self._world.robots[i].inst_w) - - message.append(self._world.running) - message.append(self._world.enableManualControl) - message.append(self._world.manualControlSpeedV) - message.append(self._world.manualControlSpeedW) - + message = { + "ball":{ + "pos_x": self._world.ball.pos[0], + "pos_y": self._world.ball.pos[1], + "vel_x": self._world.ball.vel[0], + "vel_y": self._world.ball.vel[1] + }, + "n_robots": self._world.n_robots, + "robots":{ + i: { + "pos_x": self._world.robots[i].inst_x, + "pos_y": self._world.robots[i].inst_y, + "th": self._world.robots[i].raw_th, + "vel_x": self._world.robots[i].inst_vx, + "vel_y": self._world.robots[i].inst_vy, + "w": self._world.robots[i].inst_w + } + for i in range(self._world.n_robots) + }, + "running": self._world.running, + "check_batteries": self._world.checkBatteries + } self.server_pickle.send(message) diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..56953ea --- /dev/null +++ b/requirements.txt @@ -0,0 +1,72 @@ +application-utility==1.3.2 +attrs==22.2.0 +autocommand==2.2.2 +btrfsutil==6.2.2 +certifi==2022.12.7 +cffi==1.15.1 +chardet==5.1.0 +contourpy==1.0.7 +cryptography==40.0.1 +cupshelpers==1.0 +cycler==0.11.0 +docopt==0.6.2 +fastjsonschema==2.16.3 +fonttools==4.39.4 +future==0.18.3 +idna==3.4 +inflect==6.0.4 +iso8601==1.1.0 +jaraco.context==4.3.0 +jaraco.functools==3.6.0 +jaraco.text==3.11.1 +keyutils==0.6 +kiwisolver==1.4.4 +lensfun==0.3.3 +lit==15.0.7.dev0 +manjaro-sdk==0.1 +matplotlib==3.7.1 +more-itertools==9.1.0 +netsnmp-python==1.0a1 +nftables==0.1 +npyscreen==4.10.5 +numpy==1.24.3 +opencv-python==4.7.0.72 +ordered-set==4.1.0 +packaging==23.0 +pacman-mirrors==4.23.2 +Pillow==9.4.0 +platformdirs==3.2.0 +ply==3.11 +protobuf==4.23.2 +pyCAIR==0.1.13 +pycairo==1.23.0 +pycparser==2.21 +pycryptodomex==3.12.0 +pycups==2.0.1 +pycurl==7.45.2 +pydantic==1.10.7 +pygame==2.4.0 +PyGObject==3.44.1 +pyparsing==3.0.9 +PyQt5==5.15.9 +PyQt5-sip==12.12.1 +PySide6==6.5.0 +pysmbc==1.0.23 +python-dateutil==2.8.2 +PyYAML==6.0 +reportlab==3.6.12 +requests==2.28.2 +scipy==1.10.1 +serial==0.0.97 +shiboken6==6.5.0 +shiboken6-generator==6.5.0 +simplejson==3.18.0 +six==1.16.0 +systemd-python==235 +team==1.0 +tomli==2.0.1 +trove-classifiers==2023.4.20 +typing_extensions==4.5.0 +udiskie==2.4.2 +urllib3==1.26.13 +validate-pyproject==0.12.2.post1.dev0+g2940279.d20230328 From d56fa94af7b5e68fc2a5e3cba661be277b11da32 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 17:54:53 -0300 Subject: [PATCH 18/88] Suporte para testes de controle --- ALP-Winners/src/loop.py | 6 ++++++ ALP-Winners/src/world/__init__.py | 4 +++- MainSystem/controller/states/debugHLC.py | 11 ++++++----- MainSystem/controller/vision/__init__.py | 4 ++++ MainSystem/controller/world/__init__.py | 4 +++- 5 files changed, 22 insertions(+), 7 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 215cf7b..66ca95a 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -72,6 +72,7 @@ def loop(self): # Executa o controle # control_output = [robot.entity.control.actuateNoControl(robot) for robot in self.world.team if robot.entity is not None] + control_output = [] for robot in self.world.team: # if robot.entity.__class__.__name__ == "GoalKeeper": @@ -80,6 +81,11 @@ def loop(self): # print('th_raw:', robot.th_raw, '. th:', self.world.field.side * robot.th_raw) # print('w_raw:', robot.w_raw, '. w:', self.world.field.side * robot.w_raw) + if self.world.enableManualControl: + print("Controle manual") + print("V:", self.world.manualControlSpeedV) + print("W:", self.world.manualControlSpeedW) + if robot.entity is not None: if(robot.entity.__class__.__name__ == "GoalKeeper" or robot.entity.__class__.__name__ == "Defender"): control_output.append(robot.entity.control.actuateNoControl(robot)) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index e8b3354..8093993 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -58,7 +58,9 @@ def __init__(self, n_robots=5, side=1, vss=None, team_yellow=False, immediate_st self.allyGoals = 0 self.enemyGoals = 0 self.updateCount = 0 - self.checkBatteries = False + self.enableManualControl = False + self.manualControlSpeedV = 0 + self.manualControlSpeedW = 0 def update(self, message): if self.team_yellow: diff --git a/MainSystem/controller/states/debugHLC.py b/MainSystem/controller/states/debugHLC.py index 53228de..516db1b 100644 --- a/MainSystem/controller/states/debugHLC.py +++ b/MainSystem/controller/states/debugHLC.py @@ -22,6 +22,8 @@ def __init__(self, controller): "manualControlSpeedV": 0, "manualControlSpeedW": 0, "enableManualControl": False, + "manualControlSpeedV" : 0, + "manualControlSpeedW" : 0, "selectedField": "UVF", "selectableFinalPoint": False, "runVision": True, @@ -162,11 +164,6 @@ def update(self): dt = time.time()-self.t self.t = time.time() - #Atualiza o mundo com a nova funcionalidade de enableManualControl, checar baterias - if self.getParam("enableManualControl"): - self.world.checkBatteries = True - else: - self.world.checkBatteries = False # Atualiza o mundo com a visão if self.getParam("runVision"): @@ -183,11 +180,15 @@ def update(self): # Controle manual if self.getParam("enableManualControl"): + self.world.enableManualControl = True + self.world.manualControlSpeedV = self.getParam("manualControlSpeedV") + self.world.manualControlSpeedW = self.getParam("manualControlSpeedW") manualSpeed = SpeedPair(self.getParam("manualControlSpeedV"), self.getParam("manualControlSpeedW")) speeds = [manualSpeed, manualSpeed, manualSpeed] # Controle de alto nível else: + self.world.enableManualControl = False speeds = [r.actuate() for r in self.robots] # Obtém o target instantâneo diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 4545a40..99db19b 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -67,6 +67,10 @@ def update(self): "running": self._world.running, "check_batteries": self._world.checkBatteries } + # TODO: acrescentar: + # message.append(self._world.manualControlSpeedV) + # message.append(self._world.manualControlSpeedW) + self.server_pickle.send(message) diff --git a/MainSystem/controller/world/__init__.py b/MainSystem/controller/world/__init__.py index ef7f810..c14f3bd 100644 --- a/MainSystem/controller/world/__init__.py +++ b/MainSystem/controller/world/__init__.py @@ -44,7 +44,9 @@ def __init__(self, n_robots): self.n_robots = n_robots self.fieldSide = Field.RIGHT self.running = False - self.checkBatteries = False + self.enableManualControl = False + self.manualControlSpeedV = 0 + self.manualControlSpeedW = 0 self.mus = [0.07, 0.07, 0.12, 0.07, 0.07] self.robots = [Robot(self, i, self.mus[i]) for i in range(self.n_robots)] self.enemyRobots = [] From db23a8e0969ca8755e4663c6fb40910d7e739d02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 17:56:30 -0300 Subject: [PATCH 19/88] =?UTF-8?q?manda=20actuate=20para=20todos=20os=20rob?= =?UTF-8?q?=C3=B4s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/loop.py | 26 ++------------------------ 1 file changed, 2 insertions(+), 24 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 66ca95a..2f849bc 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -70,30 +70,8 @@ def loop(self): self.strategy.update() # Executa o controle - # control_output = [robot.entity.control.actuateNoControl(robot) for robot in self.world.team if robot.entity is not None] - - - control_output = [] - for robot in self.world.team: - # if robot.entity.__class__.__name__ == "GoalKeeper": - # print('x_raw:', robot.x_raw, '. x:', self.world.field.side * robot.x_raw) - # print('vx_raw:', robot.vx_raw, '. vx:', self.world.field.side * robot.vx_raw) - # print('th_raw:', robot.th_raw, '. th:', self.world.field.side * robot.th_raw) - # print('w_raw:', robot.w_raw, '. w:', self.world.field.side * robot.w_raw) - - if self.world.enableManualControl: - print("Controle manual") - print("V:", self.world.manualControlSpeedV) - print("W:", self.world.manualControlSpeedW) - - if robot.entity is not None: - if(robot.entity.__class__.__name__ == "GoalKeeper" or robot.entity.__class__.__name__ == "Defender"): - control_output.append(robot.entity.control.actuateNoControl(robot)) - else: - control_output.append(robot.entity.control.actuate(robot)) - - # print('controle:', control_output) - + control_output = [robot.entity.control.actuateNoControl(robot) for robot in self.world.team if robot.entity is not None] + if self.execute: for robot in self.world.raw_team: robot.turnOn() self.radio.send(control_output) From 7c2d92e65b46ee31eecb71a381a4c0127ec68e2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 17:57:04 -0300 Subject: [PATCH 20/88] remove controle antigo --- ALP-Winners/src/control/__init__.py | 53 +++++--------------- ALP-Winners/src/control/motorControl.py | 64 ------------------------- 2 files changed, 13 insertions(+), 104 deletions(-) delete mode 100644 ALP-Winners/src/control/motorControl.py diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index f3b64b1..40c0929 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -1,6 +1,5 @@ from abc import ABC, abstractmethod from tools import speeds2motors, deadzone, sat -from control.motorControl import MotorControl import numpy as np import time @@ -10,12 +9,6 @@ def __init__(self, world): ABC.__init__(self) self.world = world - self.motor_vr_control = MotorControl(3, 0.2 , 0.05, 32) - self.motor_vl_control = MotorControl(3, 0.2, 0.05, 32) - # self.motor_vr_control = MotorControl(1, 1, 3, 32) - # self.motor_vl_control = MotorControl(1, 1, 3, 32) - - self.last_w = 0 @abstractmethod def output(self, robot): @@ -42,43 +35,23 @@ def actuate(self, robot): return (0, 0) v, w = self.output(robot) - # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) - # w = 0 robot.lastControlLinVel = v vr, vl = speeds2motors(v, w) - vision_w = self.last_w + sat(robot.angvel-self.last_w, 0.1) - vision_vr, vision_vl = speeds2motors(robot.v_signed, vision_w) - - ur = self.motor_vr_control.actuate(vr, vr - vision_vr) - ul = self.motor_vl_control.actuate(vl, vl - vision_vl) - - if robot.id == 0: - pass - # print( - # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # print( - # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") - # print( - # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") - # print( - # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") - - # vr, vl = speeds2motors(uv, uw) - self.last_w = vision_w - return ur, ul - - # vr, vl = speeds2motors(v, self.world.field.side * w) - - # # return (vr, vl) - - # vision_vr, vision_vl = speeds2motors(robot.velmod, robot.angvel) + vr = int(deadzone(sat(vr, 255), 32, -32)) + vl = int(deadzone(sat(vl, 255), 32, -32)) + + # print( + # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") + # print( + # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") + # print( + # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") + # print( + # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") + + return vr, vl - # ur = self.motor_r_control.actuate(vr - vision_vr) - # ul = self.motor_l_control.actuate(vl - vision_vl) - # if robot.id == 0: - # print(f"ur: {ur}, e: {vr-vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # return (int(ur), int(ul)) diff --git a/ALP-Winners/src/control/motorControl.py b/ALP-Winners/src/control/motorControl.py deleted file mode 100644 index 2fcef9e..0000000 --- a/ALP-Winners/src/control/motorControl.py +++ /dev/null @@ -1,64 +0,0 @@ -import time -from tools import sat, deadzone - -class MotorControl: - def __init__(self, refk, kp, ki, deadzone): - self.kp = kp - self.ki = ki - self.refk = refk - self.deadzone = deadzone - self.old_err = 0 - self.old_out = 0 - - self.int = 0 - self.last_time = -1 - - def actuate(self, ref, err): - now = time.time() - dt = now - self.last_time if self.last_time > 0 else 0.040 - - self.int = sat(self.int + err * dt, 64) - out = self.refk * ref + self.kp * err + self.ki * self.int - - - return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) - - # def actuate(self, ref, err): - # ki = self.ki - # kp = self.kp - # T = 0.04 - # # now = time.time() - # # T = now - self.last_time if self.last_time > 0 else 0.040 - - # c0 = -kp + T/2 * ki - # c1 = kp + T/2 * ki - # A = 1 / T - - # out = A * (c1 * err + c0 * self.old_err) + self.old_out - # self.old_err = err - # self.old_out = out if abs(out) <= 127 else 0 - - # return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) - - # double PImotorA(double err){ - # static double old_err; - # static double old_out; - # double out = ( 1.6 * (err - 0.91 * old_err) + old_out); - # old_err = err; //- (saturation(out)-out); - # // 1.6 * (1-0.91 * z^-1)/(1-z^-1) - # // 1.6 * (z-0.91)/(z-1) Projetado a partir do LGR pro motor (identificação o motor) - # // - # // 1 - Identificar novo motor (com carga/com roda e no chão) - # // -MATLAB identificação de sistemas - # // -sintonização de constantes - # // 2 - Projetar um controlador (contínuo) - # // 3 - Discretizar controlador (mapeamento direto com transformada z) - # // -comando MATLAB: c2d(tf,T,"matched") - # // 4 - Expandir função de transferencia e isolar U(z) - # // 5 - Transformada inversa de z - # // - # // z = e^(sT) - - # old_out = (abs(out) < 255)? out : 0; // anti-windup (evita que o erro de saturação seja considerado como erro) - # return out; - # } From 491bad603c66d1b45292724c7d6d83e8cf4bd84e Mon Sep 17 00:00:00 2001 From: adornelas Date: Thu, 20 Jul 2023 17:58:26 -0300 Subject: [PATCH 21/88] =?UTF-8?q?Revert=20"21=20minimizar=20prints=20duran?= =?UTF-8?q?te=20execu=C3=A7=C3=A3o=20do=20malp"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/loop.py | 5 +---- ALP-Winners/src/main.py | 4 +--- MainSystem/controller/__init__.py | 6 ++---- MainSystem/controller/communication/server_pickle.py | 3 +-- MainSystem/controller/vision/__init__.py | 4 ++-- MainSystem/controller/vision/mainVision/__init__.py | 4 ++-- MainSystem/main.py | 3 +-- MainSystem/view/vision/mainVision/visaoAltoNivel.py | 1 + README.md | 2 +- 9 files changed, 12 insertions(+), 20 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 2f849bc..5009a06 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -24,7 +24,6 @@ def __init__( static_entities=False, port=5001, n_robots=3, - debug=False ): # Instancia interface com o simulador #self.vss = VSS(team_yellow=team_yellow) @@ -45,7 +44,6 @@ def __init__( self.radio = SerialRadio() self.execute = False self.t0 = time.time() - self.debug= debug # Interface gráfica para mostrar campos self.draw_uvf = draw_uvf @@ -60,8 +58,7 @@ def __init__( def loop(self): if self.world.updateCount == self.lastupdatecount: return - if(self.debug): print((time.time()-self.t0)*1000) - + print((time.time()-self.t0)*1000) self.t0 = time.time() self.lastupdatecount = self.world.updateCount diff --git a/ALP-Winners/src/main.py b/ALP-Winners/src/main.py index 36d39d2..23e2e79 100644 --- a/ALP-Winners/src/main.py +++ b/ALP-Winners/src/main.py @@ -14,7 +14,6 @@ parser.add_argument('--disable-alp-gui', dest='disable_alp_gui', action='store_const', const=True, default=False, help='If set, no communciation with ALP-GUI overhead will be added.') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') -parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() if args.disable_alp_gui: client.gui.disabled = True @@ -27,8 +26,7 @@ immediate_start=True, static_entities=False, port=args.port, - n_robots=3, - debug=args.debug, + n_robots=3 ) loop.run() \ No newline at end of file diff --git a/MainSystem/controller/__init__.py b/MainSystem/controller/__init__.py index 9d44d31..923b2b7 100644 --- a/MainSystem/controller/__init__.py +++ b/MainSystem/controller/__init__.py @@ -15,7 +15,7 @@ class Controller: """Classe que declara a thread do backend e define o estado do sistema""" - def __init__(self, port, n_robots, debug): + def __init__(self, port, n_robots): self.__thread = Thread(target=self.loop) """Thread que executa o backend do sistema""" @@ -31,15 +31,13 @@ def __init__(self, port, n_robots, debug): self.world = World(n_robots=n_robots) """Essa é uma instância do mundo. O mundo contém informações sobre estado do campo como posição de robôs, velocidades, posição de bola e limites do campo.""" - self.visionSystem = MainVision(self.world, port, debug) + self.visionSystem = MainVision(self.world, port) """Instância do sistema de visão""" self.communicationSystems = Mux([SerialRadio(self.world)]) """Instância do sistema que se comunica com o rádio""" self.__thread.start() - - self.__debug = debug def addEvent(self, method, *args, run_when_done_with_glib=None): """Adiciona um evento a fila de eventos agendados para serem executados no início do próximo loop do backend. Se `run_when_done_with_glib` estiver definido como a tupla `(method,args)` o método dessa tupla será executado depois que o evento for executado.""" diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/communication/server_pickle.py index c5c84b5..0eee132 100644 --- a/MainSystem/controller/communication/server_pickle.py +++ b/MainSystem/controller/communication/server_pickle.py @@ -17,8 +17,7 @@ def __init__(self, port): self.t0 = time.time() def send(self, data): - - if(self.debug): print(1000*(time.time()-self.t0)) + print(1000*(time.time()-self.t0)) self.t0 = time.time() message = pickle.dumps(data,-1) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 99db19b..edba2ea 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -8,7 +8,7 @@ class Vision(ABC): """Classe que define as interfaces que qualquer sistema de visão deve ter no sistema.""" - def __init__(self, world, port, debug): + def __init__(self, world, port): super().__init__() self.cameraHandler = CameraHandler() @@ -20,7 +20,7 @@ def __init__(self, world, port, debug): self.usePastPositions = False self.lastCandidateUse = 0 - self.server_pickle = ServerPickle(port, debug) + self.server_pickle = ServerPickle(port) @abstractmethod def process(self, frame): diff --git a/MainSystem/controller/vision/mainVision/__init__.py b/MainSystem/controller/vision/mainVision/__init__.py index 314635a..a01d1b5 100644 --- a/MainSystem/controller/vision/mainVision/__init__.py +++ b/MainSystem/controller/vision/mainVision/__init__.py @@ -10,8 +10,8 @@ class MainVision(Vision): """Classe que implementa a visão principal da UnBall, que utiliza segmentação por única cor e faz a identificação por forma.""" - def __init__(self, world, port, debug): - super().__init__(world=world, port=port, debug=debug) + def __init__(self, world, port): + super().__init__(world=world, port=port) self.__model = MainVisionModel() """Modelo `MainSystem.model.vision.mainVisionModel.MainVisionModel` que mantém as variáveis da visão""" diff --git a/MainSystem/main.py b/MainSystem/main.py index 3b5ff6a..a07cf34 100755 --- a/MainSystem/main.py +++ b/MainSystem/main.py @@ -11,12 +11,11 @@ parser = argparse.ArgumentParser(description='UnBall Main System') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') -parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() def main(): """Função principal que instancia os componentes base, abre a interface gráfica e faz o flush dos dados em memória permanente ao terminar""" - controller = Controller(port=args.port, n_robots=3, debug=args.debug) + controller = Controller(port=args.port, n_robots=3) view = View(controller) view.run() diff --git a/MainSystem/view/vision/mainVision/visaoAltoNivel.py b/MainSystem/view/vision/mainVision/visaoAltoNivel.py index b8638af..dd88fa5 100644 --- a/MainSystem/view/vision/mainVision/visaoAltoNivel.py +++ b/MainSystem/view/vision/mainVision/visaoAltoNivel.py @@ -38,6 +38,7 @@ def getFrame(self): img_filtered = cv2.bitwise_and(img_warpped, img_warpped, mask=fgMask) t0 = time.time() message = self.__visionSystem.process(frame) + print((time.time()-t0)*1000) GLib.idle_add(self.updateRobotsInfo, message) Drawing.draw_field(self.__world, img_filtered) diff --git a/README.md b/README.md index c02a0d6..ecd6088 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ pip install -r requirements.txt 4. No terminal 2, mover para o diretório ALP-Winners e executar com: ``` -python3 src/main.py --port 5001 +python3 src/main.py --team-color blue --team-side left --immediate-start --port 5001 --n_robots 3 ``` From 844544720aa35aef61f9155da0eedb5db59555ec Mon Sep 17 00:00:00 2001 From: Maria Claudia Date: Thu, 20 Jul 2023 17:58:47 -0300 Subject: [PATCH 22/88] implementa variavel para auxiliar na atuacao em rampa dos motores --- ALP-Winners/src/loop.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 5009a06..e8e824e 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -45,6 +45,8 @@ def __init__( self.execute = False self.t0 = time.time() + self.increment_control = 0 + # Interface gráfica para mostrar campos self.draw_uvf = draw_uvf if self.draw_uvf: From 35225abb7fa89315ac3cbfdfe0bd85e275d95c8e Mon Sep 17 00:00:00 2001 From: Maria Claudia Date: Thu, 20 Jul 2023 17:58:58 -0300 Subject: [PATCH 23/88] gera lista com entrada e saidas de interesse para identificacao do sistema pro controle --- ALP-Winners/src/loop.py | 4 ++++ ALP-Winners/src/main.py | 9 +++++++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index e8e824e..9f9bb98 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -24,6 +24,8 @@ def __init__( static_entities=False, port=5001, n_robots=3, + file_log="loop.log" + ): # Instancia interface com o simulador #self.vss = VSS(team_yellow=team_yellow) @@ -46,6 +48,8 @@ def __init__( self.t0 = time.time() self.increment_control = 0 + self.file_log = file_log + self.increment_time_control = 0 # Interface gráfica para mostrar campos self.draw_uvf = draw_uvf diff --git a/ALP-Winners/src/main.py b/ALP-Winners/src/main.py index 23e2e79..75e54b6 100644 --- a/ALP-Winners/src/main.py +++ b/ALP-Winners/src/main.py @@ -18,6 +18,8 @@ if args.disable_alp_gui: client.gui.disabled = True +file_log = open("logIdentificacao.txt", "w") +file_log.write("tempo w(entrada) x(saida) y(saida) th(saida)\n") # Instancia o programa principal loop = Loop( draw_uvf=False, @@ -26,7 +28,10 @@ immediate_start=True, static_entities=False, port=args.port, - n_robots=3 + n_robots=3, + file_log=file_log ) -loop.run() \ No newline at end of file +loop.run() + +file_log.close() \ No newline at end of file From 05c11a32d6df5f940c639e09896c2d9c4037a794 Mon Sep 17 00:00:00 2001 From: Maria Claudia Date: Thu, 20 Jul 2023 17:59:31 -0300 Subject: [PATCH 24/88] corrige para coletar omega diretamente da visao --- MainSystem/controller/vision/__init__.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index edba2ea..757a3e3 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -8,7 +8,7 @@ class Vision(ABC): """Classe que define as interfaces que qualquer sistema de visão deve ter no sistema.""" - def __init__(self, world, port): + def __init__(self, world, port, file_log): super().__init__() self.cameraHandler = CameraHandler() @@ -22,6 +22,10 @@ def __init__(self, world, port): self.server_pickle = ServerPickle(port) + self.file_log = file_log + self.increment_time_control = 0 + + @abstractmethod def process(self, frame): """Método abstrato que recebe um frame do tipo numpy array no formato (height, width, depth). Retorna uma mensagem de alteração do tipo `MainSystem.controller.vision.visionMessage.VisionMessage`""" @@ -72,6 +76,14 @@ def update(self): # message.append(self._world.manualControlSpeedW) + # Salvar tempo e inst_w no arquivo (usar robo 0) + + log_line = f"{self._world.robots[0].inst_w}" + self.file_log.write(log_line) + self.file_log.write("\n") + + self.increment_time_control += (time.time()-self.t0)*1000 + self.server_pickle.send(message) if self.usePastPositions is False: From 9c7df757718629577f57c6f9af6bd81de05f8b5a Mon Sep 17 00:00:00 2001 From: Maria Claudia Date: Thu, 20 Jul 2023 18:02:26 -0300 Subject: [PATCH 25/88] =?UTF-8?q?Revert=20"Merge=20branch=20'23-gerar-list?= =?UTF-8?q?a-de-dados-para-identifica=C3=A7=C3=A3o-do-sistema-controle'"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit dd920e01e98e7498f27eb896ed2ec33a2b2185f6, reversing changes made to a6d0796236588f515f1fb325b43ac1fac31d092a. --- ALP-Winners/src/control/__init__.py | 27 +++------- ALP-Winners/src/control/motorControl.py | 64 ++++++++++++++++++++++++ ALP-Winners/src/loop.py | 6 --- ALP-Winners/src/main.py | 9 +--- MainSystem/controller/vision/__init__.py | 14 +----- 5 files changed, 74 insertions(+), 46 deletions(-) create mode 100644 ALP-Winners/src/control/motorControl.py diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index 40c0929..9019a82 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -1,5 +1,6 @@ from abc import ABC, abstractmethod from tools import speeds2motors, deadzone, sat +from control.motorControl import MotorControl import numpy as np import time @@ -9,6 +10,12 @@ def __init__(self, world): ABC.__init__(self) self.world = world + self.motor_vr_control = MotorControl(3, 0.2 , 0.05, 32) + self.motor_vl_control = MotorControl(3, 0.2, 0.05, 32) + # self.motor_vr_control = MotorControl(1, 1, 3, 32) + # self.motor_vl_control = MotorControl(1, 1, 3, 32) + + self.last_w = 0 @abstractmethod def output(self, robot): @@ -34,24 +41,4 @@ def actuate(self, robot): if not robot.on: return (0, 0) - v, w = self.output(robot) - robot.lastControlLinVel = v - - vr, vl = speeds2motors(v, w) - - vr = int(deadzone(sat(vr, 255), 32, -32)) - vl = int(deadzone(sat(vl, 255), 32, -32)) - - # print( - # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # print( - # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") - # print( - # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") - # print( - # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") - - return vr, vl - - diff --git a/ALP-Winners/src/control/motorControl.py b/ALP-Winners/src/control/motorControl.py new file mode 100644 index 0000000..2fcef9e --- /dev/null +++ b/ALP-Winners/src/control/motorControl.py @@ -0,0 +1,64 @@ +import time +from tools import sat, deadzone + +class MotorControl: + def __init__(self, refk, kp, ki, deadzone): + self.kp = kp + self.ki = ki + self.refk = refk + self.deadzone = deadzone + self.old_err = 0 + self.old_out = 0 + + self.int = 0 + self.last_time = -1 + + def actuate(self, ref, err): + now = time.time() + dt = now - self.last_time if self.last_time > 0 else 0.040 + + self.int = sat(self.int + err * dt, 64) + out = self.refk * ref + self.kp * err + self.ki * self.int + + + return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) + + # def actuate(self, ref, err): + # ki = self.ki + # kp = self.kp + # T = 0.04 + # # now = time.time() + # # T = now - self.last_time if self.last_time > 0 else 0.040 + + # c0 = -kp + T/2 * ki + # c1 = kp + T/2 * ki + # A = 1 / T + + # out = A * (c1 * err + c0 * self.old_err) + self.old_out + # self.old_err = err + # self.old_out = out if abs(out) <= 127 else 0 + + # return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) + + # double PImotorA(double err){ + # static double old_err; + # static double old_out; + # double out = ( 1.6 * (err - 0.91 * old_err) + old_out); + # old_err = err; //- (saturation(out)-out); + # // 1.6 * (1-0.91 * z^-1)/(1-z^-1) + # // 1.6 * (z-0.91)/(z-1) Projetado a partir do LGR pro motor (identificação o motor) + # // + # // 1 - Identificar novo motor (com carga/com roda e no chão) + # // -MATLAB identificação de sistemas + # // -sintonização de constantes + # // 2 - Projetar um controlador (contínuo) + # // 3 - Discretizar controlador (mapeamento direto com transformada z) + # // -comando MATLAB: c2d(tf,T,"matched") + # // 4 - Expandir função de transferencia e isolar U(z) + # // 5 - Transformada inversa de z + # // + # // z = e^(sT) + + # old_out = (abs(out) < 255)? out : 0; // anti-windup (evita que o erro de saturação seja considerado como erro) + # return out; + # } diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 9f9bb98..5009a06 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -24,8 +24,6 @@ def __init__( static_entities=False, port=5001, n_robots=3, - file_log="loop.log" - ): # Instancia interface com o simulador #self.vss = VSS(team_yellow=team_yellow) @@ -47,10 +45,6 @@ def __init__( self.execute = False self.t0 = time.time() - self.increment_control = 0 - self.file_log = file_log - self.increment_time_control = 0 - # Interface gráfica para mostrar campos self.draw_uvf = draw_uvf if self.draw_uvf: diff --git a/ALP-Winners/src/main.py b/ALP-Winners/src/main.py index 75e54b6..23e2e79 100644 --- a/ALP-Winners/src/main.py +++ b/ALP-Winners/src/main.py @@ -18,8 +18,6 @@ if args.disable_alp_gui: client.gui.disabled = True -file_log = open("logIdentificacao.txt", "w") -file_log.write("tempo w(entrada) x(saida) y(saida) th(saida)\n") # Instancia o programa principal loop = Loop( draw_uvf=False, @@ -28,10 +26,7 @@ immediate_start=True, static_entities=False, port=args.port, - n_robots=3, - file_log=file_log + n_robots=3 ) -loop.run() - -file_log.close() \ No newline at end of file +loop.run() \ No newline at end of file diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 757a3e3..edba2ea 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -8,7 +8,7 @@ class Vision(ABC): """Classe que define as interfaces que qualquer sistema de visão deve ter no sistema.""" - def __init__(self, world, port, file_log): + def __init__(self, world, port): super().__init__() self.cameraHandler = CameraHandler() @@ -22,10 +22,6 @@ def __init__(self, world, port, file_log): self.server_pickle = ServerPickle(port) - self.file_log = file_log - self.increment_time_control = 0 - - @abstractmethod def process(self, frame): """Método abstrato que recebe um frame do tipo numpy array no formato (height, width, depth). Retorna uma mensagem de alteração do tipo `MainSystem.controller.vision.visionMessage.VisionMessage`""" @@ -76,14 +72,6 @@ def update(self): # message.append(self._world.manualControlSpeedW) - # Salvar tempo e inst_w no arquivo (usar robo 0) - - log_line = f"{self._world.robots[0].inst_w}" - self.file_log.write(log_line) - self.file_log.write("\n") - - self.increment_time_control += (time.time()-self.t0)*1000 - self.server_pickle.send(message) if self.usePastPositions is False: From 2458b88810091daf207ee6f207c7999732944887 Mon Sep 17 00:00:00 2001 From: ayssag Date: Thu, 20 Jul 2023 18:02:26 -0300 Subject: [PATCH 26/88] Envio das constantes do controle sentido MS -> ALP --- ALP-Winners/src/loop.py | 1 + MainSystem/controller/vision/__init__.py | 13 ++++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 5009a06..d915e03 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -82,6 +82,7 @@ def loop(self): def busyLoop(self): message = self.pclient.receive() self.execute = message["running"] + print(message) if message is not None: self.world.update(message) # command = self.rc.receive() diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index edba2ea..17f966a 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -65,7 +65,18 @@ def update(self): for i in range(self._world.n_robots) }, "running": self._world.running, - "check_batteries": self._world.checkBatteries + "check_batteries": self._world.checkBatteries, + "control_params":{ + "kw": self._world.robots[0].controlSystem.getParam("kw"), + "kp": self._world.robots[0].controlSystem.getParam("kp"), + "L": self._world.robots[0].controlSystem.getParam("L"), + "amax": .12*self._world.robots[0].controlSystem.g, + "vmax": self._world.robots[0].controlSystem.getParam("vmax"), + "motorangaccelmax": self._world.robots[0].controlSystem.getParam("motorangaccelmax"), + "r": self._world.robots[0].controlSystem.getParam("r"), + "maxangerror": self._world.robots[0].controlSystem.getParam("maxangerror"), + "tau": self._world.robots[0].controlSystem.getParam("tau") + } } # TODO: acrescentar: # message.append(self._world.manualControlSpeedV) From e2531321c8ad486e09536be2de47b6b73c55f057 Mon Sep 17 00:00:00 2001 From: ayssag Date: Thu, 20 Jul 2023 18:02:26 -0300 Subject: [PATCH 27/88] Revert "Envio das constantes do controle sentido MS -> ALP" This reverts commit 883a9d5b37795f16564fa351405c372f7a8df638. --- ALP-Winners/src/loop.py | 1 - MainSystem/controller/vision/__init__.py | 13 +------------ 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index d915e03..5009a06 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -82,7 +82,6 @@ def loop(self): def busyLoop(self): message = self.pclient.receive() self.execute = message["running"] - print(message) if message is not None: self.world.update(message) # command = self.rc.receive() diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 17f966a..edba2ea 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -65,18 +65,7 @@ def update(self): for i in range(self._world.n_robots) }, "running": self._world.running, - "check_batteries": self._world.checkBatteries, - "control_params":{ - "kw": self._world.robots[0].controlSystem.getParam("kw"), - "kp": self._world.robots[0].controlSystem.getParam("kp"), - "L": self._world.robots[0].controlSystem.getParam("L"), - "amax": .12*self._world.robots[0].controlSystem.g, - "vmax": self._world.robots[0].controlSystem.getParam("vmax"), - "motorangaccelmax": self._world.robots[0].controlSystem.getParam("motorangaccelmax"), - "r": self._world.robots[0].controlSystem.getParam("r"), - "maxangerror": self._world.robots[0].controlSystem.getParam("maxangerror"), - "tau": self._world.robots[0].controlSystem.getParam("tau") - } + "check_batteries": self._world.checkBatteries } # TODO: acrescentar: # message.append(self._world.manualControlSpeedV) From 9e1313bf6195aae3d7ddd5deaf2cc8bb994f6c92 Mon Sep 17 00:00:00 2001 From: ayssag Date: Thu, 20 Jul 2023 18:03:17 -0300 Subject: [PATCH 28/88] Revert "Merge branch 'main' into dev-controle" This reverts commit 5cfe367993d6dab9815c591d146f0c6fcddad89e, reversing changes made to cfed2a569b51f732e71b90331767ec561b65a380. --- ALP-Winners/src/control/UFC.py | 216 ++---------------- ALP-Winners/src/control/__init__.py | 30 ++- ALP-Winners/src/control/math.md | 29 +++ ALP-Winners/src/control/motorControl.py | 64 ------ ALP-Winners/src/loop.py | 2 +- ALP-Winners/src/tools/__init__.py | 20 +- ALP-Winners/src/world/__init__.py | 27 +-- MainSystem/controller/vision/__init__.py | 2 +- .../server_pickle.py | 0 requirements.txt | 72 ------ 10 files changed, 80 insertions(+), 382 deletions(-) create mode 100644 ALP-Winners/src/control/math.md delete mode 100644 ALP-Winners/src/control/motorControl.py rename MainSystem/controller/{communication => vision}/server_pickle.py (100%) delete mode 100644 requirements.txt diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index e5a8c50..abe99ff 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -5,14 +5,6 @@ import math import time -PLOT_CONTROL = False -if PLOT_CONTROL: - import matplotlib.pyplot as plt - -def close_event(): - plt.close() - - class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=False): @@ -26,6 +18,9 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vmax = vmax self.L = L self.kv = 10 + self.ki = 10 + self.kd = 1 + self.integral = 0 self.vbias = 0.2 self.sd_min = 1e-4 @@ -44,53 +39,31 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vPb = np.array([0,0]) self.eth = 0 - self.plots = {"ref":[], "out": [], "eth":[], "vref": [], "v": [], "wref": [], "w": [], "sd": [], "injection": [], "dth": []} @property def error(self): return self.eth - def abs_path_dth(self, initial_pose, error, field, step = 0.01, n = 10): - pos = np.array(initial_pose[:2]) - thlist = np.array([]) - - count = 0 - - for i in range(n): - th = field.F(pos) - thlist = np.append(thlist, th) - pos = pos + step * unit(th) - count += 1 - - aes = np.sum(np.abs(angError(thlist[1:], thlist[:-1]))) / n - - return aes + 0.05 * abs(error) - - def controlLine(self, x, xmax, xmin): - return sats((xmax - x) / (xmax - xmin), 0, 1) - - def output(self, robot): if robot.field is None: return 0,0 # Ângulo de referência - #th = (time.time()/1) % (2*np.pi) - np.pi#np.pi/2 * np.sign(time.time() % 3 - 1.5)#robot.field.F(robot.pose) th = robot.field.F(robot.pose) + # Erro angular eth = angError(th, robot.th) # Tempo desde a última atuação dt = self.interval.getInterval() - # Derivada da referência - dth = filt(0.5 * (th - self.lastth[-1]) / dt + 0.2 * (th - self.lastth[-2]) / (2*dt) + 0.2 * (th - self.lastth[-3]) / (3*dt) + 0.1 * (th - self.lastth[-4]) / (4*dt), 10) - #dth = filt((th - self.lastth) / dt, 10) + # Calcula a integral e satura (descobrir o quanto saturar, tirei esse 64 rad/s) + self.integral = sat(self.integral + eth * dt, 64) - # Erro de velocidade angular - ew = self.lastwref - robot.w + # Calcula a derivada + # self.dth = eth/dt + # print("dth: ", self.dth) - # Lei de controle da velocidade angular - w = dth + self.kw * np.sqrt(abs(eth)) * np.sign(eth) #* (robot.velmod + 1) - #w = self.kw * eth + 0.3 * ew + # Velocidade angular com controle PID + w = self.kp * eth + self.ki*self.integral# + self.kd*self.dth # Velocidade limite de deslizamento v1 = self.amax / np.abs(w) @@ -101,171 +74,28 @@ def output(self, robot): # Velocidade limite de aproximação v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - # v4 = self.kv / abs(eth) + self.vbias - - # Velocidade linear é menor de todas - sd = self.abs_path_dth(robot.pose, eth, robot.field) - #if robot.id == 0: print(sd) - Pb = np.array(robot.field.Pb[:2]) - - if self.enableInjection: - currentnorm = norm(robot.pos, robot.field.Pb) - #print(self.integrateinjection) - # if self.integrateinjection > 10: - # injection = 0 - # else: - # injection = 0.0015 / (abs(currentnorm - self.lastnorm) + 1e-3) - # self.integrateinjection = max(self.integrateinjection + injection - 0.5, 0) - # if abs(currentnorm - self.lastnorm) < 0.001: - # self.loadedInjection = 0.90 * self.loadedInjection + 10 * 0.10 - # else: - # self.loadedInjection = 0.90 * self.loadedInjection - #injection = 0.0010 / (abs(currentnorm - self.lastnorm) + 1e-3) - #injection = 2 * norml(self.vPb) * (np.dot(self.vPb, unit(robot.field.Pb[2])) < 0) * 0.10 / norm(robot.pos, robot.field.Pb[:2]) - - # Only apply injection near - d = norm(robot.pos, robot.field.Pb[:2]) - r1 = 0.10 - r2 = 0.50 - eta = sats((r2 - d) / (r2 - r1), 0, 1) - - # Injection if ball runs in oposite direction - if np.dot(self.vPb, unit(robot.field.Pb[2])) < 0: - raw_injection = max(-np.dot(self.vPb, unit(robot.field.Pb[2])), 0) - else: - raw_injection = 0.5 * max(np.dot(self.vPb, unit(robot.field.Pb[2])), 0) - - injection = raw_injection * eta - else: - currentnorm = 0 - injection = 0 - - v5 = self.vbias + (self.vmax-self.vbias) * self.controlLine(np.log(sd), np.log(self.sd_max), np.log(self.sd_min)) - v = min(v5, v3) + sat(injection, 1) - #print(vtarget) - #v = max(min(self.vbias + (self.vmax-self.vbias) * np.exp(-self.kapd * sd), v3), self.loadedInjection * vtarget)#max(min(v1, v2, v3, v4), 0) - #ev = self.lastvref - robot.velmod - #v = 1 * norm(robot.pos, robot.field.Pb) + 0.1 * ev + 0.2 - # v = 0.25#0.5*np.sin(2*time.time())+0.5 - # w = 0#2*np.sin(5*time.time()) + v = min(v1, v2, v3) + if self.world.enableManualControl: + v = self.world.manualControlSpeedV + # w = self.world.manualControlSpeedW + + if robot.id == 0: + print(f"ref(th): {(th * 180 / np.pi):.0f}º") + print(f"erro(th): {(eth * 180 / np.pi):.0f}º") + print(f"vref: {v:.2f}", end='') + print(f", wref: {w:.2f}") + print(f"v: {robot.velmod:.2f}", end='') + print(f", w: {robot.w:.2f}") # Atualiza a última referência self.lastth = self.lastth[1:] + [th] - self.lastnorm = currentnorm robot.lastControlLinVel = v # Atualiza variáveis de estado self.eth = eth - self.lastdth = dth self.lastwref = w self.lastvref = v - self.vPb = 0.90 * self.vPb + 0.10 * (Pb - self.lastPb) / dt - self.lastPb = Pb - - if PLOT_CONTROL: - self.plots["eth"].append(eth * 180 / np.pi) - self.plots["ref"].append(th * 180 / np.pi) - self.plots["out"].append(robot.th * 180 / np.pi) - self.plots["vref"].append(abs(v)) - self.plots["wref"].append(w) - self.plots["v"].append(robot.velmod) - self.plots["w"].append(robot.w) - self.plots["sd"].append(sd) - self.plots["injection"].append(injection) - self.plots["dth"].append(dth) - if len(self.plots["eth"]) >= 300 and robot.id == 0: - t = np.linspace(0, 300 * 0.016, 300) - fig = plt.figure() - #timer = fig.canvas.new_timer(interval = 5000) - #timer.add_callback(close_event) - plt.subplot(7,1,1) - plt.plot(t, self.plots["eth"], label='eth') - plt.plot(t, np.zeros_like(t), '--') - plt.legend() - plt.subplot(7,1,2) - plt.plot(t, self.plots["ref"], '--', label='th_ref') - plt.plot(t, self.plots["out"], label='th') - plt.legend() - plt.subplot(7,1,3) - plt.plot(t, self.plots["vref"], '--', label='vref') - plt.plot(t, self.plots["v"], label='v') - plt.legend() - plt.subplot(7,1,4) - plt.plot(t, self.plots["wref"], '--', label='wref') - plt.plot(t, self.plots["w"], label='w') - plt.legend() - plt.subplot(7,1,5) - plt.plot(t, self.plots["sd"], label='sd') - plt.legend() - plt.subplot(7,1,6) - plt.plot(t, self.plots["injection"], label='injection') - plt.legend() - plt.subplot(7,1,7) - plt.plot(t, self.plots["dth"], label='dth') - plt.legend() - #timer.start() - robot.stop() - plt.show() - #timer.stop() - for plot in self.plots.keys(): self.plots[plot] = [] - - #return (0,0) if robot.spin == 0: return (v * robot.direction, w) else: return (0, 60 * robot.spin) -# class UFC(): -# """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" -# def __init__(self): -# self.g = 9.8 -# self.kw = 5.5 -# self.kp = 10 -# self.mu = 0.7 -# self.amax = self.mu * self.g -# self.vmax = 2 -# self.L = 0.075 - -# self.lastth = 0 -# self.interval = Interval(filter=True, initial_dt=0.016) - -# def actuate(self, robot): -# if robot.field is None: return 0,0 -# # Ângulo de referência -# th = robot.field.F(robot.pose) -# # Erro angular -# eth = angError(th, robot.th) -# # Tempo desde a última atuação -# dt = self.interval.getInterval() -# # Derivada da referência -# dth = sat(angError(th, self.lastth) / dt, 15) -# # Computa phi -# phi = robot.field.phi(robot.pose) -# # Computa gamma -# gamma = robot.field.gamma(dth, robot.velmod, phi) -# #print("eth: {:.4f}\tphi: {:.4f}\tth: {:.4f}\trth: {:.4f}\t".format(eth*180/np.pi, phi, th*180/np.pi, robot.th*180/np.pi)) - -# # Computa omega -# omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) + gamma - -# # Velocidade limite de deslizamento -# if phi != 0: v1 = (-np.abs(omega) + np.sqrt(omega**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi)) -# if phi == 0: v1 = self.amax / np.abs(omega) - -# # Velocidade limite das rodas -# v2 = (2*self.vmax - self.L * np.abs(omega)) / (2 + self.L * np.abs(phi)) - -# # Velocidade limite de aproximação -# v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - -# # Velocidade linear é menor de todas -# v = max(min(v1, v2, v3), 0) - -# # Lei de controle da velocidade angular -# w = v * phi + omega - -# # Atualiza a última referência -# self.lastth = th -# robot.lastControlLinVel = v - -# if robot.spin == 0: return speeds2motors(v * robot.direction, w) -# else: return speeds2motors(0, 60 * robot.spin) \ No newline at end of file diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index 9019a82..7a40785 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -1,6 +1,5 @@ from abc import ABC, abstractmethod from tools import speeds2motors, deadzone, sat -from control.motorControl import MotorControl import numpy as np import time @@ -10,35 +9,32 @@ def __init__(self, world): ABC.__init__(self) self.world = world - self.motor_vr_control = MotorControl(3, 0.2 , 0.05, 32) - self.motor_vl_control = MotorControl(3, 0.2, 0.05, 32) - # self.motor_vr_control = MotorControl(1, 1, 3, 32) - # self.motor_vl_control = MotorControl(1, 1, 3, 32) - - self.last_w = 0 @abstractmethod def output(self, robot): pass - def actuateNoControl(self, robot): + def actuate(self, robot): if not robot.on: return (0, 0) v, w = self.output(robot) - # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) - # w = 0 robot.lastControlLinVel = v vr, vl = speeds2motors(v, w) - - vr = 3*int(sat(vr * 127 / 110, 127)) - vl = 3*int(sat(vl * 127 / 110, 127)) + + vr = int(deadzone(sat(vr, 255), 32, -32)) + vl = int(deadzone(sat(vl, 255), 32, -32)) + + # print( + # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") + # print( + # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") + # print( + # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") + # print( + # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") return vr, vl - def actuate(self, robot): - if not robot.on: - return (0, 0) - diff --git a/ALP-Winners/src/control/math.md b/ALP-Winners/src/control/math.md new file mode 100644 index 0000000..5377069 --- /dev/null +++ b/ALP-Winners/src/control/math.md @@ -0,0 +1,29 @@ +ALP-Winners + +Lei de controle da velocidade angular: + +Valor lido: $\theta_a$ (ângulo atual do robô) + +Referência: $\theta_r$ (ângulo calculado pelo UVF) + +Erro: $e_{th}$ ($\theta_r$ -$\theta_a$ ) + +Saída: w (velocidade angular) + +$$ w = \dot{\theta_r} + k_w\cdot \sqrt{e_{th}} +$$ +------ +Main System + +$$ v = \min(v_1,v_2,v_3)\\ +v_1 = \frac{-K_{\omega} \sqrt{|\theta_e|} + \\sqrt{K_{\omega}^2 |\theta_e| + 4 |\phi| A_m}}{2|\phi|}\\\\ + + +v_2 = \frac{2 \cdot V_m - L \cdot K_{\omega} \sqrt{|\theta_e|} }{2+L \cdot |\phi|}\\\\ + +v_3 = K_p ||\vec{r}-\vec{P}(1)||\\\\ + +\phi = \frac{\partial \theta_d}{\partial x} \cos(\theta) + \frac{\partial \theta_d}{\partial y} \sin(\theta)\\\\ + +\omega = v \cdot \phi + K_{\omega} \cdot \text{sign}(\theta_e) \sqrt{|\theta_e|} +$$ \ No newline at end of file diff --git a/ALP-Winners/src/control/motorControl.py b/ALP-Winners/src/control/motorControl.py deleted file mode 100644 index 2fcef9e..0000000 --- a/ALP-Winners/src/control/motorControl.py +++ /dev/null @@ -1,64 +0,0 @@ -import time -from tools import sat, deadzone - -class MotorControl: - def __init__(self, refk, kp, ki, deadzone): - self.kp = kp - self.ki = ki - self.refk = refk - self.deadzone = deadzone - self.old_err = 0 - self.old_out = 0 - - self.int = 0 - self.last_time = -1 - - def actuate(self, ref, err): - now = time.time() - dt = now - self.last_time if self.last_time > 0 else 0.040 - - self.int = sat(self.int + err * dt, 64) - out = self.refk * ref + self.kp * err + self.ki * self.int - - - return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) - - # def actuate(self, ref, err): - # ki = self.ki - # kp = self.kp - # T = 0.04 - # # now = time.time() - # # T = now - self.last_time if self.last_time > 0 else 0.040 - - # c0 = -kp + T/2 * ki - # c1 = kp + T/2 * ki - # A = 1 / T - - # out = A * (c1 * err + c0 * self.old_err) + self.old_out - # self.old_err = err - # self.old_out = out if abs(out) <= 127 else 0 - - # return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) - - # double PImotorA(double err){ - # static double old_err; - # static double old_out; - # double out = ( 1.6 * (err - 0.91 * old_err) + old_out); - # old_err = err; //- (saturation(out)-out); - # // 1.6 * (1-0.91 * z^-1)/(1-z^-1) - # // 1.6 * (z-0.91)/(z-1) Projetado a partir do LGR pro motor (identificação o motor) - # // - # // 1 - Identificar novo motor (com carga/com roda e no chão) - # // -MATLAB identificação de sistemas - # // -sintonização de constantes - # // 2 - Projetar um controlador (contínuo) - # // 3 - Discretizar controlador (mapeamento direto com transformada z) - # // -comando MATLAB: c2d(tf,T,"matched") - # // 4 - Expandir função de transferencia e isolar U(z) - # // 5 - Transformada inversa de z - # // - # // z = e^(sT) - - # old_out = (abs(out) < 255)? out : 0; // anti-windup (evita que o erro de saturação seja considerado como erro) - # return out; - # } diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 5009a06..534d48d 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -81,7 +81,7 @@ def loop(self): def busyLoop(self): message = self.pclient.receive() - self.execute = message["running"] + self.execute = message[23] if message is not None: self.world.update(message) # command = self.rc.receive() diff --git a/ALP-Winners/src/tools/__init__.py b/ALP-Winners/src/tools/__init__.py index 78dc3fc..36f0b3c 100644 --- a/ALP-Winners/src/tools/__init__.py +++ b/ALP-Winners/src/tools/__init__.py @@ -1,15 +1,9 @@ import numpy as np # Constantes físicas do robô -wheel_reduction = 5 - -# Supostamente devia ser -#r = 0.0325 -#L = 0.075 - -# O que realmente é r = 0.016 -L = 0.052 +L = 0.075 +wheel_reduction = 1 wheel_w_max = 110 conversion = 127 / wheel_w_max @@ -18,16 +12,12 @@ def deadzone(vin, up, down): return vin+up if (vin > 0) else vin-abs(down) return 0 -def speeds2motors(v: float, w: float) -> (int, int): +def speeds2motors(v: float, w: float): """Recebe velocidade linear e angular e retorna velocidades para as duas rodas""" # Computa a velocidade angular de rotação de cada roda - vr = (v + (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction - vl = (v - (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction - - #if fabs(vr) > max_motor_speed or fabs(vl) > max_motor_speed: - # vr = max_motor_speed * vr / max(vr, vl) - # vl = max_motor_speed * vl / max(vr, vl) + vr = (v + (L/2)*w) / r + vl = (v - (L/2)*w) / r # vr = 3*int(sat(vr * conversion, 127)) # vl = 3*int(sat(vl * conversion, 127)) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index 8093993..357d9b3 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -71,29 +71,18 @@ def update(self, message): blue = self.team robot_id = 0 - for robot in range(message["n_robots"]): + for robot in range(message[4]): if self.team_yellow: - yellow[robot_id].update( - message["robots"][robot_id]["pos_x"], - message["robots"][robot_id]["pos_y"], - message["robots"][robot_id]["th"], - message["robots"][robot_id]["vel_x"], - message["robots"][robot_id]["vel_y"], - message["robots"][robot_id]["w"] - ) + yellow[robot_id].update(message[5+6*robot], message[6+6*robot], message[7+6*robot], message[8+6*robot], message[9+6*robot], message[10+6*robot]) else: - blue[robot_id].update( - message["robots"][robot_id]["pos_x"], - message["robots"][robot_id]["pos_y"], - message["robots"][robot_id]["th"], - message["robots"][robot_id]["vel_x"], - message["robots"][robot_id]["vel_y"], - message["robots"][robot_id]["w"] - ) + blue[robot_id].update(message[5+6*robot], message[6+6*robot], message[7+6*robot], message[8+6*robot], message[9+6*robot], message[10+6*robot]) robot_id+=1 - self.ball.update(message["ball"]["pos_x"], message["ball"]["pos_y"], message["ball"]["vel_x"], message["ball"]["vel_y"]) - self.checkBatteries = message["check_batteries"] + self.ball.update(message[0], message[1], message[2], message[3]) + self.enableManualControl = message[24] + self.manualControlSpeedV = message[25] + self.manualControlSpeedW = message[26] + self.updateCount += 1 diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index edba2ea..3881fe7 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -1,7 +1,7 @@ from abc import ABC, abstractmethod import time import numpy as np -from controller.communication.server_pickle import ServerPickle +from controller.vision.server_pickle import ServerPickle from controller.vision.cameras import CameraHandler from controller.vision.visionMessage import VisionMessage diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/vision/server_pickle.py similarity index 100% rename from MainSystem/controller/communication/server_pickle.py rename to MainSystem/controller/vision/server_pickle.py diff --git a/requirements.txt b/requirements.txt deleted file mode 100644 index 56953ea..0000000 --- a/requirements.txt +++ /dev/null @@ -1,72 +0,0 @@ -application-utility==1.3.2 -attrs==22.2.0 -autocommand==2.2.2 -btrfsutil==6.2.2 -certifi==2022.12.7 -cffi==1.15.1 -chardet==5.1.0 -contourpy==1.0.7 -cryptography==40.0.1 -cupshelpers==1.0 -cycler==0.11.0 -docopt==0.6.2 -fastjsonschema==2.16.3 -fonttools==4.39.4 -future==0.18.3 -idna==3.4 -inflect==6.0.4 -iso8601==1.1.0 -jaraco.context==4.3.0 -jaraco.functools==3.6.0 -jaraco.text==3.11.1 -keyutils==0.6 -kiwisolver==1.4.4 -lensfun==0.3.3 -lit==15.0.7.dev0 -manjaro-sdk==0.1 -matplotlib==3.7.1 -more-itertools==9.1.0 -netsnmp-python==1.0a1 -nftables==0.1 -npyscreen==4.10.5 -numpy==1.24.3 -opencv-python==4.7.0.72 -ordered-set==4.1.0 -packaging==23.0 -pacman-mirrors==4.23.2 -Pillow==9.4.0 -platformdirs==3.2.0 -ply==3.11 -protobuf==4.23.2 -pyCAIR==0.1.13 -pycairo==1.23.0 -pycparser==2.21 -pycryptodomex==3.12.0 -pycups==2.0.1 -pycurl==7.45.2 -pydantic==1.10.7 -pygame==2.4.0 -PyGObject==3.44.1 -pyparsing==3.0.9 -PyQt5==5.15.9 -PyQt5-sip==12.12.1 -PySide6==6.5.0 -pysmbc==1.0.23 -python-dateutil==2.8.2 -PyYAML==6.0 -reportlab==3.6.12 -requests==2.28.2 -scipy==1.10.1 -serial==0.0.97 -shiboken6==6.5.0 -shiboken6-generator==6.5.0 -simplejson==3.18.0 -six==1.16.0 -systemd-python==235 -team==1.0 -tomli==2.0.1 -trove-classifiers==2023.4.20 -typing_extensions==4.5.0 -udiskie==2.4.2 -urllib3==1.26.13 -validate-pyproject==0.12.2.post1.dev0+g2940279.d20230328 From a862d47ab4492649f7a67a134631431b46431545 Mon Sep 17 00:00:00 2001 From: ayssag Date: Thu, 20 Jul 2023 18:03:44 -0300 Subject: [PATCH 29/88] Revert "Revert "Merge branch 'main' into dev-controle"" This reverts commit 99d5c41353828315d1430cb5d633703cca18a867. --- ALP-Winners/src/control/UFC.py | 216 ++++++++++++++++-- ALP-Winners/src/control/__init__.py | 70 ++++-- ALP-Winners/src/control/math.md | 29 --- ALP-Winners/src/control/motorControl.py | 64 ++++++ ALP-Winners/src/loop.py | 2 +- ALP-Winners/src/tools/__init__.py | 20 +- ALP-Winners/src/world/__init__.py | 27 ++- .../server_pickle.py | 0 MainSystem/controller/vision/__init__.py | 2 +- requirements.txt | 72 ++++++ 10 files changed, 422 insertions(+), 80 deletions(-) delete mode 100644 ALP-Winners/src/control/math.md create mode 100644 ALP-Winners/src/control/motorControl.py rename MainSystem/controller/{vision => communication}/server_pickle.py (100%) create mode 100644 requirements.txt diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index abe99ff..e5a8c50 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -5,6 +5,14 @@ import math import time +PLOT_CONTROL = False +if PLOT_CONTROL: + import matplotlib.pyplot as plt + +def close_event(): + plt.close() + + class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=False): @@ -18,9 +26,6 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vmax = vmax self.L = L self.kv = 10 - self.ki = 10 - self.kd = 1 - self.integral = 0 self.vbias = 0.2 self.sd_min = 1e-4 @@ -39,31 +44,53 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vPb = np.array([0,0]) self.eth = 0 + self.plots = {"ref":[], "out": [], "eth":[], "vref": [], "v": [], "wref": [], "w": [], "sd": [], "injection": [], "dth": []} @property def error(self): return self.eth + def abs_path_dth(self, initial_pose, error, field, step = 0.01, n = 10): + pos = np.array(initial_pose[:2]) + thlist = np.array([]) + + count = 0 + + for i in range(n): + th = field.F(pos) + thlist = np.append(thlist, th) + pos = pos + step * unit(th) + count += 1 + + aes = np.sum(np.abs(angError(thlist[1:], thlist[:-1]))) / n + + return aes + 0.05 * abs(error) + + def controlLine(self, x, xmax, xmin): + return sats((xmax - x) / (xmax - xmin), 0, 1) + + def output(self, robot): if robot.field is None: return 0,0 # Ângulo de referência + #th = (time.time()/1) % (2*np.pi) - np.pi#np.pi/2 * np.sign(time.time() % 3 - 1.5)#robot.field.F(robot.pose) th = robot.field.F(robot.pose) - # Erro angular eth = angError(th, robot.th) # Tempo desde a última atuação dt = self.interval.getInterval() - # Calcula a integral e satura (descobrir o quanto saturar, tirei esse 64 rad/s) - self.integral = sat(self.integral + eth * dt, 64) + # Derivada da referência + dth = filt(0.5 * (th - self.lastth[-1]) / dt + 0.2 * (th - self.lastth[-2]) / (2*dt) + 0.2 * (th - self.lastth[-3]) / (3*dt) + 0.1 * (th - self.lastth[-4]) / (4*dt), 10) + #dth = filt((th - self.lastth) / dt, 10) - # Calcula a derivada - # self.dth = eth/dt - # print("dth: ", self.dth) + # Erro de velocidade angular + ew = self.lastwref - robot.w - # Velocidade angular com controle PID - w = self.kp * eth + self.ki*self.integral# + self.kd*self.dth + # Lei de controle da velocidade angular + w = dth + self.kw * np.sqrt(abs(eth)) * np.sign(eth) #* (robot.velmod + 1) + #w = self.kw * eth + 0.3 * ew # Velocidade limite de deslizamento v1 = self.amax / np.abs(w) @@ -74,28 +101,171 @@ def output(self, robot): # Velocidade limite de aproximação v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - v = min(v1, v2, v3) - if self.world.enableManualControl: - v = self.world.manualControlSpeedV - # w = self.world.manualControlSpeedW - - if robot.id == 0: - print(f"ref(th): {(th * 180 / np.pi):.0f}º") - print(f"erro(th): {(eth * 180 / np.pi):.0f}º") - print(f"vref: {v:.2f}", end='') - print(f", wref: {w:.2f}") - print(f"v: {robot.velmod:.2f}", end='') - print(f", w: {robot.w:.2f}") + # v4 = self.kv / abs(eth) + self.vbias + + # Velocidade linear é menor de todas + sd = self.abs_path_dth(robot.pose, eth, robot.field) + #if robot.id == 0: print(sd) + Pb = np.array(robot.field.Pb[:2]) + + if self.enableInjection: + currentnorm = norm(robot.pos, robot.field.Pb) + #print(self.integrateinjection) + # if self.integrateinjection > 10: + # injection = 0 + # else: + # injection = 0.0015 / (abs(currentnorm - self.lastnorm) + 1e-3) + # self.integrateinjection = max(self.integrateinjection + injection - 0.5, 0) + # if abs(currentnorm - self.lastnorm) < 0.001: + # self.loadedInjection = 0.90 * self.loadedInjection + 10 * 0.10 + # else: + # self.loadedInjection = 0.90 * self.loadedInjection + #injection = 0.0010 / (abs(currentnorm - self.lastnorm) + 1e-3) + #injection = 2 * norml(self.vPb) * (np.dot(self.vPb, unit(robot.field.Pb[2])) < 0) * 0.10 / norm(robot.pos, robot.field.Pb[:2]) + + # Only apply injection near + d = norm(robot.pos, robot.field.Pb[:2]) + r1 = 0.10 + r2 = 0.50 + eta = sats((r2 - d) / (r2 - r1), 0, 1) + + # Injection if ball runs in oposite direction + if np.dot(self.vPb, unit(robot.field.Pb[2])) < 0: + raw_injection = max(-np.dot(self.vPb, unit(robot.field.Pb[2])), 0) + else: + raw_injection = 0.5 * max(np.dot(self.vPb, unit(robot.field.Pb[2])), 0) + + injection = raw_injection * eta + else: + currentnorm = 0 + injection = 0 + + v5 = self.vbias + (self.vmax-self.vbias) * self.controlLine(np.log(sd), np.log(self.sd_max), np.log(self.sd_min)) + v = min(v5, v3) + sat(injection, 1) + #print(vtarget) + #v = max(min(self.vbias + (self.vmax-self.vbias) * np.exp(-self.kapd * sd), v3), self.loadedInjection * vtarget)#max(min(v1, v2, v3, v4), 0) + #ev = self.lastvref - robot.velmod + #v = 1 * norm(robot.pos, robot.field.Pb) + 0.1 * ev + 0.2 + # v = 0.25#0.5*np.sin(2*time.time())+0.5 + # w = 0#2*np.sin(5*time.time()) # Atualiza a última referência self.lastth = self.lastth[1:] + [th] + self.lastnorm = currentnorm robot.lastControlLinVel = v # Atualiza variáveis de estado self.eth = eth + self.lastdth = dth self.lastwref = w self.lastvref = v + self.vPb = 0.90 * self.vPb + 0.10 * (Pb - self.lastPb) / dt + self.lastPb = Pb + + if PLOT_CONTROL: + self.plots["eth"].append(eth * 180 / np.pi) + self.plots["ref"].append(th * 180 / np.pi) + self.plots["out"].append(robot.th * 180 / np.pi) + self.plots["vref"].append(abs(v)) + self.plots["wref"].append(w) + self.plots["v"].append(robot.velmod) + self.plots["w"].append(robot.w) + self.plots["sd"].append(sd) + self.plots["injection"].append(injection) + self.plots["dth"].append(dth) + if len(self.plots["eth"]) >= 300 and robot.id == 0: + t = np.linspace(0, 300 * 0.016, 300) + fig = plt.figure() + #timer = fig.canvas.new_timer(interval = 5000) + #timer.add_callback(close_event) + plt.subplot(7,1,1) + plt.plot(t, self.plots["eth"], label='eth') + plt.plot(t, np.zeros_like(t), '--') + plt.legend() + plt.subplot(7,1,2) + plt.plot(t, self.plots["ref"], '--', label='th_ref') + plt.plot(t, self.plots["out"], label='th') + plt.legend() + plt.subplot(7,1,3) + plt.plot(t, self.plots["vref"], '--', label='vref') + plt.plot(t, self.plots["v"], label='v') + plt.legend() + plt.subplot(7,1,4) + plt.plot(t, self.plots["wref"], '--', label='wref') + plt.plot(t, self.plots["w"], label='w') + plt.legend() + plt.subplot(7,1,5) + plt.plot(t, self.plots["sd"], label='sd') + plt.legend() + plt.subplot(7,1,6) + plt.plot(t, self.plots["injection"], label='injection') + plt.legend() + plt.subplot(7,1,7) + plt.plot(t, self.plots["dth"], label='dth') + plt.legend() + #timer.start() + robot.stop() + plt.show() + #timer.stop() + for plot in self.plots.keys(): self.plots[plot] = [] + + #return (0,0) if robot.spin == 0: return (v * robot.direction, w) else: return (0, 60 * robot.spin) +# class UFC(): +# """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" +# def __init__(self): +# self.g = 9.8 +# self.kw = 5.5 +# self.kp = 10 +# self.mu = 0.7 +# self.amax = self.mu * self.g +# self.vmax = 2 +# self.L = 0.075 + +# self.lastth = 0 +# self.interval = Interval(filter=True, initial_dt=0.016) + +# def actuate(self, robot): +# if robot.field is None: return 0,0 +# # Ângulo de referência +# th = robot.field.F(robot.pose) +# # Erro angular +# eth = angError(th, robot.th) +# # Tempo desde a última atuação +# dt = self.interval.getInterval() +# # Derivada da referência +# dth = sat(angError(th, self.lastth) / dt, 15) +# # Computa phi +# phi = robot.field.phi(robot.pose) +# # Computa gamma +# gamma = robot.field.gamma(dth, robot.velmod, phi) +# #print("eth: {:.4f}\tphi: {:.4f}\tth: {:.4f}\trth: {:.4f}\t".format(eth*180/np.pi, phi, th*180/np.pi, robot.th*180/np.pi)) + +# # Computa omega +# omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) + gamma + +# # Velocidade limite de deslizamento +# if phi != 0: v1 = (-np.abs(omega) + np.sqrt(omega**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi)) +# if phi == 0: v1 = self.amax / np.abs(omega) + +# # Velocidade limite das rodas +# v2 = (2*self.vmax - self.L * np.abs(omega)) / (2 + self.L * np.abs(phi)) + +# # Velocidade limite de aproximação +# v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref + +# # Velocidade linear é menor de todas +# v = max(min(v1, v2, v3), 0) + +# # Lei de controle da velocidade angular +# w = v * phi + omega + +# # Atualiza a última referência +# self.lastth = th +# robot.lastControlLinVel = v + +# if robot.spin == 0: return speeds2motors(v * robot.direction, w) +# else: return speeds2motors(0, 60 * robot.spin) \ No newline at end of file diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index 7a40785..f3b64b1 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -1,5 +1,6 @@ from abc import ABC, abstractmethod from tools import speeds2motors, deadzone, sat +from control.motorControl import MotorControl import numpy as np import time @@ -9,32 +10,75 @@ def __init__(self, world): ABC.__init__(self) self.world = world + self.motor_vr_control = MotorControl(3, 0.2 , 0.05, 32) + self.motor_vl_control = MotorControl(3, 0.2, 0.05, 32) + # self.motor_vr_control = MotorControl(1, 1, 3, 32) + # self.motor_vl_control = MotorControl(1, 1, 3, 32) + + self.last_w = 0 @abstractmethod def output(self, robot): pass - def actuate(self, robot): + def actuateNoControl(self, robot): if not robot.on: return (0, 0) v, w = self.output(robot) + # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) + # w = 0 robot.lastControlLinVel = v vr, vl = speeds2motors(v, w) - - vr = int(deadzone(sat(vr, 255), 32, -32)) - vl = int(deadzone(sat(vl, 255), 32, -32)) - - # print( - # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # print( - # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") - # print( - # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") - # print( - # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") + + vr = 3*int(sat(vr * 127 / 110, 127)) + vl = 3*int(sat(vl * 127 / 110, 127)) return vr, vl + def actuate(self, robot): + if not robot.on: + return (0, 0) + + v, w = self.output(robot) + # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) + # w = 0 + robot.lastControlLinVel = v + + vr, vl = speeds2motors(v, w) + vision_w = self.last_w + sat(robot.angvel-self.last_w, 0.1) + vision_vr, vision_vl = speeds2motors(robot.v_signed, vision_w) + + ur = self.motor_vr_control.actuate(vr, vr - vision_vr) + ul = self.motor_vl_control.actuate(vl, vl - vision_vl) + + if robot.id == 0: + pass + # print( + # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") + # print( + # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") + # print( + # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") + # print( + # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") + + # vr, vl = speeds2motors(uv, uw) + + self.last_w = vision_w + return ur, ul + + # vr, vl = speeds2motors(v, self.world.field.side * w) + + # # return (vr, vl) + + # vision_vr, vision_vl = speeds2motors(robot.velmod, robot.angvel) + + # ur = self.motor_r_control.actuate(vr - vision_vr) + # ul = self.motor_l_control.actuate(vl - vision_vl) + + # if robot.id == 0: + # print(f"ur: {ur}, e: {vr-vision_vr}, vr: {vr}, vision_vr: {vision_vr}") + # return (int(ur), int(ul)) diff --git a/ALP-Winners/src/control/math.md b/ALP-Winners/src/control/math.md deleted file mode 100644 index 5377069..0000000 --- a/ALP-Winners/src/control/math.md +++ /dev/null @@ -1,29 +0,0 @@ -ALP-Winners - -Lei de controle da velocidade angular: - -Valor lido: $\theta_a$ (ângulo atual do robô) - -Referência: $\theta_r$ (ângulo calculado pelo UVF) - -Erro: $e_{th}$ ($\theta_r$ -$\theta_a$ ) - -Saída: w (velocidade angular) - -$$ w = \dot{\theta_r} + k_w\cdot \sqrt{e_{th}} -$$ ------- -Main System - -$$ v = \min(v_1,v_2,v_3)\\ -v_1 = \frac{-K_{\omega} \sqrt{|\theta_e|} + \\sqrt{K_{\omega}^2 |\theta_e| + 4 |\phi| A_m}}{2|\phi|}\\\\ - - -v_2 = \frac{2 \cdot V_m - L \cdot K_{\omega} \sqrt{|\theta_e|} }{2+L \cdot |\phi|}\\\\ - -v_3 = K_p ||\vec{r}-\vec{P}(1)||\\\\ - -\phi = \frac{\partial \theta_d}{\partial x} \cos(\theta) + \frac{\partial \theta_d}{\partial y} \sin(\theta)\\\\ - -\omega = v \cdot \phi + K_{\omega} \cdot \text{sign}(\theta_e) \sqrt{|\theta_e|} -$$ \ No newline at end of file diff --git a/ALP-Winners/src/control/motorControl.py b/ALP-Winners/src/control/motorControl.py new file mode 100644 index 0000000..2fcef9e --- /dev/null +++ b/ALP-Winners/src/control/motorControl.py @@ -0,0 +1,64 @@ +import time +from tools import sat, deadzone + +class MotorControl: + def __init__(self, refk, kp, ki, deadzone): + self.kp = kp + self.ki = ki + self.refk = refk + self.deadzone = deadzone + self.old_err = 0 + self.old_out = 0 + + self.int = 0 + self.last_time = -1 + + def actuate(self, ref, err): + now = time.time() + dt = now - self.last_time if self.last_time > 0 else 0.040 + + self.int = sat(self.int + err * dt, 64) + out = self.refk * ref + self.kp * err + self.ki * self.int + + + return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) + + # def actuate(self, ref, err): + # ki = self.ki + # kp = self.kp + # T = 0.04 + # # now = time.time() + # # T = now - self.last_time if self.last_time > 0 else 0.040 + + # c0 = -kp + T/2 * ki + # c1 = kp + T/2 * ki + # A = 1 / T + + # out = A * (c1 * err + c0 * self.old_err) + self.old_out + # self.old_err = err + # self.old_out = out if abs(out) <= 127 else 0 + + # return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) + + # double PImotorA(double err){ + # static double old_err; + # static double old_out; + # double out = ( 1.6 * (err - 0.91 * old_err) + old_out); + # old_err = err; //- (saturation(out)-out); + # // 1.6 * (1-0.91 * z^-1)/(1-z^-1) + # // 1.6 * (z-0.91)/(z-1) Projetado a partir do LGR pro motor (identificação o motor) + # // + # // 1 - Identificar novo motor (com carga/com roda e no chão) + # // -MATLAB identificação de sistemas + # // -sintonização de constantes + # // 2 - Projetar um controlador (contínuo) + # // 3 - Discretizar controlador (mapeamento direto com transformada z) + # // -comando MATLAB: c2d(tf,T,"matched") + # // 4 - Expandir função de transferencia e isolar U(z) + # // 5 - Transformada inversa de z + # // + # // z = e^(sT) + + # old_out = (abs(out) < 255)? out : 0; // anti-windup (evita que o erro de saturação seja considerado como erro) + # return out; + # } diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 534d48d..5009a06 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -81,7 +81,7 @@ def loop(self): def busyLoop(self): message = self.pclient.receive() - self.execute = message[23] + self.execute = message["running"] if message is not None: self.world.update(message) # command = self.rc.receive() diff --git a/ALP-Winners/src/tools/__init__.py b/ALP-Winners/src/tools/__init__.py index 36f0b3c..78dc3fc 100644 --- a/ALP-Winners/src/tools/__init__.py +++ b/ALP-Winners/src/tools/__init__.py @@ -1,9 +1,15 @@ import numpy as np # Constantes físicas do robô +wheel_reduction = 5 + +# Supostamente devia ser +#r = 0.0325 +#L = 0.075 + +# O que realmente é r = 0.016 -L = 0.075 -wheel_reduction = 1 +L = 0.052 wheel_w_max = 110 conversion = 127 / wheel_w_max @@ -12,12 +18,16 @@ def deadzone(vin, up, down): return vin+up if (vin > 0) else vin-abs(down) return 0 -def speeds2motors(v: float, w: float): +def speeds2motors(v: float, w: float) -> (int, int): """Recebe velocidade linear e angular e retorna velocidades para as duas rodas""" # Computa a velocidade angular de rotação de cada roda - vr = (v + (L/2)*w) / r - vl = (v - (L/2)*w) / r + vr = (v + (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction + vl = (v - (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction + + #if fabs(vr) > max_motor_speed or fabs(vl) > max_motor_speed: + # vr = max_motor_speed * vr / max(vr, vl) + # vl = max_motor_speed * vl / max(vr, vl) # vr = 3*int(sat(vr * conversion, 127)) # vl = 3*int(sat(vl * conversion, 127)) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index 357d9b3..8093993 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -71,18 +71,29 @@ def update(self, message): blue = self.team robot_id = 0 - for robot in range(message[4]): + for robot in range(message["n_robots"]): if self.team_yellow: - yellow[robot_id].update(message[5+6*robot], message[6+6*robot], message[7+6*robot], message[8+6*robot], message[9+6*robot], message[10+6*robot]) + yellow[robot_id].update( + message["robots"][robot_id]["pos_x"], + message["robots"][robot_id]["pos_y"], + message["robots"][robot_id]["th"], + message["robots"][robot_id]["vel_x"], + message["robots"][robot_id]["vel_y"], + message["robots"][robot_id]["w"] + ) else: - blue[robot_id].update(message[5+6*robot], message[6+6*robot], message[7+6*robot], message[8+6*robot], message[9+6*robot], message[10+6*robot]) + blue[robot_id].update( + message["robots"][robot_id]["pos_x"], + message["robots"][robot_id]["pos_y"], + message["robots"][robot_id]["th"], + message["robots"][robot_id]["vel_x"], + message["robots"][robot_id]["vel_y"], + message["robots"][robot_id]["w"] + ) robot_id+=1 - self.ball.update(message[0], message[1], message[2], message[3]) - self.enableManualControl = message[24] - self.manualControlSpeedV = message[25] - self.manualControlSpeedW = message[26] - + self.ball.update(message["ball"]["pos_x"], message["ball"]["pos_y"], message["ball"]["vel_x"], message["ball"]["vel_y"]) + self.checkBatteries = message["check_batteries"] self.updateCount += 1 diff --git a/MainSystem/controller/vision/server_pickle.py b/MainSystem/controller/communication/server_pickle.py similarity index 100% rename from MainSystem/controller/vision/server_pickle.py rename to MainSystem/controller/communication/server_pickle.py diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 3881fe7..edba2ea 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -1,7 +1,7 @@ from abc import ABC, abstractmethod import time import numpy as np -from controller.vision.server_pickle import ServerPickle +from controller.communication.server_pickle import ServerPickle from controller.vision.cameras import CameraHandler from controller.vision.visionMessage import VisionMessage diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..56953ea --- /dev/null +++ b/requirements.txt @@ -0,0 +1,72 @@ +application-utility==1.3.2 +attrs==22.2.0 +autocommand==2.2.2 +btrfsutil==6.2.2 +certifi==2022.12.7 +cffi==1.15.1 +chardet==5.1.0 +contourpy==1.0.7 +cryptography==40.0.1 +cupshelpers==1.0 +cycler==0.11.0 +docopt==0.6.2 +fastjsonschema==2.16.3 +fonttools==4.39.4 +future==0.18.3 +idna==3.4 +inflect==6.0.4 +iso8601==1.1.0 +jaraco.context==4.3.0 +jaraco.functools==3.6.0 +jaraco.text==3.11.1 +keyutils==0.6 +kiwisolver==1.4.4 +lensfun==0.3.3 +lit==15.0.7.dev0 +manjaro-sdk==0.1 +matplotlib==3.7.1 +more-itertools==9.1.0 +netsnmp-python==1.0a1 +nftables==0.1 +npyscreen==4.10.5 +numpy==1.24.3 +opencv-python==4.7.0.72 +ordered-set==4.1.0 +packaging==23.0 +pacman-mirrors==4.23.2 +Pillow==9.4.0 +platformdirs==3.2.0 +ply==3.11 +protobuf==4.23.2 +pyCAIR==0.1.13 +pycairo==1.23.0 +pycparser==2.21 +pycryptodomex==3.12.0 +pycups==2.0.1 +pycurl==7.45.2 +pydantic==1.10.7 +pygame==2.4.0 +PyGObject==3.44.1 +pyparsing==3.0.9 +PyQt5==5.15.9 +PyQt5-sip==12.12.1 +PySide6==6.5.0 +pysmbc==1.0.23 +python-dateutil==2.8.2 +PyYAML==6.0 +reportlab==3.6.12 +requests==2.28.2 +scipy==1.10.1 +serial==0.0.97 +shiboken6==6.5.0 +shiboken6-generator==6.5.0 +simplejson==3.18.0 +six==1.16.0 +systemd-python==235 +team==1.0 +tomli==2.0.1 +trove-classifiers==2023.4.20 +typing_extensions==4.5.0 +udiskie==2.4.2 +urllib3==1.26.13 +validate-pyproject==0.12.2.post1.dev0+g2940279.d20230328 From f6d5cc99bf4f33fc56ed5e71c7eb2ded4df25ad8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:37:07 -0300 Subject: [PATCH 30/88] Adiciona comentario de debug --- ALP-Winners/src/loop.py | 7 ++++--- README.md | 5 ++++- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 5009a06..691592c 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -58,7 +58,8 @@ def __init__( def loop(self): if self.world.updateCount == self.lastupdatecount: return - print((time.time()-self.t0)*1000) + if(self.debug): print((time.time()-self.t0)*1000) + self.t0 = time.time() self.lastupdatecount = self.world.updateCount @@ -67,8 +68,8 @@ def loop(self): self.strategy.update() # Executa o controle - control_output = [robot.entity.control.actuateNoControl(robot) for robot in self.world.team if robot.entity is not None] - + control_output = [robot.entity.control.actuate(robot) for robot in self.world.team if robot.entity is not None] + if self.execute: for robot in self.world.raw_team: robot.turnOn() self.radio.send(control_output) diff --git a/README.md b/README.md index ecd6088..6741e4d 100644 --- a/README.md +++ b/README.md @@ -21,9 +21,12 @@ pip install -r requirements.txt 4. No terminal 2, mover para o diretório ALP-Winners e executar com: ``` -python3 src/main.py --team-color blue --team-side left --immediate-start --port 5001 --n_robots 3 +python3 src/main.py --port 5001 ``` +## Para debug + +Para debugar, basta incluir como argumento ```--debug``` tanto quando for executar o MainSystem quanto o ALP-Winners. Por enquanto, as unicas informações que o sistema entrega no modo de debug é o tempo do loop. > ### Importante!! > From b58442d58b15e5b564c850acbe23791a8fa8238a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:37:41 -0300 Subject: [PATCH 31/88] add argumento de debug --- ALP-Winners/src/main.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ALP-Winners/src/main.py b/ALP-Winners/src/main.py index 23e2e79..36d39d2 100644 --- a/ALP-Winners/src/main.py +++ b/ALP-Winners/src/main.py @@ -14,6 +14,7 @@ parser.add_argument('--disable-alp-gui', dest='disable_alp_gui', action='store_const', const=True, default=False, help='If set, no communciation with ALP-GUI overhead will be added.') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') +parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() if args.disable_alp_gui: client.gui.disabled = True @@ -26,7 +27,8 @@ immediate_start=True, static_entities=False, port=args.port, - n_robots=3 + n_robots=3, + debug=args.debug, ) loop.run() \ No newline at end of file From decd3b1cdaf6799d1a8cafd35f301b2113e7553f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:38:45 -0300 Subject: [PATCH 32/88] actuate para controle UVF --- ALP-Winners/src/control/__init__.py | 74 +++++++---------------------- 1 file changed, 16 insertions(+), 58 deletions(-) diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index f3b64b1..cf18828 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -1,84 +1,42 @@ from abc import ABC, abstractmethod from tools import speeds2motors, deadzone, sat -from control.motorControl import MotorControl import numpy as np import time - class Control(ABC): def __init__(self, world): ABC.__init__(self) self.world = world - self.motor_vr_control = MotorControl(3, 0.2 , 0.05, 32) - self.motor_vl_control = MotorControl(3, 0.2, 0.05, 32) - # self.motor_vr_control = MotorControl(1, 1, 3, 32) - # self.motor_vl_control = MotorControl(1, 1, 3, 32) - - self.last_w = 0 @abstractmethod def output(self, robot): pass - def actuateNoControl(self, robot): + def actuate(self, robot, increment_control): if not robot.on: return (0, 0) - v, w = self.output(robot) - # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) - # w = 0 + # v, w = self.output(robot) + v = 0 + w = increment_control robot.lastControlLinVel = v vr, vl = speeds2motors(v, w) - - vr = 3*int(sat(vr * 127 / 110, 127)) - vl = 3*int(sat(vl * 127 / 110, 127)) - - return vr, vl - def actuate(self, robot): - if not robot.on: - return (0, 0) - - v, w = self.output(robot) - # v = 0.1 * np.sign(np.sin(2*np.pi*0.5*time.time())) - # w = 0 - robot.lastControlLinVel = v + vr = int(deadzone(sat(vr, 255), 32, -32)) + vl = int(deadzone(sat(vl, 255), 32, -32)) + + # print( + # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") + # print( + # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") + # print( + # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") + # print( + # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") - vr, vl = speeds2motors(v, w) - vision_w = self.last_w + sat(robot.angvel-self.last_w, 0.1) - vision_vr, vision_vl = speeds2motors(robot.v_signed, vision_w) - - ur = self.motor_vr_control.actuate(vr, vr - vision_vr) - ul = self.motor_vl_control.actuate(vl, vl - vision_vl) - - if robot.id == 0: - pass - # print( - # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # print( - # f"ul: {ul}\tel: {vl - vision_vl}, vl: {vl}, vision_vl: {vision_vl}") - # print( - # f"ur: {ur}\tev: {v - robot.v_signed}, v: {v}, vision_v: {robot.v_signed}") - # print( - # f"ul: {ul}\tew: {w - vision_w}, w: {w}, vision_w: {vision_w}") - - # vr, vl = speeds2motors(uv, uw) - - self.last_w = vision_w - return ur, ul - - # vr, vl = speeds2motors(v, self.world.field.side * w) - - # # return (vr, vl) - - # vision_vr, vision_vl = speeds2motors(robot.velmod, robot.angvel) + return vr, vl - # ur = self.motor_r_control.actuate(vr - vision_vr) - # ul = self.motor_l_control.actuate(vl - vision_vl) - # if robot.id == 0: - # print(f"ur: {ur}, e: {vr-vision_vr}, vr: {vr}, vision_vr: {vision_vr}") - # return (int(ur), int(ul)) From 98a33abb81135974b215adc34a5ceed53310e6c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:39:14 -0300 Subject: [PATCH 33/88] explicacao do controle do alp e MS --- ALP-Winners/src/control/math.md | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 ALP-Winners/src/control/math.md diff --git a/ALP-Winners/src/control/math.md b/ALP-Winners/src/control/math.md new file mode 100644 index 0000000..5377069 --- /dev/null +++ b/ALP-Winners/src/control/math.md @@ -0,0 +1,29 @@ +ALP-Winners + +Lei de controle da velocidade angular: + +Valor lido: $\theta_a$ (ângulo atual do robô) + +Referência: $\theta_r$ (ângulo calculado pelo UVF) + +Erro: $e_{th}$ ($\theta_r$ -$\theta_a$ ) + +Saída: w (velocidade angular) + +$$ w = \dot{\theta_r} + k_w\cdot \sqrt{e_{th}} +$$ +------ +Main System + +$$ v = \min(v_1,v_2,v_3)\\ +v_1 = \frac{-K_{\omega} \sqrt{|\theta_e|} + \\sqrt{K_{\omega}^2 |\theta_e| + 4 |\phi| A_m}}{2|\phi|}\\\\ + + +v_2 = \frac{2 \cdot V_m - L \cdot K_{\omega} \sqrt{|\theta_e|} }{2+L \cdot |\phi|}\\\\ + +v_3 = K_p ||\vec{r}-\vec{P}(1)||\\\\ + +\phi = \frac{\partial \theta_d}{\partial x} \cos(\theta) + \frac{\partial \theta_d}{\partial y} \sin(\theta)\\\\ + +\omega = v \cdot \phi + K_{\omega} \cdot \text{sign}(\theta_e) \sqrt{|\theta_e|} +$$ \ No newline at end of file From 7117f9828d4cb8a76e0b4a1b745c768ce2e46651 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:39:50 -0300 Subject: [PATCH 34/88] =?UTF-8?q?implementa=20controle=20PI=20anal=C3=B3gi?= =?UTF-8?q?co?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/control/UFC.py | 216 ++++----------------------------- 1 file changed, 23 insertions(+), 193 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index e5a8c50..abe99ff 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -5,14 +5,6 @@ import math import time -PLOT_CONTROL = False -if PLOT_CONTROL: - import matplotlib.pyplot as plt - -def close_event(): - plt.close() - - class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=False): @@ -26,6 +18,9 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vmax = vmax self.L = L self.kv = 10 + self.ki = 10 + self.kd = 1 + self.integral = 0 self.vbias = 0.2 self.sd_min = 1e-4 @@ -44,53 +39,31 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.vPb = np.array([0,0]) self.eth = 0 - self.plots = {"ref":[], "out": [], "eth":[], "vref": [], "v": [], "wref": [], "w": [], "sd": [], "injection": [], "dth": []} @property def error(self): return self.eth - def abs_path_dth(self, initial_pose, error, field, step = 0.01, n = 10): - pos = np.array(initial_pose[:2]) - thlist = np.array([]) - - count = 0 - - for i in range(n): - th = field.F(pos) - thlist = np.append(thlist, th) - pos = pos + step * unit(th) - count += 1 - - aes = np.sum(np.abs(angError(thlist[1:], thlist[:-1]))) / n - - return aes + 0.05 * abs(error) - - def controlLine(self, x, xmax, xmin): - return sats((xmax - x) / (xmax - xmin), 0, 1) - - def output(self, robot): if robot.field is None: return 0,0 # Ângulo de referência - #th = (time.time()/1) % (2*np.pi) - np.pi#np.pi/2 * np.sign(time.time() % 3 - 1.5)#robot.field.F(robot.pose) th = robot.field.F(robot.pose) + # Erro angular eth = angError(th, robot.th) # Tempo desde a última atuação dt = self.interval.getInterval() - # Derivada da referência - dth = filt(0.5 * (th - self.lastth[-1]) / dt + 0.2 * (th - self.lastth[-2]) / (2*dt) + 0.2 * (th - self.lastth[-3]) / (3*dt) + 0.1 * (th - self.lastth[-4]) / (4*dt), 10) - #dth = filt((th - self.lastth) / dt, 10) + # Calcula a integral e satura (descobrir o quanto saturar, tirei esse 64 rad/s) + self.integral = sat(self.integral + eth * dt, 64) - # Erro de velocidade angular - ew = self.lastwref - robot.w + # Calcula a derivada + # self.dth = eth/dt + # print("dth: ", self.dth) - # Lei de controle da velocidade angular - w = dth + self.kw * np.sqrt(abs(eth)) * np.sign(eth) #* (robot.velmod + 1) - #w = self.kw * eth + 0.3 * ew + # Velocidade angular com controle PID + w = self.kp * eth + self.ki*self.integral# + self.kd*self.dth # Velocidade limite de deslizamento v1 = self.amax / np.abs(w) @@ -101,171 +74,28 @@ def output(self, robot): # Velocidade limite de aproximação v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - # v4 = self.kv / abs(eth) + self.vbias - - # Velocidade linear é menor de todas - sd = self.abs_path_dth(robot.pose, eth, robot.field) - #if robot.id == 0: print(sd) - Pb = np.array(robot.field.Pb[:2]) - - if self.enableInjection: - currentnorm = norm(robot.pos, robot.field.Pb) - #print(self.integrateinjection) - # if self.integrateinjection > 10: - # injection = 0 - # else: - # injection = 0.0015 / (abs(currentnorm - self.lastnorm) + 1e-3) - # self.integrateinjection = max(self.integrateinjection + injection - 0.5, 0) - # if abs(currentnorm - self.lastnorm) < 0.001: - # self.loadedInjection = 0.90 * self.loadedInjection + 10 * 0.10 - # else: - # self.loadedInjection = 0.90 * self.loadedInjection - #injection = 0.0010 / (abs(currentnorm - self.lastnorm) + 1e-3) - #injection = 2 * norml(self.vPb) * (np.dot(self.vPb, unit(robot.field.Pb[2])) < 0) * 0.10 / norm(robot.pos, robot.field.Pb[:2]) - - # Only apply injection near - d = norm(robot.pos, robot.field.Pb[:2]) - r1 = 0.10 - r2 = 0.50 - eta = sats((r2 - d) / (r2 - r1), 0, 1) - - # Injection if ball runs in oposite direction - if np.dot(self.vPb, unit(robot.field.Pb[2])) < 0: - raw_injection = max(-np.dot(self.vPb, unit(robot.field.Pb[2])), 0) - else: - raw_injection = 0.5 * max(np.dot(self.vPb, unit(robot.field.Pb[2])), 0) - - injection = raw_injection * eta - else: - currentnorm = 0 - injection = 0 - - v5 = self.vbias + (self.vmax-self.vbias) * self.controlLine(np.log(sd), np.log(self.sd_max), np.log(self.sd_min)) - v = min(v5, v3) + sat(injection, 1) - #print(vtarget) - #v = max(min(self.vbias + (self.vmax-self.vbias) * np.exp(-self.kapd * sd), v3), self.loadedInjection * vtarget)#max(min(v1, v2, v3, v4), 0) - #ev = self.lastvref - robot.velmod - #v = 1 * norm(robot.pos, robot.field.Pb) + 0.1 * ev + 0.2 - # v = 0.25#0.5*np.sin(2*time.time())+0.5 - # w = 0#2*np.sin(5*time.time()) + v = min(v1, v2, v3) + if self.world.enableManualControl: + v = self.world.manualControlSpeedV + # w = self.world.manualControlSpeedW + + if robot.id == 0: + print(f"ref(th): {(th * 180 / np.pi):.0f}º") + print(f"erro(th): {(eth * 180 / np.pi):.0f}º") + print(f"vref: {v:.2f}", end='') + print(f", wref: {w:.2f}") + print(f"v: {robot.velmod:.2f}", end='') + print(f", w: {robot.w:.2f}") # Atualiza a última referência self.lastth = self.lastth[1:] + [th] - self.lastnorm = currentnorm robot.lastControlLinVel = v # Atualiza variáveis de estado self.eth = eth - self.lastdth = dth self.lastwref = w self.lastvref = v - self.vPb = 0.90 * self.vPb + 0.10 * (Pb - self.lastPb) / dt - self.lastPb = Pb - - if PLOT_CONTROL: - self.plots["eth"].append(eth * 180 / np.pi) - self.plots["ref"].append(th * 180 / np.pi) - self.plots["out"].append(robot.th * 180 / np.pi) - self.plots["vref"].append(abs(v)) - self.plots["wref"].append(w) - self.plots["v"].append(robot.velmod) - self.plots["w"].append(robot.w) - self.plots["sd"].append(sd) - self.plots["injection"].append(injection) - self.plots["dth"].append(dth) - if len(self.plots["eth"]) >= 300 and robot.id == 0: - t = np.linspace(0, 300 * 0.016, 300) - fig = plt.figure() - #timer = fig.canvas.new_timer(interval = 5000) - #timer.add_callback(close_event) - plt.subplot(7,1,1) - plt.plot(t, self.plots["eth"], label='eth') - plt.plot(t, np.zeros_like(t), '--') - plt.legend() - plt.subplot(7,1,2) - plt.plot(t, self.plots["ref"], '--', label='th_ref') - plt.plot(t, self.plots["out"], label='th') - plt.legend() - plt.subplot(7,1,3) - plt.plot(t, self.plots["vref"], '--', label='vref') - plt.plot(t, self.plots["v"], label='v') - plt.legend() - plt.subplot(7,1,4) - plt.plot(t, self.plots["wref"], '--', label='wref') - plt.plot(t, self.plots["w"], label='w') - plt.legend() - plt.subplot(7,1,5) - plt.plot(t, self.plots["sd"], label='sd') - plt.legend() - plt.subplot(7,1,6) - plt.plot(t, self.plots["injection"], label='injection') - plt.legend() - plt.subplot(7,1,7) - plt.plot(t, self.plots["dth"], label='dth') - plt.legend() - #timer.start() - robot.stop() - plt.show() - #timer.stop() - for plot in self.plots.keys(): self.plots[plot] = [] - - #return (0,0) if robot.spin == 0: return (v * robot.direction, w) else: return (0, 60 * robot.spin) -# class UFC(): -# """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" -# def __init__(self): -# self.g = 9.8 -# self.kw = 5.5 -# self.kp = 10 -# self.mu = 0.7 -# self.amax = self.mu * self.g -# self.vmax = 2 -# self.L = 0.075 - -# self.lastth = 0 -# self.interval = Interval(filter=True, initial_dt=0.016) - -# def actuate(self, robot): -# if robot.field is None: return 0,0 -# # Ângulo de referência -# th = robot.field.F(robot.pose) -# # Erro angular -# eth = angError(th, robot.th) -# # Tempo desde a última atuação -# dt = self.interval.getInterval() -# # Derivada da referência -# dth = sat(angError(th, self.lastth) / dt, 15) -# # Computa phi -# phi = robot.field.phi(robot.pose) -# # Computa gamma -# gamma = robot.field.gamma(dth, robot.velmod, phi) -# #print("eth: {:.4f}\tphi: {:.4f}\tth: {:.4f}\trth: {:.4f}\t".format(eth*180/np.pi, phi, th*180/np.pi, robot.th*180/np.pi)) - -# # Computa omega -# omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) + gamma - -# # Velocidade limite de deslizamento -# if phi != 0: v1 = (-np.abs(omega) + np.sqrt(omega**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi)) -# if phi == 0: v1 = self.amax / np.abs(omega) - -# # Velocidade limite das rodas -# v2 = (2*self.vmax - self.L * np.abs(omega)) / (2 + self.L * np.abs(phi)) - -# # Velocidade limite de aproximação -# v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - -# # Velocidade linear é menor de todas -# v = max(min(v1, v2, v3), 0) - -# # Lei de controle da velocidade angular -# w = v * phi + omega - -# # Atualiza a última referência -# self.lastth = th -# robot.lastControlLinVel = v - -# if robot.spin == 0: return speeds2motors(v * robot.direction, w) -# else: return speeds2motors(0, 60 * robot.spin) \ No newline at end of file From 5ce7d18df79e418b3272a37c90a2aa9b2a5cde8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:40:40 -0300 Subject: [PATCH 35/88] corrige wheel_reduction e L --- ALP-Winners/src/tools/__init__.py | 20 +++++--------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/ALP-Winners/src/tools/__init__.py b/ALP-Winners/src/tools/__init__.py index 78dc3fc..36f0b3c 100644 --- a/ALP-Winners/src/tools/__init__.py +++ b/ALP-Winners/src/tools/__init__.py @@ -1,15 +1,9 @@ import numpy as np # Constantes físicas do robô -wheel_reduction = 5 - -# Supostamente devia ser -#r = 0.0325 -#L = 0.075 - -# O que realmente é r = 0.016 -L = 0.052 +L = 0.075 +wheel_reduction = 1 wheel_w_max = 110 conversion = 127 / wheel_w_max @@ -18,16 +12,12 @@ def deadzone(vin, up, down): return vin+up if (vin > 0) else vin-abs(down) return 0 -def speeds2motors(v: float, w: float) -> (int, int): +def speeds2motors(v: float, w: float): """Recebe velocidade linear e angular e retorna velocidades para as duas rodas""" # Computa a velocidade angular de rotação de cada roda - vr = (v + (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction - vl = (v - (L/2)*w) / r#/ (2*np.pi*r) * wheel_reduction - - #if fabs(vr) > max_motor_speed or fabs(vl) > max_motor_speed: - # vr = max_motor_speed * vr / max(vr, vl) - # vl = max_motor_speed * vl / max(vr, vl) + vr = (v + (L/2)*w) / r + vl = (v - (L/2)*w) / r # vr = 3*int(sat(vr * conversion, 127)) # vl = 3*int(sat(vl * conversion, 127)) From e8eae7cb58915e375ceb448c730236df8ad74db0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:44:12 -0300 Subject: [PATCH 36/88] troca nome de checkBatteries --- ALP-Winners/src/world/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index 8093993..78741f2 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -61,7 +61,7 @@ def __init__(self, n_robots=5, side=1, vss=None, team_yellow=False, immediate_st self.enableManualControl = False self.manualControlSpeedV = 0 self.manualControlSpeedW = 0 - + def update(self, message): if self.team_yellow: yellow = self.team @@ -93,7 +93,7 @@ def update(self, message): robot_id+=1 self.ball.update(message["ball"]["pos_x"], message["ball"]["pos_y"], message["ball"]["vel_x"], message["ball"]["vel_y"]) - self.checkBatteries = message["check_batteries"] + self.enableManualControl = message["check_batteries"] self.updateCount += 1 From 0b25ee61e95131b99283a7198c155d91f6d53992 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:45:37 -0300 Subject: [PATCH 37/88] adiciona arg de debug --- MainSystem/main.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/MainSystem/main.py b/MainSystem/main.py index a07cf34..3b5ff6a 100755 --- a/MainSystem/main.py +++ b/MainSystem/main.py @@ -11,11 +11,12 @@ parser = argparse.ArgumentParser(description='UnBall Main System') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') +parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() def main(): """Função principal que instancia os componentes base, abre a interface gráfica e faz o flush dos dados em memória permanente ao terminar""" - controller = Controller(port=args.port, n_robots=3) + controller = Controller(port=args.port, n_robots=3, debug=args.debug) view = View(controller) view.run() From 2a274a06131fc4ba037b26908028c6e3fd8f4017 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:45:55 -0300 Subject: [PATCH 38/88] adiciona debug --- MainSystem/controller/communication/server_pickle.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/communication/server_pickle.py index 0eee132..0b9d090 100644 --- a/MainSystem/controller/communication/server_pickle.py +++ b/MainSystem/controller/communication/server_pickle.py @@ -3,7 +3,7 @@ import socket class ServerPickle: - def __init__(self, port): + def __init__(self, port, debug): self.host = socket.gethostname() self.port = port @@ -13,11 +13,14 @@ def __init__(self, port): psocket.listen(2) self.conn, self.address = psocket.accept() print("Connection from: " + str(self.address)) + + self.debug = debug self.t0 = time.time() def send(self, data): - print(1000*(time.time()-self.t0)) + + if(self.debug): print(1000*(time.time()-self.t0)) self.t0 = time.time() message = pickle.dumps(data,-1) From 4b3893b7a96e8aec97726f034909b2e243959307 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:50:03 -0300 Subject: [PATCH 39/88] adiciona debug e TODO controlVeW --- MainSystem/controller/vision/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index edba2ea..8e36254 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -8,7 +8,7 @@ class Vision(ABC): """Classe que define as interfaces que qualquer sistema de visão deve ter no sistema.""" - def __init__(self, world, port): + def __init__(self, world, port,debug): super().__init__() self.cameraHandler = CameraHandler() @@ -20,7 +20,7 @@ def __init__(self, world, port): self.usePastPositions = False self.lastCandidateUse = 0 - self.server_pickle = ServerPickle(port) + self.server_pickle = ServerPickle(port, debug) @abstractmethod def process(self, frame): From c01784db47039473437cb201cfee50b4a725b501 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:50:18 -0300 Subject: [PATCH 40/88] add debug --- MainSystem/controller/vision/mainVision/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MainSystem/controller/vision/mainVision/__init__.py b/MainSystem/controller/vision/mainVision/__init__.py index a01d1b5..314635a 100644 --- a/MainSystem/controller/vision/mainVision/__init__.py +++ b/MainSystem/controller/vision/mainVision/__init__.py @@ -10,8 +10,8 @@ class MainVision(Vision): """Classe que implementa a visão principal da UnBall, que utiliza segmentação por única cor e faz a identificação por forma.""" - def __init__(self, world, port): - super().__init__(world=world, port=port) + def __init__(self, world, port, debug): + super().__init__(world=world, port=port, debug=debug) self.__model = MainVisionModel() """Modelo `MainSystem.model.vision.mainVisionModel.MainVisionModel` que mantém as variáveis da visão""" From a3599028ffc3083255a53907fa990f81afc41146 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 18:52:32 -0300 Subject: [PATCH 41/88] adiciona print de loop da visao --- MainSystem/view/vision/mainVision/visaoAltoNivel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MainSystem/view/vision/mainVision/visaoAltoNivel.py b/MainSystem/view/vision/mainVision/visaoAltoNivel.py index dd88fa5..f995d23 100644 --- a/MainSystem/view/vision/mainVision/visaoAltoNivel.py +++ b/MainSystem/view/vision/mainVision/visaoAltoNivel.py @@ -38,7 +38,7 @@ def getFrame(self): img_filtered = cv2.bitwise_and(img_warpped, img_warpped, mask=fgMask) t0 = time.time() message = self.__visionSystem.process(frame) - print((time.time()-t0)*1000) + print("loop da visao:", (time.time()-t0)*1000) GLib.idle_add(self.updateRobotsInfo, message) Drawing.draw_field(self.__world, img_filtered) From ef7d2fa84b220cac512976a6659c99dc6c8d0a67 Mon Sep 17 00:00:00 2001 From: ayssag Date: Thu, 20 Jul 2023 17:27:14 -0500 Subject: [PATCH 42/88] =?UTF-8?q?Altera=C3=A7=C3=A3o=20para=20rodar=20em?= =?UTF-8?q?=20bash?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- run.sh | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100755 run.sh diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..906e31f --- /dev/null +++ b/run.sh @@ -0,0 +1,9 @@ +mkdir -p logs + +PORTA="5001" + +if [[ -n $1 ]]; then + PORTA="$1" +fi + +( (cd MainSystem && ./main.py --port "$PORTA") & (sleep 0.5s && python3 ALP-Winners/src/main.py --port "$PORTA") ) \ No newline at end of file From 8f5a8862627d748c4cef7c6d660b3a23dd086526 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 19:17:28 -0300 Subject: [PATCH 43/88] remove debug --- ALP-Winners/src/loop.py | 2 +- ALP-Winners/src/main.py | 2 -- MainSystem/controller/communication/server_pickle.py | 6 ++---- MainSystem/controller/vision/__init__.py | 4 ++-- MainSystem/controller/vision/mainVision/__init__.py | 4 ++-- MainSystem/main.py | 3 +-- 6 files changed, 8 insertions(+), 13 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 691592c..49b04f2 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -58,7 +58,7 @@ def __init__( def loop(self): if self.world.updateCount == self.lastupdatecount: return - if(self.debug): print((time.time()-self.t0)*1000) + print((time.time()-self.t0)*1000) self.t0 = time.time() diff --git a/ALP-Winners/src/main.py b/ALP-Winners/src/main.py index 36d39d2..d4c1cd3 100644 --- a/ALP-Winners/src/main.py +++ b/ALP-Winners/src/main.py @@ -14,7 +14,6 @@ parser.add_argument('--disable-alp-gui', dest='disable_alp_gui', action='store_const', const=True, default=False, help='If set, no communciation with ALP-GUI overhead will be added.') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') -parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() if args.disable_alp_gui: client.gui.disabled = True @@ -28,7 +27,6 @@ static_entities=False, port=args.port, n_robots=3, - debug=args.debug, ) loop.run() \ No newline at end of file diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/communication/server_pickle.py index 0b9d090..9c7fbef 100644 --- a/MainSystem/controller/communication/server_pickle.py +++ b/MainSystem/controller/communication/server_pickle.py @@ -3,7 +3,7 @@ import socket class ServerPickle: - def __init__(self, port, debug): + def __init__(self, port): self.host = socket.gethostname() self.port = port @@ -13,14 +13,12 @@ def __init__(self, port, debug): psocket.listen(2) self.conn, self.address = psocket.accept() print("Connection from: " + str(self.address)) - - self.debug = debug self.t0 = time.time() def send(self, data): - if(self.debug): print(1000*(time.time()-self.t0)) + print(1000*(time.time()-self.t0)) self.t0 = time.time() message = pickle.dumps(data,-1) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 8e36254..edba2ea 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -8,7 +8,7 @@ class Vision(ABC): """Classe que define as interfaces que qualquer sistema de visão deve ter no sistema.""" - def __init__(self, world, port,debug): + def __init__(self, world, port): super().__init__() self.cameraHandler = CameraHandler() @@ -20,7 +20,7 @@ def __init__(self, world, port,debug): self.usePastPositions = False self.lastCandidateUse = 0 - self.server_pickle = ServerPickle(port, debug) + self.server_pickle = ServerPickle(port) @abstractmethod def process(self, frame): diff --git a/MainSystem/controller/vision/mainVision/__init__.py b/MainSystem/controller/vision/mainVision/__init__.py index 314635a..a01d1b5 100644 --- a/MainSystem/controller/vision/mainVision/__init__.py +++ b/MainSystem/controller/vision/mainVision/__init__.py @@ -10,8 +10,8 @@ class MainVision(Vision): """Classe que implementa a visão principal da UnBall, que utiliza segmentação por única cor e faz a identificação por forma.""" - def __init__(self, world, port, debug): - super().__init__(world=world, port=port, debug=debug) + def __init__(self, world, port): + super().__init__(world=world, port=port) self.__model = MainVisionModel() """Modelo `MainSystem.model.vision.mainVisionModel.MainVisionModel` que mantém as variáveis da visão""" diff --git a/MainSystem/main.py b/MainSystem/main.py index 3b5ff6a..a07cf34 100755 --- a/MainSystem/main.py +++ b/MainSystem/main.py @@ -11,12 +11,11 @@ parser = argparse.ArgumentParser(description='UnBall Main System') parser.add_argument('--port', dest='port', type=int, default=5001, help='Port number to bind the pickle socket.') parser.add_argument('--n_robots', dest='n_robots', type=int, default=5, help='Number of robots for each time in the match.') -parser.add_argument('--debug', dest='debug', action='store_const', const=True, default=False, help="Show loop execution time" ) args = parser.parse_args() def main(): """Função principal que instancia os componentes base, abre a interface gráfica e faz o flush dos dados em memória permanente ao terminar""" - controller = Controller(port=args.port, n_robots=3, debug=args.debug) + controller = Controller(port=args.port, n_robots=3) view = View(controller) view.run() From 8ef0d5fe902f5566d2953061d1b75a249ef74774 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 19:18:58 -0300 Subject: [PATCH 44/88] remove virgula --- ALP-Winners/src/main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ALP-Winners/src/main.py b/ALP-Winners/src/main.py index d4c1cd3..23e2e79 100644 --- a/ALP-Winners/src/main.py +++ b/ALP-Winners/src/main.py @@ -26,7 +26,7 @@ immediate_start=True, static_entities=False, port=args.port, - n_robots=3, + n_robots=3 ) loop.run() \ No newline at end of file From 5769397b5061045dc3a6801b655c8bf3f7932bc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 19:24:56 -0300 Subject: [PATCH 45/88] muda nome checkbatteries --- ALP-Winners/src/world/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index 78741f2..3db562c 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -58,7 +58,7 @@ def __init__(self, n_robots=5, side=1, vss=None, team_yellow=False, immediate_st self.allyGoals = 0 self.enemyGoals = 0 self.updateCount = 0 - self.enableManualControl = False + self.checkBatteries = False self.manualControlSpeedV = 0 self.manualControlSpeedW = 0 @@ -93,7 +93,7 @@ def update(self, message): robot_id+=1 self.ball.update(message["ball"]["pos_x"], message["ball"]["pos_y"], message["ball"]["vel_x"], message["ball"]["vel_y"]) - self.enableManualControl = message["check_batteries"] + self.checkBatteries = message["check_batteries"] self.updateCount += 1 From 9f33d308637c303709aa0e3c7504215967ea4653 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 19:26:08 -0300 Subject: [PATCH 46/88] muda nome checkbatteries --- MainSystem/controller/states/debugHLC.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/MainSystem/controller/states/debugHLC.py b/MainSystem/controller/states/debugHLC.py index 516db1b..01007cd 100644 --- a/MainSystem/controller/states/debugHLC.py +++ b/MainSystem/controller/states/debugHLC.py @@ -163,7 +163,6 @@ def update(self): # Computa o tempo desde o último loop e salva dt = time.time()-self.t self.t = time.time() - # Atualiza o mundo com a visão if self.getParam("runVision"): @@ -180,7 +179,7 @@ def update(self): # Controle manual if self.getParam("enableManualControl"): - self.world.enableManualControl = True + self.world.checkBatteries = True self.world.manualControlSpeedV = self.getParam("manualControlSpeedV") self.world.manualControlSpeedW = self.getParam("manualControlSpeedW") manualSpeed = SpeedPair(self.getParam("manualControlSpeedV"), self.getParam("manualControlSpeedW")) @@ -188,7 +187,7 @@ def update(self): # Controle de alto nível else: - self.world.enableManualControl = False + self.world.checkBatteries = False speeds = [r.actuate() for r in self.robots] # Obtém o target instantâneo From e5c2560dd9b3ed6e8447141a3762e0f3d21387a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 19:26:40 -0300 Subject: [PATCH 47/88] apaga comentario --- MainSystem/controller/vision/__init__.py | 1 - 1 file changed, 1 deletion(-) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index edba2ea..38c1b7c 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -71,7 +71,6 @@ def update(self): # message.append(self._world.manualControlSpeedV) # message.append(self._world.manualControlSpeedW) - self.server_pickle.send(message) if self.usePastPositions is False: From 259d8d3da4c95f93f3378855d514efc09c93d19e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 19:27:00 -0300 Subject: [PATCH 48/88] muda nome checkbatteries --- MainSystem/controller/world/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MainSystem/controller/world/__init__.py b/MainSystem/controller/world/__init__.py index c14f3bd..ddb1288 100644 --- a/MainSystem/controller/world/__init__.py +++ b/MainSystem/controller/world/__init__.py @@ -44,7 +44,7 @@ def __init__(self, n_robots): self.n_robots = n_robots self.fieldSide = Field.RIGHT self.running = False - self.enableManualControl = False + self.checkBatteries = False self.manualControlSpeedV = 0 self.manualControlSpeedW = 0 self.mus = [0.07, 0.07, 0.12, 0.07, 0.07] From 068293571330526b7ab8f4f85298816686bddee8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 19:52:46 -0300 Subject: [PATCH 49/88] remove controle antigo --- ALP-Winners/src/control/motorControl.py | 64 ------------------------- 1 file changed, 64 deletions(-) delete mode 100644 ALP-Winners/src/control/motorControl.py diff --git a/ALP-Winners/src/control/motorControl.py b/ALP-Winners/src/control/motorControl.py deleted file mode 100644 index 2fcef9e..0000000 --- a/ALP-Winners/src/control/motorControl.py +++ /dev/null @@ -1,64 +0,0 @@ -import time -from tools import sat, deadzone - -class MotorControl: - def __init__(self, refk, kp, ki, deadzone): - self.kp = kp - self.ki = ki - self.refk = refk - self.deadzone = deadzone - self.old_err = 0 - self.old_out = 0 - - self.int = 0 - self.last_time = -1 - - def actuate(self, ref, err): - now = time.time() - dt = now - self.last_time if self.last_time > 0 else 0.040 - - self.int = sat(self.int + err * dt, 64) - out = self.refk * ref + self.kp * err + self.ki * self.int - - - return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) - - # def actuate(self, ref, err): - # ki = self.ki - # kp = self.kp - # T = 0.04 - # # now = time.time() - # # T = now - self.last_time if self.last_time > 0 else 0.040 - - # c0 = -kp + T/2 * ki - # c1 = kp + T/2 * ki - # A = 1 / T - - # out = A * (c1 * err + c0 * self.old_err) + self.old_out - # self.old_err = err - # self.old_out = out if abs(out) <= 127 else 0 - - # return int(deadzone(sat(out, 255), self.deadzone, -self.deadzone)) - - # double PImotorA(double err){ - # static double old_err; - # static double old_out; - # double out = ( 1.6 * (err - 0.91 * old_err) + old_out); - # old_err = err; //- (saturation(out)-out); - # // 1.6 * (1-0.91 * z^-1)/(1-z^-1) - # // 1.6 * (z-0.91)/(z-1) Projetado a partir do LGR pro motor (identificação o motor) - # // - # // 1 - Identificar novo motor (com carga/com roda e no chão) - # // -MATLAB identificação de sistemas - # // -sintonização de constantes - # // 2 - Projetar um controlador (contínuo) - # // 3 - Discretizar controlador (mapeamento direto com transformada z) - # // -comando MATLAB: c2d(tf,T,"matched") - # // 4 - Expandir função de transferencia e isolar U(z) - # // 5 - Transformada inversa de z - # // - # // z = e^(sT) - - # old_out = (abs(out) < 255)? out : 0; // anti-windup (evita que o erro de saturação seja considerado como erro) - # return out; - # } From 88f4cadb04bbd585490f7e712b0925f7e8b6d4c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 19:57:41 -0300 Subject: [PATCH 50/88] corrige actuate --- ALP-Winners/src/control/__init__.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index cf18828..4342d38 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -13,13 +13,11 @@ def __init__(self, world): def output(self, robot): pass - def actuate(self, robot, increment_control): + def actuate(self, robot): if not robot.on: return (0, 0) - # v, w = self.output(robot) - v = 0 - w = increment_control + v, w = self.output(robot) robot.lastControlLinVel = v vr, vl = speeds2motors(v, w) From cdaf7449fbe3450ba9fbbf9622d95924faa395b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 21:57:03 -0300 Subject: [PATCH 51/88] corrige nome --- ALP-Winners/src/control/UFC.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index abe99ff..86405ae 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -75,7 +75,7 @@ def output(self, robot): v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref v = min(v1, v2, v3) - if self.world.enableManualControl: + if self.world.checkBatteries: v = self.world.manualControlSpeedV # w = self.world.manualControlSpeedW From fc6ee4d228ab19ee7bcc843a26067d49d1f49207 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 22:18:51 -0300 Subject: [PATCH 52/88] muda controle para comentado do ALP --- ALP-Winners/src/control/UFC.py | 61 +++++++++++----------------------- 1 file changed, 20 insertions(+), 41 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index 86405ae..2e5ec2d 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -20,29 +20,10 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.kv = 10 self.ki = 10 self.kd = 1 - self.integral = 0 - self.vbias = 0.2 - self.sd_min = 1e-4 - self.sd_max = 0.5 - self.lastth = [0,0,0,0] - self.lastdth = 0 + self.lastth = 0 self.interval = Interval(filter=False, initial_dt=0.016) - self.lastnorm = 0 - self.enableInjection = enableInjection - self.lastwref = 0 - self.lastvref = 0 - self.integrateinjection = 0 - self.loadedInjection = 0 - self.lastPb = np.array([0,0]) - self.vPb = np.array([0,0]) - - self.eth = 0 - - @property - def error(self): - return self.eth def output(self, robot): if robot.field is None: return 0,0 @@ -55,29 +36,33 @@ def output(self, robot): # Tempo desde a última atuação dt = self.interval.getInterval() - # Calcula a integral e satura (descobrir o quanto saturar, tirei esse 64 rad/s) - self.integral = sat(self.integral + eth * dt, 64) + # Derivada da referência + dth = sat(angError(th, self.lastth) / dt, 15) + + # Computa phi + phi = robot.field.phi(robot.pose) - # Calcula a derivada - # self.dth = eth/dt - # print("dth: ", self.dth) + # Computa gamma + gamma = robot.field.gamma(dth, robot.velmod, phi) - # Velocidade angular com controle PID - w = self.kp * eth + self.ki*self.integral# + self.kd*self.dth + # Computa omega + omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) + gamma # Velocidade limite de deslizamento - v1 = self.amax / np.abs(w) + if phi != 0: v1 = (-np.abs(omega) + np.sqrt(omega**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi)) + if phi == 0: v1 = self.amax / np.abs(omega) # Velocidade limite das rodas - v2 = self.vmax - self.L * np.abs(w) / 2 + v2 = (2*self.vmax - self.L * np.abs(omega)) / (2 + self.L * np.abs(phi)) # Velocidade limite de aproximação v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - v = min(v1, v2, v3) - if self.world.checkBatteries: - v = self.world.manualControlSpeedV - # w = self.world.manualControlSpeedW + # Velocidade linear é menor de todas + v = max(min(v1, v2, v3), 0) + + # Lei de controle da velocidade angular + w = v * phi + omega if robot.id == 0: print(f"ref(th): {(th * 180 / np.pi):.0f}º") @@ -88,14 +73,8 @@ def output(self, robot): print(f", w: {robot.w:.2f}") # Atualiza a última referência - self.lastth = self.lastth[1:] + [th] + self.lastth = th robot.lastControlLinVel = v - # Atualiza variáveis de estado - self.eth = eth - self.lastwref = w - self.lastvref = v - if robot.spin == 0: return (v * robot.direction, w) - else: return (0, 60 * robot.spin) - + else: return (0, 60 * robot.spin) \ No newline at end of file From 8f8d049a2c79c3982d3c06e78db9c78a01767b6e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 21 Jul 2023 16:30:42 -0300 Subject: [PATCH 53/88] muda constantes --- ALP-Winners/src/control/UFC.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index 2e5ec2d..15229f3 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -7,7 +7,7 @@ class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" - def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=False): + def __init__(self, world, kw=4, kp=10, mu=0.30, vmax=1.0, L=L, enableInjection=False): Control.__init__(self, world) self.g = 9.8 @@ -21,7 +21,6 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=Fa self.ki = 10 self.kd = 1 - self.lastth = 0 self.interval = Interval(filter=False, initial_dt=0.016) From 96dd9accb2ef15d3037bfd9810d089188d834213 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 21 Jul 2023 18:00:31 -0300 Subject: [PATCH 54/88] implementa entidade de controle --- ALP-Winners/src/strategy/__init__.py | 6 +++ .../src/strategy/entity/controlTest.py | 53 +++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 ALP-Winners/src/strategy/entity/controlTest.py diff --git a/ALP-Winners/src/strategy/__init__.py b/ALP-Winners/src/strategy/__init__.py index c1fdac5..5f4b593 100644 --- a/ALP-Winners/src/strategy/__init__.py +++ b/ALP-Winners/src/strategy/__init__.py @@ -3,6 +3,7 @@ from .entity.goalKeeper import GoalKeeper from .entity.defender import Defender from .entity.midfielder import Midfielder +from .entity.controlTest import ControlTester #from client.protobuf.vssref_common_pb2 import Foul from client.referee import RefereeCommands from tools import sats, norml, unit, angl, angError, projectLine, howFrontBall, norm, bestWithHyst @@ -140,6 +141,11 @@ def update(self): self.world.team[1].updateEntity(Attacker) self.world.team[0].updateEntity(GoalKeeper) + elif self.world.checkBatteries: + self.world.team[2].updateEntity(Attacker) + self.world.team[1].updateEntity(Attacker) + self.world.team[0].updateEntity(ControlTester) + else: formation = self.formationDecider() toDecide = self.availableRobotIndexes() diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py new file mode 100644 index 0000000..773cfab --- /dev/null +++ b/ALP-Winners/src/strategy/entity/controlTest.py @@ -0,0 +1,53 @@ +from ..entity import Entity +from strategy.field.UVF import UVF +from strategy.field.DirectionalField import DirectionalField +from strategy.field.goalKeeper import GoalKeeperField +from strategy.field.attractive import AttractiveField +from strategy.movements import goalkeep, spinGoalKeeper +from tools import angError, howFrontBall, howPerpBall, ang, norml, norm, angl +from tools.interval import Interval +from control.goalKeeper import GoalKeeperControl +from control.UFC import UFC_Simple +import numpy as np +import math +import time + +class ControlTester(Entity): + def __init__(self, world, robot, side=1): + super().__init__(world, robot) + + self._control = UFC_Simple(self.world) + self.lastChat = 0 + + @property + def control(self): + return self._control + + def equalsTo(self, otherTester): + return True + + def onExit(self): + pass + + def directionDecider(self): + if self.robot.field is not None: + ref_th = self.robot.field.F(self.robot.pose) + rob_th = self.robot.th + + if abs(angError(ref_th, rob_th)) > 90 * np.pi / 180: + self.robot.direction *= -1 + self.lastChat = time.time() + + # Inverter a direção se o robô ficar preso em algo + elif not self.robot.isAlive() and self.robot.spin == 0: + self.lastChat = time.time() + self.robot.direction *= -1 + + def fieldDecider(self): + rr = np.array(self.robot.pos) + + if rr[0] > 0.4: + self.robot.field = DirectionalField(np.pi) + if rr[0] < -0.4: + self.robot.field = DirectionalField(0) + From 34f53c3d7766aa2e29be897658a0452426a70b8c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 10:20:27 -0300 Subject: [PATCH 55/88] =?UTF-8?q?Fix:=20Comentando=20a=20estrat=C3=A9gia?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MainSystem/controller/states/debugHLC.py | 38 ++++++++++++------------ 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/MainSystem/controller/states/debugHLC.py b/MainSystem/controller/states/debugHLC.py index 01007cd..7e14be5 100644 --- a/MainSystem/controller/states/debugHLC.py +++ b/MainSystem/controller/states/debugHLC.py @@ -169,45 +169,45 @@ def update(self): self._controller.visionSystem.update() # Condições para rodar a estratégia - if self.runStrategyCondition(): - self.strategy.run() + # if self.runStrategyCondition(): + # self.strategy.run() # Define um controle - self.robots[0].controlSystem = self.HLCs.get() - self.robots[1].controlSystem = self.HLCs.get() - self.robots[2].controlSystem = self.HLCs.get() + # self.robots[0].controlSystem = self.HLCs.get() + # self.robots[1].controlSystem = self.HLCs.get() + # self.robots[2].controlSystem = self.HLCs.get() # Controle manual if self.getParam("enableManualControl"): self.world.checkBatteries = True self.world.manualControlSpeedV = self.getParam("manualControlSpeedV") self.world.manualControlSpeedW = self.getParam("manualControlSpeedW") - manualSpeed = SpeedPair(self.getParam("manualControlSpeedV"), self.getParam("manualControlSpeedW")) - speeds = [manualSpeed, manualSpeed, manualSpeed] # Controle de alto nível else: self.world.checkBatteries = False - speeds = [r.actuate() for r in self.robots] + + manualSpeed = SpeedPair(self.getParam("manualControlSpeedV"), self.getParam("manualControlSpeedW")) + speeds = [manualSpeed, manualSpeed, manualSpeed] # Obtém o target instantâneo - reference = self.robots[0].field.F(self.robots[0].pose) + # reference = self.robots[0].field.F(self.robots[0].pose) # Adiciona dados de debug para gráficos e para salvar - self.appendDebugData(reference, speeds, dt) + self.appendDebugData(0, speeds, dt) - if self.world.running: - # Envia mensagem ao robô - if self.getParam("runVision"): self._controller.communicationSystems.get().send(speeds) + # if self.world.running: + # # Envia mensagem ao robô + # if self.getParam("runVision"): self._controller.communicationSystems.get().send(speeds) - # Simula nova posição - else: - for robot, speed in zip(self.robots, speeds): - simulate(robot, speed.v, -speed.w, dt=dt) - #simulateBall(self.world.ball) + # # Simula nova posição + # else: + # for robot, speed in zip(self.robots, speeds): + # simulate(robot, speed.v, -speed.w, dt=dt) + # simulateBall(self.world.ball) # Envia zero para os robôs - else: self._controller.communicationSystems.get().sendZero() + # else: self._controller.communicationSystems.get().sendZero() # Garante que o tempo de loop é de no mínimo 16ms time.sleep(max(0.011-(time.time()-self.t), 0)) From 44957638e370b32e3882775d3a6569d2e83532c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 11:11:43 -0300 Subject: [PATCH 56/88] =?UTF-8?q?ajusta=20constantes=20padr=C3=A3o=20e=20c?= =?UTF-8?q?omenta=20envio=20de=20v=20e=20w?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/control/UFC.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index 86405ae..4718720 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -7,7 +7,7 @@ class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" - def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.5, L=L, enableInjection=False): + def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.0, L=L, enableInjection=False): Control.__init__(self, world) self.g = 9.8 @@ -75,8 +75,8 @@ def output(self, robot): v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref v = min(v1, v2, v3) - if self.world.checkBatteries: - v = self.world.manualControlSpeedV + # if self.world.checkBatteries: + # v = self.world.manualControlSpeedV # w = self.world.manualControlSpeedW if robot.id == 0: From 2d649eeb1811b96c440f2c81ea1768eefbe59dff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 21 Jul 2023 18:00:31 -0300 Subject: [PATCH 57/88] implementa entidade de controle --- ALP-Winners/src/strategy/__init__.py | 6 +++ .../src/strategy/entity/controlTest.py | 53 +++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 ALP-Winners/src/strategy/entity/controlTest.py diff --git a/ALP-Winners/src/strategy/__init__.py b/ALP-Winners/src/strategy/__init__.py index c1fdac5..5f4b593 100644 --- a/ALP-Winners/src/strategy/__init__.py +++ b/ALP-Winners/src/strategy/__init__.py @@ -3,6 +3,7 @@ from .entity.goalKeeper import GoalKeeper from .entity.defender import Defender from .entity.midfielder import Midfielder +from .entity.controlTest import ControlTester #from client.protobuf.vssref_common_pb2 import Foul from client.referee import RefereeCommands from tools import sats, norml, unit, angl, angError, projectLine, howFrontBall, norm, bestWithHyst @@ -140,6 +141,11 @@ def update(self): self.world.team[1].updateEntity(Attacker) self.world.team[0].updateEntity(GoalKeeper) + elif self.world.checkBatteries: + self.world.team[2].updateEntity(Attacker) + self.world.team[1].updateEntity(Attacker) + self.world.team[0].updateEntity(ControlTester) + else: formation = self.formationDecider() toDecide = self.availableRobotIndexes() diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py new file mode 100644 index 0000000..773cfab --- /dev/null +++ b/ALP-Winners/src/strategy/entity/controlTest.py @@ -0,0 +1,53 @@ +from ..entity import Entity +from strategy.field.UVF import UVF +from strategy.field.DirectionalField import DirectionalField +from strategy.field.goalKeeper import GoalKeeperField +from strategy.field.attractive import AttractiveField +from strategy.movements import goalkeep, spinGoalKeeper +from tools import angError, howFrontBall, howPerpBall, ang, norml, norm, angl +from tools.interval import Interval +from control.goalKeeper import GoalKeeperControl +from control.UFC import UFC_Simple +import numpy as np +import math +import time + +class ControlTester(Entity): + def __init__(self, world, robot, side=1): + super().__init__(world, robot) + + self._control = UFC_Simple(self.world) + self.lastChat = 0 + + @property + def control(self): + return self._control + + def equalsTo(self, otherTester): + return True + + def onExit(self): + pass + + def directionDecider(self): + if self.robot.field is not None: + ref_th = self.robot.field.F(self.robot.pose) + rob_th = self.robot.th + + if abs(angError(ref_th, rob_th)) > 90 * np.pi / 180: + self.robot.direction *= -1 + self.lastChat = time.time() + + # Inverter a direção se o robô ficar preso em algo + elif not self.robot.isAlive() and self.robot.spin == 0: + self.lastChat = time.time() + self.robot.direction *= -1 + + def fieldDecider(self): + rr = np.array(self.robot.pos) + + if rr[0] > 0.4: + self.robot.field = DirectionalField(np.pi) + if rr[0] < -0.4: + self.robot.field = DirectionalField(0) + From 6810c1eaf504581a52a4658831e831d5e175434c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 10:20:27 -0300 Subject: [PATCH 58/88] =?UTF-8?q?Fix:=20Comentando=20a=20estrat=C3=A9gia?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MainSystem/controller/states/debugHLC.py | 38 ++++++++++++------------ 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/MainSystem/controller/states/debugHLC.py b/MainSystem/controller/states/debugHLC.py index 01007cd..7e14be5 100644 --- a/MainSystem/controller/states/debugHLC.py +++ b/MainSystem/controller/states/debugHLC.py @@ -169,45 +169,45 @@ def update(self): self._controller.visionSystem.update() # Condições para rodar a estratégia - if self.runStrategyCondition(): - self.strategy.run() + # if self.runStrategyCondition(): + # self.strategy.run() # Define um controle - self.robots[0].controlSystem = self.HLCs.get() - self.robots[1].controlSystem = self.HLCs.get() - self.robots[2].controlSystem = self.HLCs.get() + # self.robots[0].controlSystem = self.HLCs.get() + # self.robots[1].controlSystem = self.HLCs.get() + # self.robots[2].controlSystem = self.HLCs.get() # Controle manual if self.getParam("enableManualControl"): self.world.checkBatteries = True self.world.manualControlSpeedV = self.getParam("manualControlSpeedV") self.world.manualControlSpeedW = self.getParam("manualControlSpeedW") - manualSpeed = SpeedPair(self.getParam("manualControlSpeedV"), self.getParam("manualControlSpeedW")) - speeds = [manualSpeed, manualSpeed, manualSpeed] # Controle de alto nível else: self.world.checkBatteries = False - speeds = [r.actuate() for r in self.robots] + + manualSpeed = SpeedPair(self.getParam("manualControlSpeedV"), self.getParam("manualControlSpeedW")) + speeds = [manualSpeed, manualSpeed, manualSpeed] # Obtém o target instantâneo - reference = self.robots[0].field.F(self.robots[0].pose) + # reference = self.robots[0].field.F(self.robots[0].pose) # Adiciona dados de debug para gráficos e para salvar - self.appendDebugData(reference, speeds, dt) + self.appendDebugData(0, speeds, dt) - if self.world.running: - # Envia mensagem ao robô - if self.getParam("runVision"): self._controller.communicationSystems.get().send(speeds) + # if self.world.running: + # # Envia mensagem ao robô + # if self.getParam("runVision"): self._controller.communicationSystems.get().send(speeds) - # Simula nova posição - else: - for robot, speed in zip(self.robots, speeds): - simulate(robot, speed.v, -speed.w, dt=dt) - #simulateBall(self.world.ball) + # # Simula nova posição + # else: + # for robot, speed in zip(self.robots, speeds): + # simulate(robot, speed.v, -speed.w, dt=dt) + # simulateBall(self.world.ball) # Envia zero para os robôs - else: self._controller.communicationSystems.get().sendZero() + # else: self._controller.communicationSystems.get().sendZero() # Garante que o tempo de loop é de no mínimo 16ms time.sleep(max(0.011-(time.time()-self.t), 0)) From b81b2125e181f168a2ea2772d1aa88af48eec4e9 Mon Sep 17 00:00:00 2001 From: ayssag Date: Sat, 22 Jul 2023 11:31:51 -0500 Subject: [PATCH 59/88] Update mensagem MS-ALP --- MainSystem/controller/vision/__init__.py | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 38c1b7c..1e3d4ac 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -60,16 +60,26 @@ def update(self): "th": self._world.robots[i].raw_th, "vel_x": self._world.robots[i].inst_vx, "vel_y": self._world.robots[i].inst_vy, - "w": self._world.robots[i].inst_w + "w": self._world.robots[i].inst_w, + "control_params":{ + "kw": self._world.robots[i].controlSystem.getParam("kw"), + "kp": self._world.robots[i].controlSystem.getParam("kp"), + "L": self._world.robots[i].controlSystem.getParam("L"), + "amax": 0.12*self._world.robots[i].controlSystem.g, + "vmax": self._world.robots[i].controlSystem.getParam("vmax"), + "motorangaccelmax": self._world.robots[i].controlSystem.getParam("motorangaccelmax"), + "r": self._world.robots[i].controlSystem.getParam("r"), + "maxangerror": self._world.robots[i].controlSystem.getParam("maxangerror"), + "tau": self._world.robots[i].controlSystem.getParam("tau") + } } for i in range(self._world.n_robots) }, "running": self._world.running, - "check_batteries": self._world.checkBatteries + "check_batteries": self._world.checkBatteries, + "manualControlSpeedV": self._world.manualControlSpeedV, + "manualControlSpeedW": self._world.manualControlSpeedW } - # TODO: acrescentar: - # message.append(self._world.manualControlSpeedV) - # message.append(self._world.manualControlSpeedW) self.server_pickle.send(message) From 7d6ca07206285581c6882e3047db5835767ac584 Mon Sep 17 00:00:00 2001 From: ayssag Date: Sat, 22 Jul 2023 11:34:19 -0500 Subject: [PATCH 60/88] Revert "Update mensagem MS-ALP" This reverts commit ee86179ab12101f363d42dade78dc6a94bb56ceb. --- MainSystem/controller/vision/__init__.py | 20 +++++--------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 1e3d4ac..38c1b7c 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -60,26 +60,16 @@ def update(self): "th": self._world.robots[i].raw_th, "vel_x": self._world.robots[i].inst_vx, "vel_y": self._world.robots[i].inst_vy, - "w": self._world.robots[i].inst_w, - "control_params":{ - "kw": self._world.robots[i].controlSystem.getParam("kw"), - "kp": self._world.robots[i].controlSystem.getParam("kp"), - "L": self._world.robots[i].controlSystem.getParam("L"), - "amax": 0.12*self._world.robots[i].controlSystem.g, - "vmax": self._world.robots[i].controlSystem.getParam("vmax"), - "motorangaccelmax": self._world.robots[i].controlSystem.getParam("motorangaccelmax"), - "r": self._world.robots[i].controlSystem.getParam("r"), - "maxangerror": self._world.robots[i].controlSystem.getParam("maxangerror"), - "tau": self._world.robots[i].controlSystem.getParam("tau") - } + "w": self._world.robots[i].inst_w } for i in range(self._world.n_robots) }, "running": self._world.running, - "check_batteries": self._world.checkBatteries, - "manualControlSpeedV": self._world.manualControlSpeedV, - "manualControlSpeedW": self._world.manualControlSpeedW + "check_batteries": self._world.checkBatteries } + # TODO: acrescentar: + # message.append(self._world.manualControlSpeedV) + # message.append(self._world.manualControlSpeedW) self.server_pickle.send(message) From 297d76f45a9419c447532955d6446708ddc19407 Mon Sep 17 00:00:00 2001 From: ayssag Date: Sat, 22 Jul 2023 11:37:25 -0500 Subject: [PATCH 61/88] Update mensagem MS-ALP --- MainSystem/controller/vision/__init__.py | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index 38c1b7c..ee7fd4b 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -60,16 +60,27 @@ def update(self): "th": self._world.robots[i].raw_th, "vel_x": self._world.robots[i].inst_vx, "vel_y": self._world.robots[i].inst_vy, - "w": self._world.robots[i].inst_w + "w": self._world.robots[i].inst_w, + "control_params":{ + "kw": self._world.robots[i].controlSystem.getParam("kw"), + "kp": self._world.robots[i].controlSystem.getParam("kp"), + "L": self._world.robots[i].controlSystem.getParam("L"), + "amax": 0.12*self._world.robots[i].controlSystem.g, + "vmax": self._world.robots[i].controlSystem.getParam("vmax"), + "motorangaccelmax": self._world.robots[i].controlSystem.getParam("motorangaccelmax"), + "r": self._world.robots[i].controlSystem.getParam("r"), + "maxangerror": self._world.robots[i].controlSystem.getParam("maxangerror"), + "tau": self._world.robots[i].controlSystem.getParam("tau") + } } for i in range(self._world.n_robots) }, "running": self._world.running, - "check_batteries": self._world.checkBatteries + "check_batteries": self._world.checkBatteries, + "manualControlSpeedV": self._world.manualControlSpeedV, + "manualControlSpeedW": self._world.manualControlSpeedW } - # TODO: acrescentar: - # message.append(self._world.manualControlSpeedV) - # message.append(self._world.manualControlSpeedW) + self.server_pickle.send(message) From 86822808ec16496146bb53e8677d2eb95c047b28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 16:44:26 -0300 Subject: [PATCH 62/88] =?UTF-8?q?corrige=20satura=C3=A7=C3=A3o?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/control/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index 4342d38..e238404 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -22,8 +22,8 @@ def actuate(self, robot): vr, vl = speeds2motors(v, w) - vr = int(deadzone(sat(vr, 255), 32, -32)) - vl = int(deadzone(sat(vl, 255), 32, -32)) + vr = int(deadzone(sat(vr, 223), 32, -32)) + vl = int(deadzone(sat(vl, 223), 32, -32)) # print( # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") From 5703fca376fb068b6f60e37215c20495ad8d74d1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 16:44:49 -0300 Subject: [PATCH 63/88] novas constantes melhores --- ALP-Winners/src/control/UFC.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index 15229f3..39cbcbf 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -7,7 +7,7 @@ class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" - def __init__(self, world, kw=4, kp=10, mu=0.30, vmax=1.0, L=L, enableInjection=False): + def __init__(self, world, kw=6, kp=10, mu=0.7, vmax=1.0, L=L, enableInjection=False): Control.__init__(self, world) self.g = 9.8 @@ -17,15 +17,16 @@ def __init__(self, world, kw=4, kp=10, mu=0.30, vmax=1.0, L=L, enableInjection=F self.amax = self.mu * self.g self.vmax = vmax self.L = L - self.kv = 10 - self.ki = 10 - self.kd = 1 self.lastth = 0 self.interval = Interval(filter=False, initial_dt=0.016) def output(self, robot): if robot.field is None: return 0,0 + + # self.vmax = 2*self.world.manualControlSpeedV + # self.kw = 2*self.world.manualControlSpeedW + # Ângulo de referência th = robot.field.F(robot.pose) @@ -58,12 +59,14 @@ def output(self, robot): v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref # Velocidade linear é menor de todas + vels = np.array([v1,v2,v3]) v = max(min(v1, v2, v3), 0) # Lei de controle da velocidade angular w = v * phi + omega if robot.id == 0: + print("v escolhido: v",(np.argmin(vels)+1)) print(f"ref(th): {(th * 180 / np.pi):.0f}º") print(f"erro(th): {(eth * 180 / np.pi):.0f}º") print(f"vref: {v:.2f}", end='') From eb0eade0a56976913a059dfe8356a32f940da337 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 16:45:15 -0300 Subject: [PATCH 64/88] insere Pb e aumenta amplitude do teste --- ALP-Winners/src/strategy/entity/controlTest.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py index 773cfab..f3af9f6 100644 --- a/ALP-Winners/src/strategy/entity/controlTest.py +++ b/ALP-Winners/src/strategy/entity/controlTest.py @@ -46,8 +46,8 @@ def directionDecider(self): def fieldDecider(self): rr = np.array(self.robot.pos) - if rr[0] > 0.4: - self.robot.field = DirectionalField(np.pi) - if rr[0] < -0.4: - self.robot.field = DirectionalField(0) + if rr[0] > 0.65: + self.robot.field = DirectionalField(np.pi, Pb=([-0.65,rr[1],np.pi])) + if rr[0] < -0.65: + self.robot.field = DirectionalField(0, Pb=([0.65,rr[1],0])) From 5e4962d002f20aeadeb1047cd8770ed8fd6469cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Thu, 20 Jul 2023 22:18:51 -0300 Subject: [PATCH 65/88] muda controle para comentado do ALP --- ALP-Winners/src/control/UFC.py | 61 +++++++++++----------------------- 1 file changed, 20 insertions(+), 41 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index 4718720..ac138d4 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -20,29 +20,10 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.0, L=L, enableInjection=Fa self.kv = 10 self.ki = 10 self.kd = 1 - self.integral = 0 - self.vbias = 0.2 - self.sd_min = 1e-4 - self.sd_max = 0.5 - self.lastth = [0,0,0,0] - self.lastdth = 0 + self.lastth = 0 self.interval = Interval(filter=False, initial_dt=0.016) - self.lastnorm = 0 - self.enableInjection = enableInjection - self.lastwref = 0 - self.lastvref = 0 - self.integrateinjection = 0 - self.loadedInjection = 0 - self.lastPb = np.array([0,0]) - self.vPb = np.array([0,0]) - - self.eth = 0 - - @property - def error(self): - return self.eth def output(self, robot): if robot.field is None: return 0,0 @@ -55,29 +36,33 @@ def output(self, robot): # Tempo desde a última atuação dt = self.interval.getInterval() - # Calcula a integral e satura (descobrir o quanto saturar, tirei esse 64 rad/s) - self.integral = sat(self.integral + eth * dt, 64) + # Derivada da referência + dth = sat(angError(th, self.lastth) / dt, 15) + + # Computa phi + phi = robot.field.phi(robot.pose) - # Calcula a derivada - # self.dth = eth/dt - # print("dth: ", self.dth) + # Computa gamma + gamma = robot.field.gamma(dth, robot.velmod, phi) - # Velocidade angular com controle PID - w = self.kp * eth + self.ki*self.integral# + self.kd*self.dth + # Computa omega + omega = self.kw * np.sign(eth) * np.sqrt(np.abs(eth)) + gamma # Velocidade limite de deslizamento - v1 = self.amax / np.abs(w) + if phi != 0: v1 = (-np.abs(omega) + np.sqrt(omega**2 + 4 * np.abs(phi) * self.amax)) / (2*np.abs(phi)) + if phi == 0: v1 = self.amax / np.abs(omega) # Velocidade limite das rodas - v2 = self.vmax - self.L * np.abs(w) / 2 + v2 = (2*self.vmax - self.L * np.abs(omega)) / (2 + self.L * np.abs(phi)) # Velocidade limite de aproximação v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref - v = min(v1, v2, v3) - # if self.world.checkBatteries: - # v = self.world.manualControlSpeedV - # w = self.world.manualControlSpeedW + # Velocidade linear é menor de todas + v = max(min(v1, v2, v3), 0) + + # Lei de controle da velocidade angular + w = v * phi + omega if robot.id == 0: print(f"ref(th): {(th * 180 / np.pi):.0f}º") @@ -88,14 +73,8 @@ def output(self, robot): print(f", w: {robot.w:.2f}") # Atualiza a última referência - self.lastth = self.lastth[1:] + [th] + self.lastth = th robot.lastControlLinVel = v - # Atualiza variáveis de estado - self.eth = eth - self.lastwref = w - self.lastvref = v - if robot.spin == 0: return (v * robot.direction, w) - else: return (0, 60 * robot.spin) - + else: return (0, 60 * robot.spin) \ No newline at end of file From a69cca8103b0d4c4c6247846f0d9d3e54814a772 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 21 Jul 2023 16:30:42 -0300 Subject: [PATCH 66/88] muda constantes --- ALP-Winners/src/control/UFC.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index ac138d4..15229f3 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -7,7 +7,7 @@ class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" - def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.0, L=L, enableInjection=False): + def __init__(self, world, kw=4, kp=10, mu=0.30, vmax=1.0, L=L, enableInjection=False): Control.__init__(self, world) self.g = 9.8 @@ -21,7 +21,6 @@ def __init__(self, world, kw=4, kp=20, mu=0.3, vmax=1.0, L=L, enableInjection=Fa self.ki = 10 self.kd = 1 - self.lastth = 0 self.interval = Interval(filter=False, initial_dt=0.016) From a658fa519a5176724207a19c263131a4c591ef08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 16:44:26 -0300 Subject: [PATCH 67/88] =?UTF-8?q?corrige=20satura=C3=A7=C3=A3o?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/control/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index 4342d38..e238404 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -22,8 +22,8 @@ def actuate(self, robot): vr, vl = speeds2motors(v, w) - vr = int(deadzone(sat(vr, 255), 32, -32)) - vl = int(deadzone(sat(vl, 255), 32, -32)) + vr = int(deadzone(sat(vr, 223), 32, -32)) + vl = int(deadzone(sat(vl, 223), 32, -32)) # print( # f"ur: {ur}\ter: {vr - vision_vr}, vr: {vr}, vision_vr: {vision_vr}") From 8feedb14a346080f864d735c8bde39f16a4c6d23 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 16:44:49 -0300 Subject: [PATCH 68/88] novas constantes melhores --- ALP-Winners/src/control/UFC.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index 15229f3..39cbcbf 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -7,7 +7,7 @@ class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" - def __init__(self, world, kw=4, kp=10, mu=0.30, vmax=1.0, L=L, enableInjection=False): + def __init__(self, world, kw=6, kp=10, mu=0.7, vmax=1.0, L=L, enableInjection=False): Control.__init__(self, world) self.g = 9.8 @@ -17,15 +17,16 @@ def __init__(self, world, kw=4, kp=10, mu=0.30, vmax=1.0, L=L, enableInjection=F self.amax = self.mu * self.g self.vmax = vmax self.L = L - self.kv = 10 - self.ki = 10 - self.kd = 1 self.lastth = 0 self.interval = Interval(filter=False, initial_dt=0.016) def output(self, robot): if robot.field is None: return 0,0 + + # self.vmax = 2*self.world.manualControlSpeedV + # self.kw = 2*self.world.manualControlSpeedW + # Ângulo de referência th = robot.field.F(robot.pose) @@ -58,12 +59,14 @@ def output(self, robot): v3 = self.kp * norm(robot.pos, robot.field.Pb) ** 2 + robot.vref # Velocidade linear é menor de todas + vels = np.array([v1,v2,v3]) v = max(min(v1, v2, v3), 0) # Lei de controle da velocidade angular w = v * phi + omega if robot.id == 0: + print("v escolhido: v",(np.argmin(vels)+1)) print(f"ref(th): {(th * 180 / np.pi):.0f}º") print(f"erro(th): {(eth * 180 / np.pi):.0f}º") print(f"vref: {v:.2f}", end='') From da5e299183b44497ccbf2f821bfb2698bba7b3c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 16:59:37 -0300 Subject: [PATCH 69/88] insere Pb e aumenta amplitude do teste --- ALP-Winners/src/strategy/entity/controlTest.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/controlTest.py b/ALP-Winners/src/strategy/entity/controlTest.py index 773cfab..f3af9f6 100644 --- a/ALP-Winners/src/strategy/entity/controlTest.py +++ b/ALP-Winners/src/strategy/entity/controlTest.py @@ -46,8 +46,8 @@ def directionDecider(self): def fieldDecider(self): rr = np.array(self.robot.pos) - if rr[0] > 0.4: - self.robot.field = DirectionalField(np.pi) - if rr[0] < -0.4: - self.robot.field = DirectionalField(0) + if rr[0] > 0.65: + self.robot.field = DirectionalField(np.pi, Pb=([-0.65,rr[1],np.pi])) + if rr[0] < -0.65: + self.robot.field = DirectionalField(0, Pb=([0.65,rr[1],0])) From d577d36206f8b25cb27bd766352d1d2383c8091e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 17:24:11 -0300 Subject: [PATCH 70/88] envia velocidades v e wpara alp --- ALP-Winners/src/world/__init__.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ALP-Winners/src/world/__init__.py b/ALP-Winners/src/world/__init__.py index 3db562c..c2878d7 100644 --- a/ALP-Winners/src/world/__init__.py +++ b/ALP-Winners/src/world/__init__.py @@ -94,6 +94,8 @@ def update(self, message): self.ball.update(message["ball"]["pos_x"], message["ball"]["pos_y"], message["ball"]["vel_x"], message["ball"]["vel_y"]) self.checkBatteries = message["check_batteries"] + self.manualControlSpeedV = message["manualControlSpeedV"] + self.manualControlSpeedW = message["manualControlSpeedW"] self.updateCount += 1 From 48f41633877519e23712a2c888d9f841e019a974 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 17:24:37 -0300 Subject: [PATCH 71/88] =?UTF-8?q?manda=20inst=5Fth=20ao=20inv=C3=A9s=20de?= =?UTF-8?q?=20raw=5Ftheta?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MainSystem/controller/vision/__init__.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/MainSystem/controller/vision/__init__.py b/MainSystem/controller/vision/__init__.py index ee7fd4b..562f784 100644 --- a/MainSystem/controller/vision/__init__.py +++ b/MainSystem/controller/vision/__init__.py @@ -57,7 +57,7 @@ def update(self): i: { "pos_x": self._world.robots[i].inst_x, "pos_y": self._world.robots[i].inst_y, - "th": self._world.robots[i].raw_th, + "th": self._world.robots[i].inst_th, "vel_x": self._world.robots[i].inst_vx, "vel_y": self._world.robots[i].inst_vy, "w": self._world.robots[i].inst_w, @@ -81,7 +81,6 @@ def update(self): "manualControlSpeedW": self._world.manualControlSpeedW } - self.server_pickle.send(message) if self.usePastPositions is False: From 7ee1d8a362cffdaa0b3353fc4399d9c7dbd53e22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sat, 22 Jul 2023 17:25:00 -0300 Subject: [PATCH 72/88] =?UTF-8?q?field=20side=20n=C3=A3o=20est=C3=A1=20mai?= =?UTF-8?q?s=20em=20nenhum=20lugar=20no=20c=C3=B3digo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MainSystem/controller/world/element.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/MainSystem/controller/world/element.py b/MainSystem/controller/world/element.py index a56064b..9de7684 100644 --- a/MainSystem/controller/world/element.py +++ b/MainSystem/controller/world/element.py @@ -78,7 +78,7 @@ def __repr__(self): return info def update(self, x=0, y=0, th=0): - self.raw_update(x * self.world.fieldSide, y, th * self.world.fieldSide) + self.raw_update(x, y, th) def raw_update(self, x=0, y=0, th=0): """Atualiza a posição do objeto, atualizando também o valor das posições anteriores.""" @@ -132,7 +132,7 @@ def raw_th(self, th): def setTh(self, th): """Atualiza o ângulo do objeto diretamente (sem afetar o ângulo anterior).""" thVec = unit(th) - self.inst_th = angl((-thVec[0] * self.world.fieldSide, thVec[1])) + self.inst_th = angl((-thVec[0], thVec[1])) @property def vel(self): @@ -145,7 +145,7 @@ def raw_vel(self): @property def vx(self): - return self.inst_vx * self.world.fieldSide + return self.inst_vx @property def vy(self): @@ -154,7 +154,7 @@ def vy(self): @property def acc(self): """Retorna a aceleração do objeto no formato de lista: \\([a_x, a_y]\\)""" - return [self.inst_ax * self.world.fieldSide, self.inst_ay] + return [self.inst_ax, self.inst_ay] @property def raw_acc(self): @@ -173,7 +173,7 @@ def accmod(self): @property def velang(self): """Retorna o ângulo do vetor velocidade do objeto: \\(\\text{arctan2}(v_y, v_x)\\)""" - return np.arctan2(self.inst_vy, self.inst_vx * self.world.fieldSide) + return np.arctan2(self.inst_vy, self.inst_vx) @property def raw_velang(self): @@ -229,7 +229,7 @@ def calc_velocities(self, dt, alpha=0.5, thalpha=0.8, accalpha=0.2): @property def x(self): """Retorna a posição \\(x\\) atual do objeto""" - return self.inst_x * self.world.fieldSide #+ self.timeStep * self.vx * self.world.dt + return self.inst_x #+ self.timeStep * self.vx * self.world.dt @property def raw_x(self): From 8b27a12ac0fdacab88cd33e036fb2d8bae12f63c Mon Sep 17 00:00:00 2001 From: Maria Claudia Date: Wed, 26 Jul 2023 12:04:50 -0300 Subject: [PATCH 73/88] altera busyloop para loop --- ALP-Winners/src/loop.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 49b04f2..d297d62 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -73,9 +73,11 @@ def loop(self): if self.execute: for robot in self.world.raw_team: robot.turnOn() self.radio.send(control_output) + self.busyLoop() else: self.radio.send([(0,0) for robot in self.world.team]) for robot in self.world.raw_team: robot.turnOff() + self.busyLoop() # Desenha no ALP-GUI self.draw() @@ -104,8 +106,11 @@ def run(self): while self.running: # Executa o loop de visão e referee até dar o tempo de executar o resto - self.busyLoop() - while time.time() - t0 < self.loopTime: + # self.busyLoop() + # while time.time() - t0 < self.loopTime: + # self.busyLoop() + + if(self.world.updateCount == 0): self.busyLoop() # Tempo inicial do loop From dabd3333e03d94190d6578e1eb5268df9bccdcd9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Wed, 26 Jul 2023 12:16:58 -0300 Subject: [PATCH 74/88] =?UTF-8?q?corrige=20n=C3=BAmero=20de=20filtro=20pra?= =?UTF-8?q?=20varia=C3=A7=C3=A3o=20de=20angulo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MainSystem/controller/world/__init__.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/MainSystem/controller/world/__init__.py b/MainSystem/controller/world/__init__.py index ddb1288..c85190a 100644 --- a/MainSystem/controller/world/__init__.py +++ b/MainSystem/controller/world/__init__.py @@ -63,10 +63,10 @@ def update(self, visionMessage): if not allyPose[3]: continue # O ângulo do robô não pode variar de um loop para outro mais que 70% de pi/2, se isso ocorrer deve ser algum erro da visão - #if self.robots[i].poseDefined and np.arccos(np.cos(visionMessage.th[i]-self.robots[i].th)) > 0.5*np.pi/2: - # theta = self.robots[i].th - #else: - theta = allyPose[2] + if self.robots[i].poseDefined and np.arccos(np.cos(allyPose[2]-self.robots[i].th)) > 0.49*np.pi/2: + theta = self.robots[i].th + else: + theta = allyPose[2] self.robots[i].raw_update(allyPose[0], allyPose[1], theta) From daf78dfa8637a624e7a2fcb3753cf262667dc26c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Wed, 26 Jul 2023 15:49:07 -0300 Subject: [PATCH 75/88] melhores constantes de controle pro atacante --- ALP-Winners/src/control/UFC.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index 39cbcbf..e8850a3 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -7,7 +7,7 @@ class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" - def __init__(self, world, kw=6, kp=10, mu=0.7, vmax=1.0, L=L, enableInjection=False): + def __init__(self, world, kw=3, kp=10, mu=0.7, vmax=2.0, L=L, enableInjection=False): Control.__init__(self, world) self.g = 9.8 From 7b7f31aabdaa9e4bcad556724e88986b80ad9aa7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Wed, 26 Jul 2023 15:49:18 -0300 Subject: [PATCH 76/88] comenta filtro (tava dando erro) --- MainSystem/controller/world/__init__.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/MainSystem/controller/world/__init__.py b/MainSystem/controller/world/__init__.py index c85190a..3e111bf 100644 --- a/MainSystem/controller/world/__init__.py +++ b/MainSystem/controller/world/__init__.py @@ -63,10 +63,10 @@ def update(self, visionMessage): if not allyPose[3]: continue # O ângulo do robô não pode variar de um loop para outro mais que 70% de pi/2, se isso ocorrer deve ser algum erro da visão - if self.robots[i].poseDefined and np.arccos(np.cos(allyPose[2]-self.robots[i].th)) > 0.49*np.pi/2: - theta = self.robots[i].th - else: - theta = allyPose[2] + # if self.robots[i].poseDefined and np.arccos(np.cos(allyPose[2]-self.robots[i].th)) > 0.49*np.pi/2: + # theta = self.robots[i].th + # else: + theta = allyPose[2] self.robots[i].raw_update(allyPose[0], allyPose[1], theta) From 5f5c71b6e9176c69e3fd2623aecf95d6b76511dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Wed, 26 Jul 2023 19:04:23 -0300 Subject: [PATCH 77/88] Melhora print --- ALP-Winners/src/loop.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index d297d62..950e544 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -58,7 +58,7 @@ def __init__( def loop(self): if self.world.updateCount == self.lastupdatecount: return - print((time.time()-self.t0)*1000) + print("loop ALP:",(time.time()-self.t0)*1000) self.t0 = time.time() From 53969582a6014c759b171b12b5bdef12e995a592 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Wed, 26 Jul 2023 19:04:34 -0300 Subject: [PATCH 78/88] melhora print --- MainSystem/controller/communication/server_pickle.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/communication/server_pickle.py index 9c7fbef..f057e94 100644 --- a/MainSystem/controller/communication/server_pickle.py +++ b/MainSystem/controller/communication/server_pickle.py @@ -18,7 +18,7 @@ def __init__(self, port): def send(self, data): - print(1000*(time.time()-self.t0)) + print("envio msg MS:",1000*(time.time()-self.t0)) self.t0 = time.time() message = pickle.dumps(data,-1) From c5e6d93530e591d4ce21f5f1ae7b6bb345f96199 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 28 Jul 2023 02:31:18 -0300 Subject: [PATCH 79/88] comenta prints --- ALP-Winners/src/control/UFC.py | 16 ++++++++-------- ALP-Winners/src/control/__init__.py | 7 +++++++ ALP-Winners/src/loop.py | 2 +- .../controller/communication/server_pickle.py | 2 +- .../view/vision/mainVision/visaoAltoNivel.py | 2 +- 5 files changed, 18 insertions(+), 11 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index e8850a3..e740ed9 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -65,14 +65,14 @@ def output(self, robot): # Lei de controle da velocidade angular w = v * phi + omega - if robot.id == 0: - print("v escolhido: v",(np.argmin(vels)+1)) - print(f"ref(th): {(th * 180 / np.pi):.0f}º") - print(f"erro(th): {(eth * 180 / np.pi):.0f}º") - print(f"vref: {v:.2f}", end='') - print(f", wref: {w:.2f}") - print(f"v: {robot.velmod:.2f}", end='') - print(f", w: {robot.w:.2f}") + # if robot.id == 0: + # print("v escolhido: v",(np.argmin(vels)+1)) + # print(f"ref(th): {(th * 180 / np.pi):.0f}º") + # print(f"erro(th): {(eth * 180 / np.pi):.0f}º") + # print(f"vref: {v:.2f}", end='') + # print(f", wref: {w:.2f}") + # print(f"v: {robot.velmod:.2f}", end='') + # print(f", w: {robot.w:.2f}") # Atualiza a última referência self.lastth = th diff --git a/ALP-Winners/src/control/__init__.py b/ALP-Winners/src/control/__init__.py index e238404..4e6eb52 100644 --- a/ALP-Winners/src/control/__init__.py +++ b/ALP-Winners/src/control/__init__.py @@ -16,6 +16,13 @@ def output(self, robot): def actuate(self, robot): if not robot.on: return (0, 0) + + # if robot.id == 0: + # return (1,1) + # elif robot.id == 1: + # return (2,2) + # elif robot.id == 2: + # return (3,3) v, w = self.output(robot) robot.lastControlLinVel = v diff --git a/ALP-Winners/src/loop.py b/ALP-Winners/src/loop.py index 950e544..3b09759 100644 --- a/ALP-Winners/src/loop.py +++ b/ALP-Winners/src/loop.py @@ -58,7 +58,7 @@ def __init__( def loop(self): if self.world.updateCount == self.lastupdatecount: return - print("loop ALP:",(time.time()-self.t0)*1000) + # print("loop ALP:",(time.time()-self.t0)*1000) self.t0 = time.time() diff --git a/MainSystem/controller/communication/server_pickle.py b/MainSystem/controller/communication/server_pickle.py index f057e94..4d70eda 100644 --- a/MainSystem/controller/communication/server_pickle.py +++ b/MainSystem/controller/communication/server_pickle.py @@ -18,7 +18,7 @@ def __init__(self, port): def send(self, data): - print("envio msg MS:",1000*(time.time()-self.t0)) + # print("envio msg MS:",1000*(time.time()-self.t0)) self.t0 = time.time() message = pickle.dumps(data,-1) diff --git a/MainSystem/view/vision/mainVision/visaoAltoNivel.py b/MainSystem/view/vision/mainVision/visaoAltoNivel.py index f995d23..9e83400 100644 --- a/MainSystem/view/vision/mainVision/visaoAltoNivel.py +++ b/MainSystem/view/vision/mainVision/visaoAltoNivel.py @@ -38,7 +38,7 @@ def getFrame(self): img_filtered = cv2.bitwise_and(img_warpped, img_warpped, mask=fgMask) t0 = time.time() message = self.__visionSystem.process(frame) - print("loop da visao:", (time.time()-t0)*1000) + # print("loop da visao:", (time.time()-t0)*1000) GLib.idle_add(self.updateRobotsInfo, message) Drawing.draw_field(self.__world, img_filtered) From 053c57293023fcfc3c98b52b5100a71e8c20cb8c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 28 Jul 2023 02:31:36 -0300 Subject: [PATCH 80/88] muda constantes do goleiro --- ALP-Winners/src/control/goalKeeper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ALP-Winners/src/control/goalKeeper.py b/ALP-Winners/src/control/goalKeeper.py index 863714a..6315db5 100644 --- a/ALP-Winners/src/control/goalKeeper.py +++ b/ALP-Winners/src/control/goalKeeper.py @@ -7,7 +7,7 @@ class GoalKeeperControl(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" - def __init__(self, world, kw=5, kp=100, mu=0.07, vmax=0.5, L=0.075): + def __init__(self, world, kw=3, kp=100, mu=0.7, vmax=0.5, L=0.075): Control.__init__(self, world) self.g = 9.8 From 94f6eb29d585f622b49d3e0ea3d5292106493c0f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 28 Jul 2023 11:51:33 -0300 Subject: [PATCH 81/88] muda offset do gol para goleiro --- ALP-Winners/src/strategy/entity/goalKeeper.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/goalKeeper.py b/ALP-Winners/src/strategy/entity/goalKeeper.py index 6f8c385..a042e41 100644 --- a/ALP-Winners/src/strategy/entity/goalKeeper.py +++ b/ALP-Winners/src/strategy/entity/goalKeeper.py @@ -66,7 +66,7 @@ def fieldDeciderMS(self): rb = np.array(self.world.ball.pos) vb = np.array(self.world.ball.v) rg = -np.array(self.world.field.goalPos) - rg[0] += 0.17 + rg[0] += 0.10 # Aplica o movimento self.robot.vref = 0 @@ -92,7 +92,7 @@ def fieldDecider(self): rb = np.array(self.world.ball.pos) vb = np.array(self.world.ball.v) rg = -np.array(self.world.field.goalPos) - rg[0] += 0.17 + rg[0] += 0.15 # Aplica o movimento self.robot.vref = 0 @@ -100,7 +100,7 @@ def fieldDecider(self): self.robot.setSpin(spinGoalKeeper(rb, rr, rg), timeOut = 0.1) Pb = goalkeep(rb, vb, rr, rg) - # print(Pb) + print(Pb) # print('isAlive:' , self.robot.isAlive()) # print(f"angulo: {thr}") if self.state == "Stable": @@ -124,7 +124,7 @@ def fieldDecider(self): # self.robot.field = UVF(Pb, spiral=0.01) # self.robot.field = DirectionalField(Pb[2], Pb=Pb) if np.abs(rr[0]-Pb[0]) < 0.07 else UVF(Pb, spiral=0.01) - # print('estado do goleiro: ' + self.state) + print('estado do goleiro: ' + self.state) if self.state == "Stable": self.robot.field = DirectionalField(Pb[2], Pb=(rr[0], Pb[1], Pb[2])) elif self.state == "Unstable": From 54bd37c8660130f973a1e9c0feb5bf8863ba3370 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 28 Jul 2023 11:51:45 -0300 Subject: [PATCH 82/88] =?UTF-8?q?usa=20entidades=20est=C3=A1ticas=20por=20?= =?UTF-8?q?padr=C3=A3o?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ALP-Winners/src/strategy/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ALP-Winners/src/strategy/__init__.py b/ALP-Winners/src/strategy/__init__.py index 5f4b593..b74de8d 100644 --- a/ALP-Winners/src/strategy/__init__.py +++ b/ALP-Winners/src/strategy/__init__.py @@ -136,7 +136,7 @@ def decideBestMasterAttackerBetweenTwo(self, formation, toDecide): return formation, toDecide def update(self): - if self.static_entities: + if not self.static_entities: self.world.team[2].updateEntity(Attacker) self.world.team[1].updateEntity(Attacker) self.world.team[0].updateEntity(GoalKeeper) From f26e5d176ec1acfc0788bd2234701bf2cf00571b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 28 Jul 2023 18:46:10 -0300 Subject: [PATCH 83/88] atualiza static entities --- ALP-Winners/src/strategy/__init__.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ALP-Winners/src/strategy/__init__.py b/ALP-Winners/src/strategy/__init__.py index b74de8d..8da035c 100644 --- a/ALP-Winners/src/strategy/__init__.py +++ b/ALP-Winners/src/strategy/__init__.py @@ -136,10 +136,10 @@ def decideBestMasterAttackerBetweenTwo(self, formation, toDecide): return formation, toDecide def update(self): - if not self.static_entities: - self.world.team[2].updateEntity(Attacker) + if self.static_entities: + self.world.team[2].updateEntity(GoalKeeper) self.world.team[1].updateEntity(Attacker) - self.world.team[0].updateEntity(GoalKeeper) + self.world.team[0].updateEntity(Attacker) elif self.world.checkBatteries: self.world.team[2].updateEntity(Attacker) From 17ce1aac3026d22fc624a280a2206869e602fa8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 28 Jul 2023 18:46:21 -0300 Subject: [PATCH 84/88] desomenta campo repulsivo do midfielder --- ALP-Winners/src/strategy/entity/attacker.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/attacker.py b/ALP-Winners/src/strategy/entity/attacker.py index afed686..cf54ae3 100644 --- a/ALP-Winners/src/strategy/entity/attacker.py +++ b/ALP-Winners/src/strategy/entity/attacker.py @@ -196,14 +196,15 @@ def fieldDecider(self): # if np.abs(ang(unit(angl(robot.pos-rr)), unit(self.robot.th))) < 30 * np.pi / 180: # self.robot.field = AvoidanceField(self.robot.field, AvoidCircle(robot.pos, 0.08), borderSize=0.20) - # if self.slave and self.attackState == 0: - # print("I {0} am slave".format(self.robot.id)) - # for robot in [r for r in otherAllies if r.entity.__class__.__name__ == "Attacker"]: - # self.robot.field = AvoidanceField(self.robot.field, AvoidCircle(robot.pos, 0.08), borderSize=0.20) + # Campo repulsivo pro midfielder não atrapalhar o atacante + if self.slave and self.attackState == 0: + print("I {0} am slave".format(self.robot.id)) + for robot in [r for r in otherAllies if r.entity.__class__.__name__ == "Attacker"]: + self.robot.field = AvoidanceField(self.robot.field, AvoidCircle(robot.pos, 0.08), borderSize=0.20) # Campo para evitar outro robô, (só se não estiver alinhado) - #if self.attackState == 0: + # if self.attackState == 0: # for robot in otherAllies + enemies: # self.robot.field = AvoidanceField(self.robot.field, AvoidCircle(robot.pos, 0.08), borderSize=0.20) From 8bcaf7dd1869dd7e67fdb360367ea47e72359307 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Fri, 28 Jul 2023 18:46:40 -0300 Subject: [PATCH 85/88] aplica UFC_Simple pra defender --- ALP-Winners/src/strategy/entity/defender.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ALP-Winners/src/strategy/entity/defender.py b/ALP-Winners/src/strategy/entity/defender.py index 784390b..8e81e6f 100644 --- a/ALP-Winners/src/strategy/entity/defender.py +++ b/ALP-Winners/src/strategy/entity/defender.py @@ -8,6 +8,7 @@ from tools.interval import Interval from control.UFC import UFC_Simple from control.defender import DefenderControl +from control.UFC import UFC_Simple from client.gui import clientProvider import numpy as np import math @@ -16,7 +17,8 @@ class Defender(Entity): def __init__(self, world, robot, side=1): super().__init__(world, robot) - self._control = DefenderControl(self.world) + # self._control = DefenderControl(self.world) + self._control = UFC_Simple(self.world) @property def control(self): return self._control From 35a427860b929cd4619878221a6893c9f13b9c00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sun, 30 Jul 2023 15:46:23 -0300 Subject: [PATCH 86/88] muda constantes e printa velocidades --- ALP-Winners/src/control/UFC.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ALP-Winners/src/control/UFC.py b/ALP-Winners/src/control/UFC.py index e740ed9..705b70d 100644 --- a/ALP-Winners/src/control/UFC.py +++ b/ALP-Winners/src/control/UFC.py @@ -7,7 +7,7 @@ class UFC_Simple(Control): """Controle unificado para o Univector Field, utiliza o ângulo definido pelo campo como referência \\(\\theta_d\\).""" - def __init__(self, world, kw=3, kp=10, mu=0.7, vmax=2.0, L=L, enableInjection=False): + def __init__(self, world, kw=4, kp=10, mu=0.7, vmax=6.0, L=L, enableInjection=False): Control.__init__(self, world) self.g = 9.8 @@ -65,11 +65,11 @@ def output(self, robot): # Lei de controle da velocidade angular w = v * phi + omega - # if robot.id == 0: - # print("v escolhido: v",(np.argmin(vels)+1)) + if robot.id == 0: + print("v escolhido: v",(np.argmin(vels)+1)) # print(f"ref(th): {(th * 180 / np.pi):.0f}º") # print(f"erro(th): {(eth * 180 / np.pi):.0f}º") - # print(f"vref: {v:.2f}", end='') + print(f"vref: {v:.2f}") # print(f", wref: {w:.2f}") # print(f"v: {robot.velmod:.2f}", end='') # print(f", w: {robot.w:.2f}") From d84e7570f3aed571452db0fb2cea8d59cb08a664 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sun, 30 Jul 2023 15:46:56 -0300 Subject: [PATCH 87/88] defender volta a usar defendercontrol --- ALP-Winners/src/strategy/entity/defender.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/defender.py b/ALP-Winners/src/strategy/entity/defender.py index 8e81e6f..11b054f 100644 --- a/ALP-Winners/src/strategy/entity/defender.py +++ b/ALP-Winners/src/strategy/entity/defender.py @@ -17,8 +17,8 @@ class Defender(Entity): def __init__(self, world, robot, side=1): super().__init__(world, robot) - # self._control = DefenderControl(self.world) - self._control = UFC_Simple(self.world) + self._control = DefenderControl(self.world) + # self._control = UFC_Simple(self.world) @property def control(self): return self._control From 338f7a423221fc3ce43d0987223059953f87009b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Sun, 30 Jul 2023 15:47:33 -0300 Subject: [PATCH 88/88] retira o estado "far" e muda limites --- ALP-Winners/src/strategy/entity/goalKeeper.py | 33 ++++++++++++------- 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/ALP-Winners/src/strategy/entity/goalKeeper.py b/ALP-Winners/src/strategy/entity/goalKeeper.py index a042e41..70e0309 100644 --- a/ALP-Winners/src/strategy/entity/goalKeeper.py +++ b/ALP-Winners/src/strategy/entity/goalKeeper.py @@ -66,18 +66,19 @@ def fieldDeciderMS(self): rb = np.array(self.world.ball.pos) vb = np.array(self.world.ball.v) rg = -np.array(self.world.field.goalPos) - rg[0] += 0.10 + rg[0] += 0.12 # Aplica o movimento self.robot.vref = 0 self.robot.setSpin(spinGoalKeeper(rb, rr, rg), timeOut = 0.1) + + self.setGoalKeeperControl() Pb = goalkeep(rb, vb, rr, rg) if np.abs(rr[0]-rg[0]) > 0.05: Pb = goalkeep(rb, vb, rr, rg) self.robot.field = UVF((rg[0], Pb[1], Pb[2]), radius=0.04) - # rg[0], Pb[1], Pb[2] print("Instável") else: print("Estável") @@ -92,7 +93,7 @@ def fieldDecider(self): rb = np.array(self.world.ball.pos) vb = np.array(self.world.ball.v) rg = -np.array(self.world.field.goalPos) - rg[0] += 0.15 + rg[0] += 0.12 # Aplica o movimento self.robot.vref = 0 @@ -100,11 +101,12 @@ def fieldDecider(self): self.robot.setSpin(spinGoalKeeper(rb, rr, rg), timeOut = 0.1) Pb = goalkeep(rb, vb, rr, rg) - print(Pb) + # print(Pb) # print('isAlive:' , self.robot.isAlive()) # print(f"angulo: {thr}") if self.state == "Stable": # if np.abs(rr[0]-rg[0]) > 0.05 or (np.abs(thr-1.5) > 2 or np.abs(thr-4.7) > 2): + # self.setGoalKeeperControl() if np.abs(rr[0]-rg[0]) > 0.07: self.state = "Unstable" elif self.state == "Unstable": @@ -114,22 +116,29 @@ def fieldDecider(self): # SETAR INSTABILIDADE CASO O ROBO ESTEJA VIRADO E FRENTE PRO GOL # elif (np.abs(rr[0]-rg[0]) < 0.05) and (np.abs(thr-1.5) < 2 or np.abs(thr-4.7) < 2): - elif (np.abs(rr[0]-rg[0]) < 0.05): + elif (np.abs(rr[0]-rg[0]) < 0.04): + # self.setGoalKeeperControl() self.state = "Stable" else: - # if (np.abs(rr[0]-rg[0]) < 0.05) and (np.abs(thr-1.5) < 2 or np.abs(thr-4.7) < 2): - if (np.abs(rr[0]-rg[0]) < 0.05): - self.state = "Stable" - self.setGoalKeeperControl() + # if (np.abs(rr[0]-rg[0]) < 0.05) and (np.abs(thr-1.5) < 2 or np.abs(thr-4.7) < 2): + self.state = "Unstable" + # self.setGoalKeeperControl() + + # if (np.abs(rr[0]-rg[0]) < 0.04): + # self.state = "Stable" + # self.setGoalKeeperControl() + # self.robot.field = UVF(Pb, spiral=0.01) # self.robot.field = DirectionalField(Pb[2], Pb=Pb) if np.abs(rr[0]-Pb[0]) < 0.07 else UVF(Pb, spiral=0.01) - print('estado do goleiro: ' + self.state) + # print('estado do goleiro: ' + self.state) if self.state == "Stable": self.robot.field = DirectionalField(Pb[2], Pb=(rr[0], Pb[1], Pb[2])) elif self.state == "Unstable": - # self.robot.field = UVF(Pb, radius=0.02) - self.robot.field = AttractiveField((rg[0], Pb[1], Pb[2])) + self.robot.field = UVF(Pb, radius=0.02) + # self.robot.field = AttractiveField((Pb[0], rg[1], Pb[2])) + # self.robot.field = UVF((rg[0], rg[1], Pb[2]), radius=0.04) + elif self.state == "Far": self.robot.field = UVF(Pb, radius=0.04) #self.robot.field = DirectionalField(Pb[2], Pb=Pb)