def change_position_incregular(self, stop, direction, increase=2):
"""Method to changing position to the given direction as regularly and incremental when the stop flag is not triggered yet.
Args:
stop: The motion interrupt flag.
direction: The rotation way of servo motor. True is for clockwise, false is for can't clockwise.
increase: Increase amount of the collimator angle.
"""
timeout = 0
while not stop():
if timeout >= 5: # 5 * 0.2 millisecond is equal to 1 second.
break
elif timeout >= 1:
time.sleep(0.2)
if direction(): # true direction is the left way
target_angle = round(self.current_angle + increase, 1)
else:
target_angle = round(self.current_angle - increase, 1)
self.softly_goto_position(degree_to_radian(target_angle), 10)
timeout += 1
def __init__(self, joint, use_ext_driver=None):
"""Initialization method of :class:`t_system.motion.arm.Joint` class.
Args:
joint (dict): The requested_data that is contain joint's properties from the config file.
use_ext_driver (bool): The flag of external PWM driver activation.
"""
self.number = joint['joint_number']
self.is_reverse = joint['reverse']
self.motor = None
self.motor_thread_stop = None
self.motor_thread_direction = None
self.motor_thread = None
self.structure = joint['structure']
self.rotation_type = joint['rotation_type']
if self.structure == 'revolute':
self.max_q = joint['max_q']
self.min_q = joint['min_q']
elif self.structure == 'prismatic':
self.max_d = joint['max_d']
self.min_d = joint['min_d']
self.d = joint['init_d']
self.q = joint['init_q']
self.a = joint['a']
self.alpha = joint['alpha']
self.use_ext_driver = use_ext_driver
self.current_angle = degree_to_radian(self.q)
if self.is_reverse:
self.current_angle = pi - self.current_angle
if self.structure != 'constant':
if self.use_ext_driver:
self.motor = ExtServoMotor(joint['channel'])
self.motor.start(round(self.current_angle, 4))
else:
self.motor = ServoMotor(joint['motor_gpio_pin'])
self.motor.start(round(self.current_angle, 4))
self.motor_thread_stop = None
self.motor_thread_direction = None
self.motor_thread = threading.Thread(
target=self.motor.change_position_incregular,
args=(lambda: self.motor_thread_stop,
lambda: self.motor_thread_direction))
logger.info(
f'Joint{self.number} started successfully. As {self.structure}, in {self.rotation_type} rotation type, on {round(self.current_angle,4)} radian.'
)
def change_angle_by(self, delta_angle, direction):
"""The top-level method to provide servo motors moving.
Args:
delta_angle (float): Angle to rotate. In degree.
direction (bool): Rotate direction. True means CW, otherwise CCW.
"""
self.move_to_angle(
self.calc_target_angle(degree_to_radian(delta_angle), direction))
def move(self, angle, max_angle=180.0):
"""The top-level method to provide servo motors moving via given angles.
Args:
angle: Servo motor's angle. Between 0 - 180 Degree.
max_angle: Servo motor's upper edge of the angle range.
"""
if angle <= max_angle:
angle = degree_to_radian(angle)
self.motor.directly_goto_position(angle)
self.current_angle = angle
def change_angle_by(self, delta_angle, divide_count, delay, direction):
"""The top-level method to provide servo motors moving.
Args:
delta_angle (float): Angle to rotate. In degree.
divide_count (int): The count that specify motor how many steps will use.
delay (float): delay time between motor steps.
direction (bool): Rotate direction. True means CW, otherwise CCW.
"""
target_angle = round(
self.__calc_target_angle(degree_to_radian(delta_angle), direction),
5)
self.move_to_angle(target_angle, divide_count, delay)
self.current_angle = target_angle
def move(self, direction=True, delta_angle=2, divide_count=1):
"""The top-level method to provide servo motors moving by given angle difference.
Args:
direction: Direction of the moving.
delta_angle: angular difference amount of the collimator.
divide_count (int): The count that specify motor how many steps will use.
"""
target_angle = round(
self.__calc_target_angle(degree_to_radian(delta_angle), direction),
5)
self.motor.softly_goto_position(target_angle, divide_count)
self.current_angle = target_angle
def __init__(self, joint):
"""Initialization method of :class:`t_system.motion.arm.Joint` class.
Args:
joint (dict): The requested_data that is contain joint's properties from the config file.
"""
self.number = joint['joint_number']
self.is_reverse = joint['reverse']
self.motor = None
self.structure = joint['structure']
self.rotation_type = joint['rotation_type']
if self.structure == 'revolute':
self.max_q = joint['max_q']
self.min_q = joint['min_q']
elif self.structure == 'prismatic':
self.max_d = joint['max_d']
self.min_d = joint['min_d']
self.d = joint['init_d']
self.q = joint['init_q']
self.a = joint['a']
self.alpha = joint['alpha']
self.current_angle = degree_to_radian(self.q)
if self.is_reverse:
self.current_angle = pi - self.current_angle
if self.structure != 'constant':
self.motor = ServoMotor(joint['motor_gpio_pin'])
self.motor.start(round(self.current_angle, 4))
logger.info(
f'Joint{self.number} started successfully. As {self.structure}, in {self.rotation_type} rotation type, on {round(self.current_angle,4)} radian.'
)