From e179461077f07d9c1ea6d655f89f170de075b568 Mon Sep 17 00:00:00 2001 From: jsugiyama <75608221+17-sugiyama@users.noreply.github.com> Date: Wed, 30 Oct 2024 21:44:44 +0900 Subject: [PATCH 1/4] add fast_brake command for rapid dpindown --- socs/agents/hwp_supervisor/agent.py | 70 +++++++++++++++++++++++++++-- 1 file changed, 67 insertions(+), 3 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 3957b1262..a207200b7 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -657,6 +657,26 @@ class WaitForBrake: min_freq: float prev_freq: float = None + @dataclass + class FastBrake: + """ + Configure the PID and PCU agents to more actively brake the HWP + """ + freq_tol: float + freq_tol_duration: float + brake_voltage: float + + @dataclass + class WaitForFastBrake: + """ + Waits until the HWP has slowed down enough before shutting off PMX. + + min_freq : float + Frequency (Hz) below which the PMX should be shut off. + """ + min_freq: float + prev_freq: float = None + @dataclass class PmxOff: """ @@ -1143,6 +1163,42 @@ def check_acu_ok_for_spinup(): freq_tol_duration=30, )) return + elif isinstance(state, ControlState.FastBrake): + # Flip PID direciton and tune stop + pid_dir = int(query_pid_state()['direction']) + if pid_dir == 1: + self.run_and_validate( + clients.pcu.send_command, + kwargs={'command': 'on_2'}, timeout=None + ) + else: + self.run_and_validate( + clients.pcu.send_command, + kwargs={'command': 'on_1'}, timeout=None + ) + self.run_and_validate(clients.pmx.ign_ext) + self.run_and_validate(clients.pmx.set_v, kwargs={'volt': state.brake_voltage}) + self.run_and_validate(clients.pmx.set_on) + + time.sleep(10) + self.action.set_state(ControlState.WaitForFastBrake( + min_freq=0.2, + prev_freq=hwp_state.enc_freq, + )) + + elif isinstance(state, ControlState.WaitForFastBrake): + f0 = query_pid_state()['current_freq'] + time.sleep(5) + f1 = query_pid_state()['current_freq'] + if f0 < 0.2 or (f1 > f0): + self.log.info("Turning off PMX and putting PCU in stop mode") + self.run_and_validate(clients.pmx.set_off) + self.run_and_validate( + clients.pcu.send_command, + kwargs={'command': 'stop'}, timeout=None + ) + time.sleep(5) + return elif isinstance(state, ControlState.EnableDriverBoard): def set_outlet_state(outlet: int, outlet_state: bool): @@ -1549,6 +1605,7 @@ def set_const_voltage(self, session, params): @ocs_agent.param('freq_tol', type=float, default=0.05) @ocs_agent.param('freq_tol_duration', type=float, default=10) @ocs_agent.param('brake_voltage', type=float, default=10.) + @ocs_agent.param('fast', type=bool, default=False) def brake(self, session, params): """brake(freq_thresh=0.05, freq_thresh_duration=10, brake_voltage=10) @@ -1578,11 +1635,18 @@ def brake(self, session, params): 'success': True} } """ - state = ControlState.Brake( + if params['fast']: + state = ControlState.FastBrake( freq_tol=params['freq_tol'], freq_tol_duration=params['freq_tol_duration'], - brake_voltage=params['brake_voltage'], - ) + brake_voltage=30., + ) + else: + state = ControlState.Brake( + freq_tol=params['freq_tol'], + freq_tol_duration=params['freq_tol_duration'], + brake_voltage=params['brake_voltage'], + ) action = self.control_state_machine.request_new_action(state) action.sleep_until_complete(session=session) return action.success, f"Completed with state: {action.cur_state_info.state}" From 3eef07ca7a144f04d9de4e6f531c389a3e39194f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 30 Oct 2024 13:06:14 +0000 Subject: [PATCH 2/4] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- socs/agents/hwp_supervisor/agent.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index a207200b7..9f0fafac3 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -1637,16 +1637,16 @@ def brake(self, session, params): """ if params['fast']: state = ControlState.FastBrake( - freq_tol=params['freq_tol'], - freq_tol_duration=params['freq_tol_duration'], - brake_voltage=30., + freq_tol=params['freq_tol'], + freq_tol_duration=params['freq_tol_duration'], + brake_voltage=30., ) else: state = ControlState.Brake( freq_tol=params['freq_tol'], freq_tol_duration=params['freq_tol_duration'], brake_voltage=params['brake_voltage'], - ) + ) action = self.control_state_machine.request_new_action(state) action.sleep_until_complete(session=session) return action.success, f"Completed with state: {action.cur_state_info.state}" From ff039485ee294f82614864af155299434ffbdb3f Mon Sep 17 00:00:00 2001 From: Junna Sugiyama Date: Thu, 19 Dec 2024 03:08:09 +0000 Subject: [PATCH 3/4] merged 'fast_brake' and 'brake' --- socs/agents/hwp_supervisor/agent.py | 121 +++++++++------------------- 1 file changed, 36 insertions(+), 85 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 9f0fafac3..8ed66b81f 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -645,6 +645,7 @@ class Brake: freq_tol: float freq_tol_duration: float brake_voltage: float + fast: bool = False @dataclass class WaitForBrake: @@ -656,26 +657,7 @@ class WaitForBrake: """ min_freq: float prev_freq: float = None - - @dataclass - class FastBrake: - """ - Configure the PID and PCU agents to more actively brake the HWP - """ - freq_tol: float - freq_tol_duration: float - brake_voltage: float - - @dataclass - class WaitForFastBrake: - """ - Waits until the HWP has slowed down enough before shutting off PMX. - - min_freq : float - Frequency (Hz) below which the PMX should be shut off. - """ - min_freq: float - prev_freq: float = None + fast: bool = False @dataclass class PmxOff: @@ -1124,81 +1106,56 @@ def check_acu_ok_for_spinup(): self.action.set_state(ControlState.Done(success=True)) elif isinstance(state, ControlState.Brake): + pid_dir = int(query_pid_state()['direction']) + if state.fast: + # set pcu fase brake state + new_pcu_state = 'on_2' if (pid_dir == 1) else 'on_1' + min_freq = 0.2 + else: + new_pcu_state = 'off' + min_freq = 0.5 self.run_and_validate( clients.pcu.send_command, - kwargs={'command': 'off'}, timeout=None + kwargs={'command': new_pcu_state}, timeout=None ) - - # Flip PID direciton and tune stop - pid_dir = int(query_pid_state()['direction']) - new_d = '0' if (pid_dir == 1) else '1' - self.run_and_validate(clients.pid.set_direction, - kwargs=dict(direction=new_d)) - self.run_and_validate(clients.pid.tune_stop) - + if not state.fast: + # Flip PID direciton and tune stop + new_d = '0' if (pid_dir == 1) else '1' + self.run_and_validate(clients.pid.set_direction, + kwargs=dict(direction=new_d)) + self.run_and_validate(clients.pid.tune_stop) self.run_and_validate(clients.pmx.ign_ext) self.run_and_validate(clients.pmx.set_v, kwargs={'volt': state.brake_voltage}) self.run_and_validate(clients.pmx.set_on) time.sleep(10) self.action.set_state(ControlState.WaitForBrake( - min_freq=0.5, + min_freq=min_freq, prev_freq=hwp_state.enc_freq, + fast=state.fast )) elif isinstance(state, ControlState.WaitForBrake): f0 = query_pid_state()['current_freq'] time.sleep(5) f1 = query_pid_state()['current_freq'] - if f0 < 0.5 or (f1 > f0): + if f0 < state.min_freq or (f1 > f0): self.log.info("Turning off PMX and putting PCU in stop mode") self.run_and_validate(clients.pmx.set_off) self.run_and_validate( clients.pcu.send_command, kwargs={'command': 'stop'}, timeout=None ) - self.action.set_state(ControlState.WaitForTargetFreq( - target_freq=0, - freq_tol=0.05, - freq_tol_duration=30, - )) - return - elif isinstance(state, ControlState.FastBrake): - # Flip PID direciton and tune stop - pid_dir = int(query_pid_state()['direction']) - if pid_dir == 1: - self.run_and_validate( - clients.pcu.send_command, - kwargs={'command': 'on_2'}, timeout=None - ) - else: - self.run_and_validate( - clients.pcu.send_command, - kwargs={'command': 'on_1'}, timeout=None - ) - self.run_and_validate(clients.pmx.ign_ext) - self.run_and_validate(clients.pmx.set_v, kwargs={'volt': state.brake_voltage}) - self.run_and_validate(clients.pmx.set_on) - - time.sleep(10) - self.action.set_state(ControlState.WaitForFastBrake( - min_freq=0.2, - prev_freq=hwp_state.enc_freq, - )) - - elif isinstance(state, ControlState.WaitForFastBrake): - f0 = query_pid_state()['current_freq'] - time.sleep(5) - f1 = query_pid_state()['current_freq'] - if f0 < 0.2 or (f1 > f0): - self.log.info("Turning off PMX and putting PCU in stop mode") - self.run_and_validate(clients.pmx.set_off) - self.run_and_validate( - clients.pcu.send_command, - kwargs={'command': 'stop'}, timeout=None - ) - time.sleep(5) - return + if state.fast: + time.sleep(5) + return + else: + self.action.set_state(ControlState.WaitForTargetFreq( + target_freq=0, + freq_tol=0.05, + freq_tol_duration=30, + )) + return elif isinstance(state, ControlState.EnableDriverBoard): def set_outlet_state(outlet: int, outlet_state: bool): @@ -1635,18 +1592,12 @@ def brake(self, session, params): 'success': True} } """ - if params['fast']: - state = ControlState.FastBrake( - freq_tol=params['freq_tol'], - freq_tol_duration=params['freq_tol_duration'], - brake_voltage=30., - ) - else: - state = ControlState.Brake( - freq_tol=params['freq_tol'], - freq_tol_duration=params['freq_tol_duration'], - brake_voltage=params['brake_voltage'], - ) + state = ControlState.Brake( + freq_tol=params['freq_tol'], + freq_tol_duration=params['freq_tol_duration'], + brake_voltage=params['brake_voltage'], + fast=params['fast'] + ) action = self.control_state_machine.request_new_action(state) action.sleep_until_complete(session=session) return action.success, f"Completed with state: {action.cur_state_info.state}" From 85c91795ae8133bcf6fc892b53241958f7918200 Mon Sep 17 00:00:00 2001 From: ykyohei <38639108+ykyohei@users.noreply.github.com> Date: Sat, 31 May 2025 01:23:35 +0000 Subject: [PATCH 4/4] change last state to be safer, add docs --- socs/agents/hwp_supervisor/agent.py | 39 ++++++++++++++++------------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 55ef4daa8..f4a300db2 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -765,8 +765,8 @@ class ConstVolt: ----------- voltage : float Voltage to set the PMX to - start_time : float - Time that the state was entered + direction : str + Direction to set the PID. Should either be '1' or '0'. """ voltage: float direction: str @@ -782,8 +782,6 @@ class Done: Whether the last state was completed successfully msg : str Optional message to include with the Done state - start_time : float - Time that the state was entered """ success: bool msg: str = None @@ -807,6 +805,13 @@ class Error: class Brake: """ Configure the PID and PMX agents to actively brake the HWP + + Attributes + ----------- + brake_voltage : float + Voltage to use when braking the HWP. + fast : bool + If True brake HWP more actively """ freq_tol: float freq_tol_duration: float @@ -1310,12 +1315,11 @@ def check_gripper_ok_for_spinup(): elif isinstance(state, ControlState.Brake): pid_dir = int(query_pid_state()['direction']) if state.fast: - # set pcu fase brake state + # Keep pid target frequency and + # change pcu phase to brake state new_pcu_state = 'on_2' if (pid_dir == 1) else 'on_1' - min_freq = 0.2 else: new_pcu_state = 'off' - min_freq = 0.5 self.run_and_validate( clients.pcu.send_command, kwargs={'command': new_pcu_state}, timeout=None @@ -1332,7 +1336,7 @@ def check_gripper_ok_for_spinup(): time.sleep(10) self.action.set_state(ControlState.WaitForBrake( - min_freq=min_freq, + min_freq=0.5, prev_freq=hwp_state.enc_freq, fast=state.fast )) @@ -1349,15 +1353,14 @@ def check_gripper_ok_for_spinup(): kwargs={'command': 'stop'}, timeout=None ) if state.fast: - time.sleep(5) - return - else: - self.action.set_state(ControlState.WaitForTargetFreq( - target_freq=0, - freq_tol=0.05, - freq_tol_duration=30, - )) - return + # Change target frequency to 0 Hz. + self.run_and_validate(clients.pid.tune_stop) + self.action.set_state(ControlState.WaitForTargetFreq( + target_freq=0, + freq_tol=0.05, + freq_tol_duration=30, + )) + return elif isinstance(state, ControlState.EnableDriverBoard): def set_outlet_state(outlet: int, outlet_state: bool): @@ -1805,6 +1808,8 @@ def brake(self, session, params): ``target_freq`` to be considered successful. brake_voltage: float Voltage to use when braking the HWP. + fast: bool + If True, brake HWP more actively. Notes --------