From 5e7cceddbff7d4b0f6900a98e7f24432e63f377b Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Fri, 24 Dec 2021 14:19:40 +0900 Subject: [PATCH 01/14] #491 Remove unnecessary limit --- lib/device/antenna_az_commander_pid.py | 17 +++++++++++------ lib/device/antenna_el_commander_pid.py | 17 +++++++++++------ 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/lib/device/antenna_az_commander_pid.py b/lib/device/antenna_az_commander_pid.py index da4301a2..a84fc204 100644 --- a/lib/device/antenna_az_commander_pid.py +++ b/lib/device/antenna_az_commander_pid.py @@ -5,7 +5,10 @@ import rospy from std_msgs.msg import Float64 -from .antenna_pid import PIDController +if __name__ == "__main__": # ROS1 may use this. That's why we should run `chmod +x`. + from antenna_pid import PIDController +else: # For redundancy considering package name conflict. ROS2 will use this. + from .antenna_pid import PIDController node_name = "antenna_az_commander_pid" @@ -57,6 +60,7 @@ def __init__(self): self.hensa_stock = self.controller.error # Alias. self.init_ros() + # Aliases. self.topic_to = self.Publisher["calculated_rate"] self.topic_cur = self.Publisher["current_speed"] self.topic_tar = self.Publisher["target_speed"] @@ -86,12 +90,12 @@ def antenna_az_feedback(self, command: Float64): cmd_coord = self.controller.suitable_angle( self.enc_coord, command.data, [0, 360], margin=0, unit="deg" ) - speed = self.controller.get_speed(cmd_coord, self.enc_coord, unit="deg") - self.current_speed = self.controller.cmd_speed[Now] + speed = self.controller.get_speed( + cmd_coord, self.enc_coord, unit="deg", stop=self.lock + ) + self.current_speed = self.controller.cmd_speed[Now] # Alias. rate = speed * self.SPEED2RATE - if self.lock: - rate = 0 - self.speed_d = rate + self.speed_d = rate # Alias. self.Publisher["calculated_rate"].publish(rate) target_speed = ( self.controller.cmd_coord[Now] - self.controller.cmd_coord[Last] @@ -100,6 +104,7 @@ def antenna_az_feedback(self, command: Float64): self.Publisher["current_speed"].publish(self.current_speed) self.Publisher["error"].publish(self.controller.error[Now]) + # Aliases. self.t_past = self.controller.time[Last] self.t_now = self.controller.time[Now] self.pre_hensa = self.controller.error[Last] diff --git a/lib/device/antenna_el_commander_pid.py b/lib/device/antenna_el_commander_pid.py index a8dfc5dc..a2ecebd7 100644 --- a/lib/device/antenna_el_commander_pid.py +++ b/lib/device/antenna_el_commander_pid.py @@ -5,7 +5,10 @@ import rospy from std_msgs.msg import Float64 -from .antenna_pid import PIDController +if __name__ == "__main__": # ROS1 may use this. That's why we should run `chmod +x`. + from antenna_pid import PIDController +else: # For redundancy considering package name conflict. ROS2 will use this. + from .antenna_pid import PIDController node_name = "antenna_el_commander_pid" @@ -57,6 +60,7 @@ def __init__(self): self.hensa_stock = self.controller.error # Alias. self.init_ros() + # Aliases. self.topic_to = self.Publisher["calculated_rate"] self.topic_cur = self.Publisher["current_speed"] self.topic_tar = self.Publisher["target_speed"] @@ -83,12 +87,12 @@ def init_ros(self): } def antenna_el_feedback(self, command: Float64): - speed = self.controller.get_speed(command.data, self.enc_coord, unit="deg") - self.current_speed = self.controller.cmd_speed[Now] + speed = self.controller.get_speed( + command.data, self.enc_coord, unit="deg", stop=self.lock + ) + self.current_speed = self.controller.cmd_speed[Now] # Alias. rate = speed * self.SPEED2RATE - if self.lock: - rate = 0 - self.speed_d = rate + self.speed_d = rate # Alias. self.Publisher["calculated_rate"].publish(rate) target_speed = ( self.controller.cmd_coord[Now] - self.controller.cmd_coord[Last] @@ -97,6 +101,7 @@ def antenna_el_feedback(self, command: Float64): self.Publisher["current_speed"].publish(self.current_speed) self.Publisher["error"].publish(self.controller.error[Now]) + # Aliases. self.t_past = self.controller.time[Last] self.t_now = self.controller.time[Now] self.pre_hensa = self.controller.error[Last] From d5de3846abd4044fa2d738b36504899851a0c3ad Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Fri, 24 Dec 2021 14:20:36 +0900 Subject: [PATCH 02/14] #491 Add target speed limit --- lib/device/antenna_pid.py | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/lib/device/antenna_pid.py b/lib/device/antenna_pid.py index 28ece23a..368478a5 100644 --- a/lib/device/antenna_pid.py +++ b/lib/device/antenna_pid.py @@ -13,7 +13,10 @@ import numpy as np -from . import utils +if __name__ == "__main__": # If this module is included in ROS1 package. + import utils +else: + from . import utils # Indices for 2-lists (mutable version of so-called 2-tuple). Last = -2 @@ -59,7 +62,7 @@ def with_configuration( Examples -------- - >>> AntennaDevice.with_configuration("az", pid_param=[2.2, 0, 0]) + >>> AntennaDevice.with_configuration(pid_param=[2.2, 0, 0]) """ if pid_param is not None: @@ -90,14 +93,16 @@ def error_derivative(self) -> float: def set_initial_parameters(self, cmd_coord: float, enc_coord: float) -> None: self.initialize() - utils.update_list(self.cmd_speed, 0) + if getattr(self, "cmd_speed", None) is None: + utils.update_list(self.cmd_speed, 0) utils.update_list(self.time, time.time()) utils.update_list(self.cmd_coord, cmd_coord) utils.update_list(self.enc_coord, enc_coord) utils.update_list(self.error, cmd_coord - enc_coord) def initialize(self) -> None: - self.cmd_speed = DefaultTwoList.copy() + if getattr(self, "cmd_speed", None) is None: + self.cmd_speed = DefaultTwoList.copy() self.time = DefaultTwoList.copy() * int(self.ERROR_INTEG_COUNT / 2) self.cmd_coord = DefaultTwoList.copy() self.enc_coord = DefaultTwoList.copy() @@ -137,8 +142,9 @@ def get_speed( cmd_coord *= factor enc_coord *= factor - threshold = 2.5 / self.dt # 2.5deg/s - if np.isnan(self.time[Now]) or (abs(self.error_derivative) > threshold): + threshold = 100 / 3600 # 100arcsec + delta_cmd_coord = cmd_coord - self.cmd_coord[Now] + if np.isnan(self.time[Now]) or (abs(delta_cmd_coord) > threshold): self.set_initial_parameters(cmd_coord, enc_coord) # Set default values on initial run or on detection of sudden jump of error, # which may indicate a change of commanded coordinate. @@ -159,7 +165,8 @@ def get_speed( if abs(self.error[Now]) > (20 / 3600): # 20arcsec # When error is small, smooth control delays the convergence of drive. # When error is large, smooth control can avoid overshooting. - max_diff = self.MAX_ACCELERATION * self.dt + max_diff = utils.clip(self.MAX_ACCELERATION * self.dt, -0.2, 0.2) + # 0.2 clipping is to avoid large acceleration caused by large dt. speed = utils.clip( speed, current_speed - max_diff, current_speed + max_diff ) # Limit acceleration. @@ -176,6 +183,9 @@ def calc_pid(self) -> float: # Speed of the move of commanded coordinate. This includes sidereal motion, scan # speed, and other non-static component of commanded value. target_speed = (self.cmd_coord[Now] - self.cmd_coord[Last]) / self.dt + threshold = 1 # 1deg/s + if target_speed > threshold: + target_speed = 0 return ( target_speed @@ -217,12 +227,11 @@ def suitable_angle( margin *= factor # Avoid 360deg motion. - safety_margin = margin # deg target_min_candidate = target - 360 * ((target - limits[0]) // 360) target_candidates = [ angle for angle in utils.frange(target_min_candidate, limits[1], 360) - if (limits[0] + safety_margin) < angle < (limits[1] - safety_margin) + if (limits[0] + margin) < angle < (limits[1] - margin) ] if len(target_candidates) == 1: return target_candidates[0] * utils.angle_conversion_factor("deg", unit) From dd77dc0161a5bd99c88f8bb0b8c84c2b285b9acc Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Fri, 24 Dec 2021 14:47:10 +0900 Subject: [PATCH 03/14] #491 Reflect az script --- lib/device/antenna_el_commander_pid.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/lib/device/antenna_el_commander_pid.py b/lib/device/antenna_el_commander_pid.py index a2ecebd7..1c38a29b 100644 --- a/lib/device/antenna_el_commander_pid.py +++ b/lib/device/antenna_el_commander_pid.py @@ -87,6 +87,9 @@ def init_ros(self): } def antenna_el_feedback(self, command: Float64): + # cmd_coord = self.controller.suitable_angle( + # self.enc_coord, command.data, [0, 360], margin=0, unit="deg" + # ) speed = self.controller.get_speed( command.data, self.enc_coord, unit="deg", stop=self.lock ) From 3a849a8f5091a4c0a38f5eae4bbcfda2bcee45f1 Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Fri, 24 Dec 2021 15:40:48 +0900 Subject: [PATCH 04/14] #491 Change target speed application condition --- lib/device/antenna_pid.py | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/lib/device/antenna_pid.py b/lib/device/antenna_pid.py index 368478a5..dcccfad1 100644 --- a/lib/device/antenna_pid.py +++ b/lib/device/antenna_pid.py @@ -99,6 +99,7 @@ def set_initial_parameters(self, cmd_coord: float, enc_coord: float) -> None: utils.update_list(self.cmd_coord, cmd_coord) utils.update_list(self.enc_coord, enc_coord) utils.update_list(self.error, cmd_coord - enc_coord) + utils.update_list(self.target_speed, 0) def initialize(self) -> None: if getattr(self, "cmd_speed", None) is None: @@ -107,6 +108,7 @@ def initialize(self) -> None: self.cmd_coord = DefaultTwoList.copy() self.enc_coord = DefaultTwoList.copy() self.error = DefaultTwoList.copy() * int(self.ERROR_INTEG_COUNT / 2) + self.target_speed = DefaultTwoList.copy() # Without `copy()`, updating one of them updates all its shared (not copied) # objects. @@ -159,6 +161,9 @@ def get_speed( utils.update_list(self.cmd_coord, cmd_coord) utils.update_list(self.enc_coord, enc_coord) utils.update_list(self.error, cmd_coord - enc_coord) + utils.update_list( + self.target_speed, (cmd_coord - self.cmd_coord[Now]) / self.dt + ) # Calculate and validate drive speed. speed = self.calc_pid() @@ -182,13 +187,15 @@ def get_speed( def calc_pid(self) -> float: # Speed of the move of commanded coordinate. This includes sidereal motion, scan # speed, and other non-static component of commanded value. - target_speed = (self.cmd_coord[Now] - self.cmd_coord[Last]) / self.dt - threshold = 1 # 1deg/s - if target_speed > threshold: - target_speed = 0 + target_acceleration = ( + self.target_speed[Now] - self.target_speed[Last] + ) / self.dt + threshold = 2 # 2deg/s^2 + if abs(target_acceleration) > threshold: + self.target_speed[Now] = 0 return ( - target_speed + self.target_speed[Now] + self.K_p * self.error[Now] + self.K_i * self.error_integral + self.K_d * self.error_derivative From ea3d923b2f88f8df03053e7088a730ad1d418c04 Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Sat, 25 Dec 2021 11:04:16 +0900 Subject: [PATCH 05/14] #491 Fix import handling --- lib/device/antenna_pid.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lib/device/antenna_pid.py b/lib/device/antenna_pid.py index dcccfad1..8c2f96a2 100644 --- a/lib/device/antenna_pid.py +++ b/lib/device/antenna_pid.py @@ -13,10 +13,10 @@ import numpy as np -if __name__ == "__main__": # If this module is included in ROS1 package. - import utils -else: +if __name__.endswith(".antenna_pid"): from . import utils +else: + import utils # Indices for 2-lists (mutable version of so-called 2-tuple). Last = -2 From 6cf08eb946e6b5ab17d0646d3381f3ba887a8f9f Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Sat, 25 Dec 2021 13:03:53 +0900 Subject: [PATCH 06/14] #491 Remove 1.85-m specific modules --- lib/device/antenna_az_commander_pid.py | 181 ------------------------- lib/device/antenna_el_commander_pid.py | 181 ------------------------- 2 files changed, 362 deletions(-) delete mode 100644 lib/device/antenna_az_commander_pid.py delete mode 100644 lib/device/antenna_el_commander_pid.py diff --git a/lib/device/antenna_az_commander_pid.py b/lib/device/antenna_az_commander_pid.py deleted file mode 100644 index a84fc204..00000000 --- a/lib/device/antenna_az_commander_pid.py +++ /dev/null @@ -1,181 +0,0 @@ -#!/usr/bin/env python3 - -from typing import Tuple - -import rospy -from std_msgs.msg import Float64 - -if __name__ == "__main__": # ROS1 may use this. That's why we should run `chmod +x`. - from antenna_pid import PIDController -else: # For redundancy considering package name conflict. ROS2 will use this. - from .antenna_pid import PIDController - -node_name = "antenna_az_commander_pid" - -# Indices for 2-lists (mutable version of so-called 2-tuple). -Last = -2 -Now = -1 - - -class antenna_az_feedback: - """For 1.85-m telescope.""" - - speed_d = 0 - pre_deg = 0 - pre_hensa = 0 - enc_before = 0 - ihensa = 0 - i_ave_num = 10 - t_now = t_past = 0 - current_speed = 0 - deg_enc = 0 - lock = False - - SPEED2RATE: float - - def __init__(self): - self.gear_ratio = rospy.get_param("~gear_ratio") - self.pulseper360deg = rospy.get_param("~pulseper360deg") - self.pulse_a = rospy.get_param("~pulse_a") - self.pulse_b = rospy.get_param("~pulse_b") - self.SPEED2RATE = ( - (self.gear_ratio / 360) - * self.pulseper360deg - * (self.pulse_b / self.pulse_a) - ) # Speed [deg/s] to servomotor rate. - - self.p_coeff = rospy.get_param("~p_coeff") - self.i_coeff = rospy.get_param("~i_coeff") - self.d_coeff = rospy.get_param("~d_coeff") - # Following 2 are (derivative of) rate, not speed. - self.MOTOR_MAX_STEP = rospy.get_param("~MOTOR_MAXSTEP") - self.MOTOR_AZ_MAXSPEED = rospy.get_param("~MOTOR_AZ_MAXSPEED") - self.controller = PIDController.with_configuration( - pid_param=[self.p_coeff, self.i_coeff, self.d_coeff], - max_speed=self.MOTOR_AZ_MAXSPEED / self.SPEED2RATE, - max_acceleration=2, - error_integ_count=self.i_ave_num, - ) - - self.hensa_stock = self.controller.error # Alias. - - self.init_ros() - # Aliases. - self.topic_to = self.Publisher["calculated_rate"] - self.topic_cur = self.Publisher["current_speed"] - self.topic_tar = self.Publisher["target_speed"] - self.topic_hensa = self.Publisher["error"] - - def init_ros(self): - _publisher_conf = { - "calculated_rate": ["/1p85m/az_speed", Float64, 1], - "current_speed": ["/1p85m/az_current_speed", Float64, 1], - "target_speed": ["/1p85m/az_target_speed", Float64, 1], - "error": ["/1p85m/az_pid_hensa", Float64, 1], - } - _subscriber_conf = { - "commanded_coord": ["/1p85m/az_cmd2", Float64, self.antenna_az_feedback, 1], - "encoder_coord": ["/1p85m/az", Float64, self.antenna_az_encoder, 1], - } - self.Publisher = { - key: rospy.Publisher(name, msg_type, queue_size=n) - for key, (name, msg_type, n) in _publisher_conf.items() - } - self.Subscriber = { - key: rospy.Subscriber(name, msg_type, callback, queue_size=n) - for key, (name, msg_type, callback, n) in _subscriber_conf.items() - } - - def antenna_az_feedback(self, command: Float64): - cmd_coord = self.controller.suitable_angle( - self.enc_coord, command.data, [0, 360], margin=0, unit="deg" - ) - speed = self.controller.get_speed( - cmd_coord, self.enc_coord, unit="deg", stop=self.lock - ) - self.current_speed = self.controller.cmd_speed[Now] # Alias. - rate = speed * self.SPEED2RATE - self.speed_d = rate # Alias. - self.Publisher["calculated_rate"].publish(rate) - target_speed = ( - self.controller.cmd_coord[Now] - self.controller.cmd_coord[Last] - ) / self.controller.dt - self.Publisher["target_speed"].publish(target_speed) - self.Publisher["current_speed"].publish(self.current_speed) - self.Publisher["error"].publish(self.controller.error[Now]) - - # Aliases. - self.t_past = self.controller.time[Last] - self.t_now = self.controller.time[Now] - self.pre_hensa = self.controller.error[Last] - self.pre_deg = self.controller.cmd_coord[Last] - self.enc_before = self.controller.enc_coord[Last] - self.ihensa = self.controller.error_integral - - def antenna_az_encoder(self, status: Float64): - self.enc_coord = status.data - self.deg_enc = self.enc_coord # Alias. - - def antenna_az_pid(self, status: Float64): - """No topic triggers this callback function.""" - self.p_coeff = self.controller.K_p = status.data[0] - self.i_coeff = self.controller.K_i = status.data[1] - self.d_coeff = self.controller.K_d = status.data[2] - - def calc_pid( - self, - target_deg: float, - encoder_deg: float, - pre_deg: float, - pre_hensa: float, - ihensa: float, - enc_before: float, - t_now: float, - t_past: float, - p_coeff: float, - i_coeff: float, - d_coeff: float, - ) -> Tuple[float, float]: - """PID calculation. - - ..deprecated:: v3.1.0 - This function will be removed in v4.0.0, because of implementation structure - issue. Please use `AntennaDevice.calc_pid`. - - """ - calculator = PIDController.with_configuration( - pid_param=[p_coeff, i_coeff, d_coeff] - ) - - # Set `Last` parameters. - calculator._update(calculator.time, t_past) - calculator._update(calculator.cmd_coord, pre_deg) - calculator._update(calculator.enc_coord, enc_before) - calculator._update(calculator.error, pre_hensa) - - # Set `Now` parameters. - calculator._update(calculator.time, t_now) - calculator._update(calculator.cmd_coord, target_deg) - calculator._update(calculator.enc_coord, encoder_deg) - calculator._update(calculator.error, target_deg - encoder_deg) - calculator._update(calculator.error_integ, ihensa) - - speed = calculator.calc_pid() - - self.hensa_stock = calculator.error - self.current_speed = calculator.cmd_speed[Now] - - target_speed = ( - self.controller.cmd_coord[Now] - self.controller.cmd_coord[Last] - ) / self.controller.dt - self.Publisher["target_speed"].publish(target_speed) - self.Publisher["current_speed"].publish(self.current_speed) - self.Publisher["error"].publish(self.controller.error[Now]) - - return (speed, calculator.error_integ) - - -if __name__ == "__main__": - rospy.init_node(node_name) - antenna_az_feedback() - rospy.spin() diff --git a/lib/device/antenna_el_commander_pid.py b/lib/device/antenna_el_commander_pid.py deleted file mode 100644 index 1c38a29b..00000000 --- a/lib/device/antenna_el_commander_pid.py +++ /dev/null @@ -1,181 +0,0 @@ -#!/usr/bin/env python3 - -from typing import Tuple - -import rospy -from std_msgs.msg import Float64 - -if __name__ == "__main__": # ROS1 may use this. That's why we should run `chmod +x`. - from antenna_pid import PIDController -else: # For redundancy considering package name conflict. ROS2 will use this. - from .antenna_pid import PIDController - -node_name = "antenna_el_commander_pid" - -# Indices for 2-lists (mutable version of so-called 2-tuple). -Last = -2 -Now = -1 - - -class antenna_el_feedback: - """For 1.85-m telescope.""" - - speed_d = 0 - pre_deg = 0 - pre_hensa = 0 - enc_before = 0 - ihensa = 0 - i_ave_num = 10 - t_now = t_past = 0 - current_speed = 0 - deg_enc = 0 - lock = False - - SPEED2RATE: float - - def __init__(self): - self.gear_ratio = rospy.get_param("~gear_ratio") - self.pulseper360deg = rospy.get_param("~pulseper360deg") - self.pulse_a = rospy.get_param("~pulse_a") - self.pulse_b = rospy.get_param("~pulse_b") - self.SPEED2RATE = ( - (self.gear_ratio / 360) - * self.pulseper360deg - * (self.pulse_b / self.pulse_a) - ) # Speed [deg/s] to servomotor rate. - - self.p_coeff = rospy.get_param("~p_coeff") - self.i_coeff = rospy.get_param("~i_coeff") - self.d_coeff = rospy.get_param("~d_coeff") - # Following 2 are (derivative of) rate, not speed. - self.MOTOR_MAX_STEP = rospy.get_param("~MOTOR_MAXSTEP") - self.MOTOR_EL_MAXSPEED = rospy.get_param("~MOTOR_EL_MAXSPEED") - self.controller = PIDController.with_configuration( - pid_param=[self.p_coeff, self.i_coeff, self.d_coeff], - max_speed=self.MOTOR_EL_MAXSPEED / self.SPEED2RATE, - max_acceleration=2, - error_integ_count=self.i_ave_num, - ) - - self.hensa_stock = self.controller.error # Alias. - - self.init_ros() - # Aliases. - self.topic_to = self.Publisher["calculated_rate"] - self.topic_cur = self.Publisher["current_speed"] - self.topic_tar = self.Publisher["target_speed"] - self.topic_hensa = self.Publisher["error"] - - def init_ros(self): - _publisher_conf = { - "calculated_rate": ["/1p85m/el_speed", Float64, 1], - "current_speed": ["/1p85m/el_current_speed", Float64, 1], - "target_speed": ["/1p85m/el_target_speed", Float64, 1], - "error": ["/1p85m/el_pid_hensa", Float64, 1], - } - _subscriber_conf = { - "commanded_coord": ["/1p85m/el_cmd2", Float64, self.antenna_el_feedback, 1], - "encoder_coord": ["/1p85m/el", Float64, self.antenna_el_encoder, 1], - } - self.Publisher = { - key: rospy.Publisher(name, msg_type, queue_size=n) - for key, (name, msg_type, n) in _publisher_conf.items() - } - self.Subscriber = { - key: rospy.Subscriber(name, msg_type, callback, queue_size=n) - for key, (name, msg_type, callback, n) in _subscriber_conf.items() - } - - def antenna_el_feedback(self, command: Float64): - # cmd_coord = self.controller.suitable_angle( - # self.enc_coord, command.data, [0, 360], margin=0, unit="deg" - # ) - speed = self.controller.get_speed( - command.data, self.enc_coord, unit="deg", stop=self.lock - ) - self.current_speed = self.controller.cmd_speed[Now] # Alias. - rate = speed * self.SPEED2RATE - self.speed_d = rate # Alias. - self.Publisher["calculated_rate"].publish(rate) - target_speed = ( - self.controller.cmd_coord[Now] - self.controller.cmd_coord[Last] - ) / self.controller.dt - self.Publisher["target_speed"].publish(target_speed) - self.Publisher["current_speed"].publish(self.current_speed) - self.Publisher["error"].publish(self.controller.error[Now]) - - # Aliases. - self.t_past = self.controller.time[Last] - self.t_now = self.controller.time[Now] - self.pre_hensa = self.controller.error[Last] - self.pre_deg = self.controller.cmd_coord[Last] - self.enc_before = self.controller.enc_coord[Last] - self.ihensa = self.controller.error_integral - - def antenna_el_encoder(self, status: Float64): - self.enc_coord = status.data - self.deg_enc = self.enc_coord # Alias. - - def antenna_el_pid(self, status: Float64): - """No topic triggers this callback function.""" - self.p_coeff = self.controller.K_p = status.data[0] - self.i_coeff = self.controller.K_i = status.data[1] - self.d_coeff = self.controller.K_d = status.data[2] - - def calc_pid( - self, - target_deg: float, - encoder_deg: float, - pre_deg: float, - pre_hensa: float, - ihensa: float, - enc_before: float, - t_now: float, - t_past: float, - p_coeff: float, - i_coeff: float, - d_coeff: float, - ) -> Tuple[float, float]: - """PID calculation. - - ..deprecated:: v3.1.0 - This function will be removed in v4.0.0, because of implementation structure - issue. Please use `AntennaDevice.calc_pid`. - - """ - calculator = PIDController.with_configuration( - pid_param=[p_coeff, i_coeff, d_coeff] - ) - - # Set `Last` parameters. - calculator._update(calculator.time, t_past) - calculator._update(calculator.cmd_coord, pre_deg) - calculator._update(calculator.enc_coord, enc_before) - calculator._update(calculator.error, pre_hensa) - - # Set `Now` parameters. - calculator._update(calculator.time, t_now) - calculator._update(calculator.cmd_coord, target_deg) - calculator._update(calculator.enc_coord, encoder_deg) - calculator._update(calculator.error, target_deg - encoder_deg) - calculator._update(calculator.error_integ, ihensa) - - speed = calculator.calc_pid() - - self.hensa_stock = calculator.error - self.current_speed = calculator.cmd_speed[Now] - - target_speed = ( - self.controller.cmd_coord[Now] - self.controller.cmd_coord[Last] - ) / self.controller.dt - self.Publisher["target_speed"].publish(target_speed) - self.Publisher["current_speed"].publish(self.current_speed) - self.Publisher["error"].publish(self.controller.error[Now]) - - return (speed, calculator.error_integ) - - -if __name__ == "__main__": - rospy.init_node(node_name) - antenna_el_feedback() - rospy.spin() From eb27668e59839f4ff161069457cd7eaa44dae041 Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Wed, 29 Dec 2021 13:01:52 +0900 Subject: [PATCH 07/14] #491 Fix typo --- lib/device/antenna_pid.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/device/antenna_pid.py b/lib/device/antenna_pid.py index 8c2f96a2..1b342a8d 100644 --- a/lib/device/antenna_pid.py +++ b/lib/device/antenna_pid.py @@ -36,7 +36,7 @@ class PIDController: K_p: float = 1.0 K_i: float = 0.5 - K_p: float = 0.3 + K_d: float = 0.3 MAX_SPEED: float = 2 # deg/s MAX_ACCELERATION: float = 2 # deg/s^2 From b35e967d81a78438b0da8bab81ed30b82f591977 Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Thu, 3 Feb 2022 19:45:56 +0900 Subject: [PATCH 08/14] #491 Fix type annotation --- lib/device/antenna_device.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/lib/device/antenna_device.py b/lib/device/antenna_device.py index 6bac8ec6..8c15651d 100644 --- a/lib/device/antenna_device.py +++ b/lib/device/antenna_device.py @@ -13,7 +13,12 @@ import time from typing import Dict, Tuple -from .antenna_pid import PIDController +try: + from typing import Literal +except ImportError: + from typing_extensions import Literal + +from antenna_pid import PIDController # Indices for 2-lists (mutable version of so-called 2-tuple). Last = -2 @@ -124,7 +129,7 @@ def move_azel( enc_az: float, enc_el: float, pid_param: Dict[str, Tuple[float, float, float]] = None, - m_bStop: str = "FALSE", + m_bStop: Literal["FALSE", "TRUE"] = "FALSE", ) -> Tuple[float, ...]: if pid_param is not None: self.set_pid_param(pid_param) From a33a0567839e335f635d3ddd81577b2fd7c530b5 Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Thu, 3 Feb 2022 19:48:42 +0900 Subject: [PATCH 09/14] #491 Add typing dependency --- pyproject.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/pyproject.toml b/pyproject.toml index 30b5d0cd..01fdc900 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,6 +11,7 @@ matplotlib = "^3.3" n_const = "^0.3" numpy = "^1.19" # nasco_system = { git="https://github.com/ogawa-ros/nasco_system.git", branch="master" } +typing-extensions = { version = "^3.10.0", python = "<3.8" } [tool.poetry.dev-dependencies] pytest = "^5.2" From 5db712e2fa09ea9dbe3ba73b6b23aeda48bf8fdf Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Thu, 3 Feb 2022 19:50:54 +0900 Subject: [PATCH 10/14] #491 Ignore editor non-necessities --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.gitignore b/.gitignore index d1317fcf..e10b509d 100644 --- a/.gitignore +++ b/.gitignore @@ -93,3 +93,7 @@ ENV/ # Mac .DS_Store + +# editor +.vscode/ +*.code-workspace From c419ff83b68630c5ff51cbe60fecd1400835dce2 Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Thu, 17 Mar 2022 14:47:45 +0900 Subject: [PATCH 11/14] #491 Use neclib --- lib/device/antenna_device.py | 14 +- lib/device/antenna_pid.py | 250 ----------------------------- lib/device/utils/__init__.py | 5 - lib/device/utils/data_utils.py | 9 -- lib/device/utils/math_utils.py | 16 -- lib/device/utils/quantity_utils.py | 18 --- poetry.lock | 106 ++++++++---- pyproject.toml | 10 +- 8 files changed, 80 insertions(+), 348 deletions(-) delete mode 100644 lib/device/antenna_pid.py delete mode 100644 lib/device/utils/__init__.py delete mode 100644 lib/device/utils/data_utils.py delete mode 100644 lib/device/utils/math_utils.py delete mode 100644 lib/device/utils/quantity_utils.py diff --git a/lib/device/antenna_device.py b/lib/device/antenna_device.py index 8c15651d..abfd3ea7 100644 --- a/lib/device/antenna_device.py +++ b/lib/device/antenna_device.py @@ -1,15 +1,5 @@ #!/usr/bin/env python3 -""" - -..Design Policy:: - - This script will be executed in high frequency with no vectorization, and there are - many arrays updated frequently. These mean that the use of Numpy may not be the best - choice to speed up the calculation. Measure the execution time first, then implement. - -""" - import time from typing import Dict, Tuple @@ -18,9 +8,9 @@ except ImportError: from typing_extensions import Literal -from antenna_pid import PIDController +from neclib import PIDController -# Indices for 2-lists (mutable version of so-called 2-tuple). +# Indices for parameter lists. Last = -2 Now = -1 diff --git a/lib/device/antenna_pid.py b/lib/device/antenna_pid.py deleted file mode 100644 index 1b342a8d..00000000 --- a/lib/device/antenna_pid.py +++ /dev/null @@ -1,250 +0,0 @@ -""" - -..Design Policy:: - - This script will be executed in high frequency with no vectorization, and there are - many arrays updated frequently. These mean that the use of Numpy may not be the best - choice to speed up the calculation. Measure the execution time first, then implement. - -""" - -import time -from typing import Tuple - -import numpy as np - -if __name__.endswith(".antenna_pid"): - from . import utils -else: - import utils - -# Indices for 2-lists (mutable version of so-called 2-tuple). -Last = -2 -Now = -1 -# Default value for 2-lists. -DefaultTwoList = [np.nan, np.nan] - - -class PIDController: - """Controller of telescope antenna drive. - - Notes - ----- - - Both the NANTEN2 and 1.85-m telescopes uses servomotors for antenna drive. - - """ - - K_p: float = 1.0 - K_i: float = 0.5 - K_d: float = 0.3 - - MAX_SPEED: float = 2 # deg/s - MAX_ACCELERATION: float = 2 # deg/s^2 - - ERROR_INTEG_COUNT: int = 50 # Keep last 50 data for error integration. - # Time interval of error integral varies according to PID calculation frequency, - # which may cause optimal PID parameters to change according to the frequency. - - def __init__(self) -> None: - # Initialize parameters. - self.initialize() - - @classmethod - def with_configuration( - cls, - *, - pid_param: Tuple[float, float, float] = None, - max_speed: float = None, - max_acceleration: float = None, - error_integ_count: int = None, - ) -> "PIDController": - """Initialize `AntennaDevice` class with properly configured parameters. - - Examples - -------- - >>> AntennaDevice.with_configuration(pid_param=[2.2, 0, 0]) - - """ - if pid_param is not None: - cls.K_p, cls.K_i, cls.K_d = pid_param - if max_speed is not None: - cls.MAX_SPEED = max_speed - if max_acceleration is not None: - cls.MAX_ACCELERATION = max_acceleration - if error_integ_count is not None: - cls.ERROR_INTEG_COUNT = error_integ_count - return cls() - - @property - def dt(self) -> float: - """Time interval of last 2 PID calculations.""" - return self.time[Now] - self.time[Last] - - @property - def error_integral(self) -> float: - _time, _error = np.array(self.time), np.array(self.error) - dt = _time[1:] - _time[:-1] - error_interpolated = (_error[1:] + _error[:-1]) / 2 - return np.nansum(error_interpolated * dt) - - @property - def error_derivative(self) -> float: - return (self.error[Now] - self.error[Last]) / self.dt - - def set_initial_parameters(self, cmd_coord: float, enc_coord: float) -> None: - self.initialize() - if getattr(self, "cmd_speed", None) is None: - utils.update_list(self.cmd_speed, 0) - utils.update_list(self.time, time.time()) - utils.update_list(self.cmd_coord, cmd_coord) - utils.update_list(self.enc_coord, enc_coord) - utils.update_list(self.error, cmd_coord - enc_coord) - utils.update_list(self.target_speed, 0) - - def initialize(self) -> None: - if getattr(self, "cmd_speed", None) is None: - self.cmd_speed = DefaultTwoList.copy() - self.time = DefaultTwoList.copy() * int(self.ERROR_INTEG_COUNT / 2) - self.cmd_coord = DefaultTwoList.copy() - self.enc_coord = DefaultTwoList.copy() - self.error = DefaultTwoList.copy() * int(self.ERROR_INTEG_COUNT / 2) - self.target_speed = DefaultTwoList.copy() - # Without `copy()`, updating one of them updates all its shared (not copied) - # objects. - - def get_speed( - self, - cmd_coord: float, - enc_coord: float, - stop: bool = False, - unit: str = "deg", - ) -> float: - """Calculates valid drive speed. - - Parameters - ---------- - cmd_coord - Instructed AzEl coordinate. - enc_coord - AzEl encoder reading. - stop - If `True`, the telescope won't move regardless of the inputs. - unit - Unit in which `cmd_coord` and `enc_coord` are given. One of ["deg", - "arcmin", "arcsec"] - - Returns - ------- - speed - Speed which will be commanded to motor, in original unit. - - """ - # Convert to deg. - factor = utils.angle_conversion_factor(unit, "deg") - cmd_coord *= factor - enc_coord *= factor - - threshold = 100 / 3600 # 100arcsec - delta_cmd_coord = cmd_coord - self.cmd_coord[Now] - if np.isnan(self.time[Now]) or (abs(delta_cmd_coord) > threshold): - self.set_initial_parameters(cmd_coord, enc_coord) - # Set default values on initial run or on detection of sudden jump of error, - # which may indicate a change of commanded coordinate. - # This will give too small `self.dt` later, but that won't propose any - # problem, since `current_speed` goes to 0, and too large target_speed will - # be suppressed by speed and acceleration limit. - - current_speed = self.cmd_speed[Now] - # Encoder readings cannot be used, due to the lack of stability. - - utils.update_list(self.time, time.time()) - utils.update_list(self.cmd_coord, cmd_coord) - utils.update_list(self.enc_coord, enc_coord) - utils.update_list(self.error, cmd_coord - enc_coord) - utils.update_list( - self.target_speed, (cmd_coord - self.cmd_coord[Now]) / self.dt - ) - - # Calculate and validate drive speed. - speed = self.calc_pid() - if abs(self.error[Now]) > (20 / 3600): # 20arcsec - # When error is small, smooth control delays the convergence of drive. - # When error is large, smooth control can avoid overshooting. - max_diff = utils.clip(self.MAX_ACCELERATION * self.dt, -0.2, 0.2) - # 0.2 clipping is to avoid large acceleration caused by large dt. - speed = utils.clip( - speed, current_speed - max_diff, current_speed + max_diff - ) # Limit acceleration. - speed = utils.clip(speed, -1 * self.MAX_SPEED, self.MAX_SPEED) # Limit speed. - - if stop: - utils.update_list(self.cmd_speed, 0) - else: - utils.update_list(self.cmd_speed, speed) - - return self.cmd_speed[Now] * utils.angle_conversion_factor("deg", unit) - - def calc_pid(self) -> float: - # Speed of the move of commanded coordinate. This includes sidereal motion, scan - # speed, and other non-static component of commanded value. - target_acceleration = ( - self.target_speed[Now] - self.target_speed[Last] - ) / self.dt - threshold = 2 # 2deg/s^2 - if abs(target_acceleration) > threshold: - self.target_speed[Now] = 0 - - return ( - self.target_speed[Now] - + self.K_p * self.error[Now] - + self.K_i * self.error_integral - + self.K_d * self.error_derivative - ) - - @staticmethod - def suitable_angle( - current: float, - target: float, - limits: Tuple[float, float], - margin: float = 40, - unit: str = "deg", - ) -> float: - """Find suitable unwrapped angle. - - Returns - ------- - angle - Unwrapped angle in the same unit as the input. - - Notes - ----- - Azimuthal control of telescope should avoid - 1. 360deg motion during observation. This mean you should observe around - -100deg, not 260deg, to command telescope of [-270, 270]deg limit. - 2. Over-180deg motion. Both 170deg and -190deg are safe in avoiding the 360deg - motion, but if the telescope is currently directed at 10deg, you should select - 170deg to save time. - - """ - assert limits[0] < limits[1], "Limits should be given in ascending order." - factor = utils.angle_conversion_factor(unit, "deg") - current *= factor - target *= factor - limits = [lim * factor for lim in limits] - margin *= factor - - # Avoid 360deg motion. - target_min_candidate = target - 360 * ((target - limits[0]) // 360) - target_candidates = [ - angle - for angle in utils.frange(target_min_candidate, limits[1], 360) - if (limits[0] + margin) < angle < (limits[1] - margin) - ] - if len(target_candidates) == 1: - return target_candidates[0] * utils.angle_conversion_factor("deg", unit) - else: - # Avoid over-180deg motion. - suitable = [ - angle for angle in target_candidates if (angle - current) <= 180 - ][0] - return suitable * utils.angle_conversion_factor("deg", unit) diff --git a/lib/device/utils/__init__.py b/lib/device/utils/__init__.py deleted file mode 100644 index 2145a467..00000000 --- a/lib/device/utils/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# flake8: noqa - -from .data_utils import * -from .math_utils import * -from .quantity_utils import * diff --git a/lib/device/utils/data_utils.py b/lib/device/utils/data_utils.py deleted file mode 100644 index 943fc321..00000000 --- a/lib/device/utils/data_utils.py +++ /dev/null @@ -1,9 +0,0 @@ -__all__ = ["update_list"] - -from typing import Any, List - - -def update_list(param: List[Any], new_value: Any) -> None: - """Drop old parameter and assign new one, preserving array length.""" - param.pop(0) - param.append(new_value) diff --git a/lib/device/utils/math_utils.py b/lib/device/utils/math_utils.py deleted file mode 100644 index 6ffb50c7..00000000 --- a/lib/device/utils/math_utils.py +++ /dev/null @@ -1,16 +0,0 @@ -__all__ = ["clip", "frange"] - -from typing import Generator - - -def clip(value: float, minimum: float, maximum: float) -> float: - """Limit the `value` to the range [`minimum`, `maximum`].""" - return min(max(minimum, value), maximum) - - -def frange(start: float, stop: float, step: float = 1) -> Generator[float, None, None]: - """Flexible range.""" - value = start - while value < stop: - yield value - value += step diff --git a/lib/device/utils/quantity_utils.py b/lib/device/utils/quantity_utils.py deleted file mode 100644 index a227b2d3..00000000 --- a/lib/device/utils/quantity_utils.py +++ /dev/null @@ -1,18 +0,0 @@ -__all__ = ["angle_conversion_factor"] - - -def angle_conversion_factor(from_: str, to: str) -> float: - """Unit conversion. - - More general implementation may be realized using astropy.units, but it's ~1000 - times slower than this thus can be a bottleneck in this time critical - calculation. - - """ - equivalents = {"deg": 1, "arcmin": 60, "arcsec": 3600} - try: - return equivalents[to] / equivalents[from_] - except KeyError: - raise ValueError( - "Units other than than 'deg', 'arcmin' or 'arcsec' are not supported." - ) diff --git a/poetry.lock b/poetry.lock index 1b050afa..8b879911 100644 --- a/poetry.lock +++ b/poetry.lock @@ -24,19 +24,19 @@ python-versions = "*" [[package]] name = "astropy" -version = "4.1" -description = "Astronomy and astrophysics core library" +version = "3.2.3" +description = "Community-developed python astronomy tools" category = "main" optional = false -python-versions = ">=3.6" +python-versions = ">=3.5" [package.dependencies] -numpy = ">=1.16" +numpy = ">=1.13" [package.extras] -all = ["scipy (>=0.18)", "dask", "h5py", "beautifulsoup4", "html5lib", "bleach", "PyYAML (>=3.12)", "pandas", "sortedcontainers", "pytz", "jplephem", "matplotlib (>=2.1)", "mpmath", "asdf (>=2.6)", "bottleneck", "ipython", "pytest"] -docs = ["sphinx-astropy (>=1.3)", "pytest", "PyYAML (>=3.12)", "scipy", "matplotlib"] -test = ["pytest-astropy (>=0.8)", "pytest-xdist", "objgraph", "ipython", "coverage", "skyfield (>=1.20)", "sgp4 (>=2.3)"] +all = ["scipy", "h5py", "beautifulsoup4", "html5lib", "bleach", "pyyaml", "pandas", "sortedcontainers", "pytz", "jplephem", "matplotlib (>=2.0)", "scikit-image", "mpmath", "asdf (>=2.3)", "bottleneck", "ipython", "pytest"] +docs = ["sphinx-astropy"] +test = ["pytest-astropy", "pytest-xdist", "pytest-mpl", "objgraph", "ipython", "coverage", "skyfield"] [[package]] name = "atomicwrites" @@ -218,7 +218,7 @@ python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*" name = "importlib-metadata" version = "4.5.0" description = "Read metadata from Python packages" -category = "dev" +category = "main" optional = false python-versions = ">=3.6" @@ -426,17 +426,33 @@ python-versions = "*" [[package]] name = "n-const" -version = "0.3.1" -description = "Parse and declare NANTEN2/NASCO specific constants/parameters." +version = "1.0.2" +description = "Necst Constants and ObservatioN Specification Translator." category = "main" optional = false python-versions = ">=3.6,<4.0" [package.dependencies] -astropy = ">=4.1,<5.0" +astropy = ">=3.0,<4.0" +dataclasses = {version = ">=0.8,<0.9", markers = "python_version >= \"3.6\" and python_version < \"3.7\""} +numpy = ">=1.19,<2.0" +tomlkit = ">=0.10,<0.11" +typing-extensions = {version = ">=3.0,<5.0", markers = "python_version < \"3.9\""} + +[[package]] +name = "neclib" +version = "0.2.0" +description = "Pure Python tools for NECST." +category = "main" +optional = false +python-versions = ">=3.6,<3.10" + +[package.dependencies] +astropy = ">=3.0,<4.0" dataclasses = {version = ">=0.8,<0.9", markers = "python_version >= \"3.6\" and python_version < \"3.7\""} +importlib-metadata = {version = ">=4.4,<5.0", markers = "python_version < \"3.8\""} numpy = ">=1.19,<2.0" -typing-extensions = {version = ">=3.10.0,<4.0.0", markers = "python_version < \"3.9\""} +typing-extensions = {version = ">=3.0,<5.0", markers = "python_version < \"3.8\""} [[package]] name = "nest-asyncio" @@ -818,6 +834,14 @@ category = "dev" optional = false python-versions = ">=2.6, !=3.0.*, !=3.1.*, !=3.2.*" +[[package]] +name = "tomlkit" +version = "0.10.0" +description = "Style preserving TOML library" +category = "main" +optional = false +python-versions = ">=3.6,<4.0" + [[package]] name = "tornado" version = "6.1" @@ -883,7 +907,7 @@ python-versions = "*" name = "zipp" version = "3.4.1" description = "Backport of pathlib-compatible object wrapper for zip files" -category = "dev" +category = "main" optional = false python-versions = ">=3.6" @@ -893,8 +917,8 @@ testing = ["pytest (>=4.6)", "pytest-checkdocs (>=1.2.3)", "pytest-flake8", "pyt [metadata] lock-version = "1.1" -python-versions = "^3.6" -content-hash = "e1a8c429ad024b1388b76c512e65711f9fffdb181aab0a736913b326dc750602" +python-versions = ">=3.6, <3.10" +content-hash = "5635010a9dc57d7ad160243586d275a46a1008f44d87926d4431e998f3384eaf" [metadata.files] alabaster = [ @@ -910,22 +934,30 @@ appnope = [ {file = "appnope-0.1.2.tar.gz", hash = "sha256:dd83cd4b5b460958838f6eb3000c660b1f9caf2a5b1de4264e941512f603258a"}, ] astropy = [ - {file = "astropy-4.1-cp36-cp36m-macosx_10_9_x86_64.whl", hash = "sha256:0804f601dddfd6b5b72e11eb6e58133ff7db25bb8c26851449352ba1f414b74c"}, - {file = "astropy-4.1-cp36-cp36m-manylinux1_i686.whl", hash = "sha256:85f199c41d28da78c0a09076904fe3c3034c0f4917d251a23ad364ed0e9ff2d5"}, - {file = "astropy-4.1-cp36-cp36m-manylinux1_x86_64.whl", hash = "sha256:6d6792e4e49f4662be075477f75aff7d3cc5bf5c58702d6b55f0048f14610c00"}, - {file = "astropy-4.1-cp36-cp36m-win32.whl", hash = "sha256:f0fe5387992e0c96d9186752b2ac6a5801a51af0316d75427823588af9be75da"}, - {file = "astropy-4.1-cp36-cp36m-win_amd64.whl", hash = "sha256:27544cb41ba54a60acaf195c53b4912edbbfa4b0ab005c06d5d929553e0d8329"}, - {file = "astropy-4.1-cp37-cp37m-macosx_10_9_x86_64.whl", hash = "sha256:4a70a44a9f773acbb75ba24bf36e4f3a8b702f37384d03f9ff9097edd8ad0890"}, - {file = "astropy-4.1-cp37-cp37m-manylinux1_i686.whl", hash = "sha256:83cfa44aa6cac0c434b238be6a758d49a33ab608aad7bb2f7410ce497c28312d"}, - {file = "astropy-4.1-cp37-cp37m-manylinux1_x86_64.whl", hash = "sha256:2fb991128b522eb1f133e6ca1c14425368bda8888eb387dba1abc18ee76582ee"}, - {file = "astropy-4.1-cp37-cp37m-win32.whl", hash = "sha256:aca1dbca1c9f6d6ba17682e44dc53ffd62dd0d13448d38713140d634ec45b295"}, - {file = "astropy-4.1-cp37-cp37m-win_amd64.whl", hash = "sha256:70c4b97d2ac5d19fad753d0a7dccc7b14e12786546e78ae27cae3a7a73ecd11d"}, - {file = "astropy-4.1-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:5083e1e8fbd065da521b761ee14382bb561d8bbe3957653b2f941690128f8940"}, - {file = "astropy-4.1-cp38-cp38-manylinux1_i686.whl", hash = "sha256:51c3a29c4252b1697fd950e2b333d84cb6baf8d9a5258bbef6d0089b65c303b3"}, - {file = "astropy-4.1-cp38-cp38-manylinux1_x86_64.whl", hash = "sha256:cd4390da737a5123397a1bb00cf597842bf21fd199862ba161d9c18cd2d12c14"}, - {file = "astropy-4.1-cp38-cp38-win32.whl", hash = "sha256:d51f3b9091004a3ed263bc2a51a677762a6c8c7437da5115d3149dc1de6f71bf"}, - {file = "astropy-4.1-cp38-cp38-win_amd64.whl", hash = "sha256:10e45a5456929b75b3a29d8174d39466b1823280bcf99f3b4c6c51e521a6f04d"}, - {file = "astropy-4.1.tar.gz", hash = "sha256:a3bad6e1c2619135ad33e4d80452866eb9468f610bf00754da4001c63c1f7049"}, + {file = "astropy-3.2.3-cp35-cp35m-macosx_10_6_intel.macosx_10_9_intel.macosx_10_9_x86_64.macosx_10_10_intel.macosx_10_10_x86_64.whl", hash = "sha256:cad36fd1a1fcb676a0be42b74c5d2f559d1cbb8116d08e100421b6809edeb339"}, + {file = "astropy-3.2.3-cp35-cp35m-macosx_10_6_intel.whl", hash = "sha256:c2c75bd2a74ecec5a1ea09c1cf442be4adb37bc5ad904e7c437409755677abdc"}, + {file = "astropy-3.2.3-cp35-cp35m-manylinux1_i686.whl", hash = "sha256:78636648183cb0efe14d887a2dddd9877048d76121ca221571322405c0f07eb9"}, + {file = "astropy-3.2.3-cp35-cp35m-manylinux1_x86_64.whl", hash = "sha256:cfc4f02f8780fe23ff1e057c16d6318ad643e1d811e5052383fc42d603a36e2e"}, + {file = "astropy-3.2.3-cp35-cp35m-win32.whl", hash = "sha256:57f3f6b21b8e3629a1bd436594e141e24b7ce0e6b26194e2872599391a8245e8"}, + {file = "astropy-3.2.3-cp35-cp35m-win_amd64.whl", hash = "sha256:2375c06ea92b56b8ee2d54b8930a8b28cf3d7532b200c94f05e44acaa6bd5037"}, + {file = "astropy-3.2.3-cp36-cp36m-macosx_10_6_intel.macosx_10_9_intel.macosx_10_9_x86_64.macosx_10_10_intel.macosx_10_10_x86_64.whl", hash = "sha256:3a75065e9194756ba1732aa725ea41ce46ec6389a851d35071940aa60e5fa4b4"}, + {file = "astropy-3.2.3-cp36-cp36m-macosx_10_6_intel.whl", hash = "sha256:af87375bca7325fadd8cacf15446abeae68181b58fbc5b92732dd02e08033df1"}, + {file = "astropy-3.2.3-cp36-cp36m-manylinux1_i686.whl", hash = "sha256:99cbddc61cfc061fdf73ef000d7c548264bdf131a8e00652ab7183e41265a7b8"}, + {file = "astropy-3.2.3-cp36-cp36m-manylinux1_x86_64.whl", hash = "sha256:cdd87113f16a5c07522dbb30898cf6ba6571cbf912efff06da37342851d08a9d"}, + {file = "astropy-3.2.3-cp36-cp36m-win32.whl", hash = "sha256:26205b678153faf2cfdd4dcf3fb3fab94b33cbda4f6158e9d9fbe33a6c788f3e"}, + {file = "astropy-3.2.3-cp36-cp36m-win_amd64.whl", hash = "sha256:eaaf8149e27bf247132cc330ac918e3d2a72c4b5c59077da7162a09bd9a4c6b5"}, + {file = "astropy-3.2.3-cp37-cp37m-macosx_10_6_intel.macosx_10_9_intel.macosx_10_9_x86_64.macosx_10_10_intel.macosx_10_10_x86_64.whl", hash = "sha256:4a40edb974ed4866e86d1af2d7a3399ef0c634af91a6dd4fe4ff68f7f4656c84"}, + {file = "astropy-3.2.3-cp37-cp37m-macosx_10_6_intel.whl", hash = "sha256:78f743020d63203cc723b2915a80d53f60416c1fcd54efba9538579bbf794d76"}, + {file = "astropy-3.2.3-cp37-cp37m-manylinux1_i686.whl", hash = "sha256:6a22733069d352309f2c9482599fede0a249037ae6d0cd83db6f501eec2ae3ac"}, + {file = "astropy-3.2.3-cp37-cp37m-manylinux1_x86_64.whl", hash = "sha256:f35a3eb0438c0d65fe73b8cd09ff1323cd265a05701049359a203fd8f9a08dbd"}, + {file = "astropy-3.2.3-cp37-cp37m-win32.whl", hash = "sha256:729c9b905562dd7866739fd52c819c1f9437dac8d83480e4cd1e86322110ed29"}, + {file = "astropy-3.2.3-cp37-cp37m-win_amd64.whl", hash = "sha256:1f0991019df0d9049143e6f63b773aeb9d16c632d2efb6761f0582a7afa59d7d"}, + {file = "astropy-3.2.3-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:8850958fb5cdcc4c40369541cd1be1c28c5f40c1f0b372dc9b3c799ca81ca6a4"}, + {file = "astropy-3.2.3-cp38-cp38-manylinux1_i686.whl", hash = "sha256:3de20dcb6ef676c851e27778d27bbb8d7f4e1a149ec3c5968dec87ce20ea3864"}, + {file = "astropy-3.2.3-cp38-cp38-manylinux1_x86_64.whl", hash = "sha256:4c03051504655bc83d1adf5a2772e1b1b1c4b49e1d54454dbc358d99eee88902"}, + {file = "astropy-3.2.3-cp38-cp38-win32.whl", hash = "sha256:efa49863064bbcb0651eb0f6af9bd439da482e4091764660475b8ff2bcd909b9"}, + {file = "astropy-3.2.3-cp38-cp38-win_amd64.whl", hash = "sha256:bbcaee3527bbc0460a7a46ce31873076312c9b0d9b84ef027b6313b0bb5c26ac"}, + {file = "astropy-3.2.3.tar.gz", hash = "sha256:47f00816c2978fdd10f448c8f0337d6dca7b8cbeaab4bf272b5fd37cb4b890d3"}, ] atomicwrites = [ {file = "atomicwrites-1.4.0-py2.py3-none-any.whl", hash = "sha256:6d1784dea7c0c8d4a5172b6c620f40b6e4cbfdf96d783691f2e1302a7b88e197"}, @@ -1179,8 +1211,12 @@ mypy-extensions = [ {file = "mypy_extensions-0.4.3.tar.gz", hash = "sha256:2d82818f5bb3e369420cb3c4060a7970edba416647068eb4c5343488a6c604a8"}, ] n-const = [ - {file = "N-CONST-0.3.1.tar.gz", hash = "sha256:8bddddde18458b22ea807cf3473225599817e971885dfb878c25f95375dc0a8c"}, - {file = "N_CONST-0.3.1-py3-none-any.whl", hash = "sha256:081014bee3e14fb181a85984bb725ccbd6e9ff2c19b63b178b5cadd3dc14855f"}, + {file = "N-CONST-1.0.2.tar.gz", hash = "sha256:dcc33eb5f37fa8392e5c605beefd975a403729b6ddea3742dba824716b13a5da"}, + {file = "N_CONST-1.0.2-py3-none-any.whl", hash = "sha256:0e2c4d7b7a8a77aee9262bf5226b21b9bf494882913eb1c36e81aafcdb9efc5a"}, +] +neclib = [ + {file = "neclib-0.2.0-py3-none-any.whl", hash = "sha256:9e026cd31dce5469140a43d8b02908060e4cd1e395524070413a5fabc3b43a00"}, + {file = "neclib-0.2.0.tar.gz", hash = "sha256:6445bfb0412799693415bff6fd85c46fa1ab70a88ad2c0e636227b10daad3165"}, ] nest-asyncio = [ {file = "nest_asyncio-1.5.1-py3-none-any.whl", hash = "sha256:76d6e972265063fe92a90b9cc4fb82616e07d586b346ed9d2c89a4187acea39c"}, @@ -1462,6 +1498,10 @@ toml = [ {file = "toml-0.10.2-py2.py3-none-any.whl", hash = "sha256:806143ae5bfb6a3c6e736a764057db0e6a0e05e338b5630894a5f779cabb4f9b"}, {file = "toml-0.10.2.tar.gz", hash = "sha256:b3bda1d108d5dd99f4a20d24d9c348e91c4db7ab1b749200bded2f839ccbe68f"}, ] +tomlkit = [ + {file = "tomlkit-0.10.0-py3-none-any.whl", hash = "sha256:cac4aeaff42f18fef6e07831c2c2689a51df76cf2ede07a6a4fa5fcb83558870"}, + {file = "tomlkit-0.10.0.tar.gz", hash = "sha256:d99946c6aed3387c98b89d91fb9edff8f901bf9255901081266a84fb5604adcd"}, +] tornado = [ {file = "tornado-6.1-cp35-cp35m-macosx_10_9_x86_64.whl", hash = "sha256:d371e811d6b156d82aa5f9a4e08b58debf97c302a35714f6f45e35139c332e32"}, {file = "tornado-6.1-cp35-cp35m-manylinux1_i686.whl", hash = "sha256:0d321a39c36e5f2c4ff12b4ed58d41390460f798422c4504e09eb5678e09998c"}, diff --git a/pyproject.toml b/pyproject.toml index 01fdc900..72673058 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -5,13 +5,13 @@ description = "NEw Control System for Telescope" authors = ["NANTEN2 Software Team"] [tool.poetry.dependencies] -python = "^3.6" -astropy = "^4.1" +python = ">=3.6, <3.10" +astropy = ">=3.0, <5.0" matplotlib = "^3.3" -n_const = "^0.3" +n_const = "^1.0.2" +neclib = "^0.2.0" numpy = "^1.19" -# nasco_system = { git="https://github.com/ogawa-ros/nasco_system.git", branch="master" } -typing-extensions = { version = "^3.10.0", python = "<3.8" } +typing-extensions = { version = "^3.10", python = "<3.8" } [tool.poetry.dev-dependencies] pytest = "^5.2" From b187ac1b80eef1693f765c7be699cca4c42feeb3 Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Thu, 17 Mar 2022 14:50:31 +0900 Subject: [PATCH 12/14] #491 Remove module imports --- lib/device/__init__.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/lib/device/__init__.py b/lib/device/__init__.py index 650d4b23..9c0fa90a 100644 --- a/lib/device/__init__.py +++ b/lib/device/__init__.py @@ -1,7 +1 @@ # flake8: noqa - -# Modules -from . import antenna_pid - -# Subpackages -from . import utils From 80253a78a1816c076789250b3dc6b06aed50eb62 Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Thu, 17 Mar 2022 15:01:23 +0900 Subject: [PATCH 13/14] #491 Fix attribute names --- lib/device/antenna_device.py | 63 ++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 32 deletions(-) diff --git a/lib/device/antenna_device.py b/lib/device/antenna_device.py index abfd3ea7..ec8d979f 100644 --- a/lib/device/antenna_device.py +++ b/lib/device/antenna_device.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python3 - import time from typing import Dict, Tuple @@ -8,7 +6,7 @@ except ImportError: from typing_extensions import Literal -from neclib import PIDController +from neclib import PIDController, optimum_angle # Indices for parameter lists. Last = -2 @@ -33,7 +31,7 @@ def __init__(self, board_model: int, rsw_id: int) -> None: self.dio = pyinterface.open(board_model, rsw_id) self.dio.initialize() - def command(self, value: int, azel: str) -> None: + def command(self, value: int, azel: Literal["az", "el"]) -> None: # Specify which pins to use. if azel.lower() == "az": target = "OUT1_16" @@ -87,17 +85,17 @@ class antenna_device: def __init__( self, board_model: int = 2724, rsw_id: int = 0, simulator: bool = False ) -> None: - self._az = PIDController.with_configuration( + self._az = PIDController( pid_param=[self.p_coeff[0], self.i_coeff[0], self.d_coeff[0]] ) - self._el = PIDController.with_configuration( + self._el = PIDController( pid_param=[self.p_coeff[1], self.i_coeff[1], self.d_coeff[1]] ) self.simulator = simulator if not self.simulator: self.driver = AntennaDriver(board_model, rsw_id) - def _command(self, value: int, azel: str) -> None: + def _command(self, value: int, azel: Literal["az", "el"]) -> None: if not self.simulator: self.driver.command(value, azel) @@ -108,9 +106,11 @@ def init_speed(self) -> None: self._command(0, "az") self._command(0, "el") - def set_pid_param(self, param: Dict[str, Tuple[float, float, float]]) -> None: - self._az.K_p, self._az.K_i, self._az.K_d = param["az"] - self._el.K_p, self._el.K_i, self._el.K_d = param["el"] + def set_pid_param( + self, param: Dict[Literal["az", "el"], Tuple[float, float, float]] + ) -> None: + self._az.k_p, self._az.k_i, self._az.k_d = param["az"] + self._el.k_p, self._el.k_i, self._el.k_d = param["el"] def move_azel( self, @@ -129,7 +129,7 @@ def move_azel( elif m_bStop == "TRUE": stop = True - az_arcsec = self._az.suitable_angle( + az_arcsec = optimum_angle( enc_az, az_arcsec, self.LIMITS, margin=40 * 3600, unit="arcsec" ) @@ -148,9 +148,9 @@ def move_azel( return ( self._az.cmd_speed[Now] * 3600, - self._az.K_p * self._az.error[Now] * 3600, - self._az.K_i * self._az.error_integral * 3600 * self._az.dt, - self._az.K_d + self._az.k_p * self._az.error[Now] * 3600, + self._az.k_i * self._az.error_integral * 3600 * self._az.dt, + self._az.k_d * (self._az.error[Now] - self._az.error[Last]) * 3600 / self._az.dt, @@ -160,9 +160,9 @@ def move_azel( self._az.error_integral * 3600, self._az.enc_coord[Last] * 3600, self._az.cmd_speed[Now] * 3600, - self._el.K_p * self._az.error[Now] * 3600, - self._el.K_i * self._az.error_integral * 3600 * self._az.dt, - self._el.K_d + self._el.k_p * self._az.error[Now] * 3600, + self._el.k_i * self._az.error_integral * 3600 * self._az.dt, + self._el.k_d * (self._az.error[Now] - self._az.error[Last]) * 3600 / self._az.dt, @@ -205,22 +205,21 @@ def calc_pid( issue. Please use `AntennaDevice.calc_pid`. """ - calculator = PIDController.with_configuration(pid_param=[p_coeff, i_coeff, d_coeff]) + calculator = PIDController(pid_param=[p_coeff, i_coeff, d_coeff]) # Set `Last` parameters. - calculator._update(calculator.time, t_past) - calculator._update(calculator.cmd_coord, pre_arcsec / 3600) - calculator._update(calculator.enc_coord, enc_before / 3600) - calculator._update(calculator.error, pre_hensa / 3600) + calculator.time.push(t_past) + calculator.cmd_coord.push(pre_arcsec / 3600) + calculator.enc_coord.push(enc_before / 3600) + calculator.error.push(pre_hensa / 3600) - # Set `Now` parameters. - calculator._update(calculator.time, t_now) - calculator._update(calculator.cmd_coord, target_arcsec / 3600) - calculator._update(calculator.enc_coord, encoder_arcsec / 3600) - calculator._update(calculator.error, (target_arcsec - encoder_arcsec) / 3600) - calculator._update(calculator.error_integral, ihensa / 3600) + # Set new parameters. + calculator.time.push(t_now) + calculator.cmd_coord.push(target_arcsec / 3600) + calculator.enc_coord.push(encoder_arcsec / 3600) + calculator.error.push((target_arcsec - encoder_arcsec) / 3600) - speed = calculator.calc_pid() + speed = calculator._calc_pid() error_diff = (calculator.error[Now] - calculator.error[Last]) / calculator.dt if abs(error_diff) * 3600 > 1: @@ -229,7 +228,7 @@ def calc_pid( return ( speed, calculator.error_integral, - calculator.K_p * calculator.error[Now], - calculator.K_i * calculator.error_integral * calculator.dt, - calculator.K_d * error_diff / calculator.dt, + calculator.k_p * calculator.error[Now], + calculator.k_i * calculator.error_integral * calculator.dt, + calculator.k_d * error_diff / calculator.dt, ) From 4f79af166dd095a4b53d22f23c7598a28199522b Mon Sep 17 00:00:00 2001 From: Kaoru Nishikawa Date: Thu, 17 Mar 2022 15:12:20 +0900 Subject: [PATCH 14/14] #491 Fix PIDController usage --- lib/device/antenna_device.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/lib/device/antenna_device.py b/lib/device/antenna_device.py index ec8d979f..bcc301e4 100644 --- a/lib/device/antenna_device.py +++ b/lib/device/antenna_device.py @@ -85,6 +85,7 @@ class antenna_device: def __init__( self, board_model: int = 2724, rsw_id: int = 0, simulator: bool = False ) -> None: + PIDController.ANGLE_UNIT = "arcsec" self._az = PIDController( pid_param=[self.p_coeff[0], self.i_coeff[0], self.d_coeff[0]] ) @@ -133,8 +134,8 @@ def move_azel( enc_az, az_arcsec, self.LIMITS, margin=40 * 3600, unit="arcsec" ) - speed_az = self._az.get_speed(az_arcsec, enc_az, stop=stop, unit="arcsec") - speed_el = self._el.get_speed(el_arcsec, enc_el, stop=stop, unit="arcsec") + speed_az = self._az.get_speed(az_arcsec, enc_az, stop=stop) + speed_el = self._el.get_speed(el_arcsec, enc_el, stop=stop) self._command(int(speed_az * self.SPEED2RATE), "az") self._command(int(speed_el * self.SPEED2RATE), "el")