diff --git a/src/servo_pkg/config/parent_config.yaml b/src/servo_pkg/config/parent_config.yaml new file mode 100644 index 00000000..2613ef47 --- /dev/null +++ b/src/servo_pkg/config/parent_config.yaml @@ -0,0 +1,23 @@ +Parent_config: + ros__parameters: + # this is the highest servo number + max_num_servo: 3 + servo_num: 1 + # Tilt + servo0: + name: "tilt" + min: 350.0 + max: 2500.0 + rom: 6.2832 + # PAN + servo1: + name: "pan" + min: 350.0 + max: 2500.0 + rom: 6.2832 + # GRIPPER + servo3: + name: "gripper" + min: 500.0 + max: 2500.0 + rom: 3.1415 diff --git a/src/servo_pkg/config/pi_controller.yaml b/src/servo_pkg/config/pi_controller.yaml index 120ab77d..d5205587 100644 --- a/src/servo_pkg/config/pi_controller.yaml +++ b/src/servo_pkg/config/pi_controller.yaml @@ -1,16 +1,19 @@ pi_Servo_node: ros__parameters: - servos_used: 2 + # this is the highest servo number + max_num_servo: 1 # Microscope Servo servo0: + name: "microscope" out_pin: 18 min: 2.5 # Min PWM value max: 12.5 # Max PWM value - rom: 180 # 180 degrees + rom: 3.1415 # 180 degrees # Collection Servo servo1: + name: "collection" out_pin: 19 min: 4.5 # Min PWM value max: 10.5 # Max PWM value - rom: 180 # 180 degrees \ No newline at end of file + rom: 3.1415 # 180 degrees \ No newline at end of file diff --git a/src/servo_pkg/config/servo_client.yaml b/src/servo_pkg/config/servo_client.yaml new file mode 100644 index 00000000..5c9e992c --- /dev/null +++ b/src/servo_pkg/config/servo_client.yaml @@ -0,0 +1,11 @@ +Servo_Client_node: + ros__parameters: + servo_num: 1 + servo0: + name: "tilt" + # PAN + servo1: + name: "pan" + # GRIPPER + servo3: + name: "gripper" \ No newline at end of file diff --git a/src/servo_pkg/config/usb_controller.yaml b/src/servo_pkg/config/usb_controller.yaml index ecb8e41a..61fb2882 100644 --- a/src/servo_pkg/config/usb_controller.yaml +++ b/src/servo_pkg/config/usb_controller.yaml @@ -1,18 +1,24 @@ USB_Servo_node: ros__parameters: + # this is the highest servo number + max_num_servo: 3 + servo_num: 1 serial_port: "/dev/serial/by-id/usb-Pololu_Corporation_Pololu_Mini_Maestro_12-Channel_USB_Servo_Controller_00460938-if00" # Tilt - port0: - min: 350 - max: 2500 - rom: 360 + servo0: + name: "tilt" + min: 350.0 + max: 2500.0 + rom: 6.2832 # PAN - port1: - min: 350 - max: 2500 - rom: 360 + servo1: + name: "pan" + min: 350.0 + max: 2500.0 + rom: 6.2832 # GRIPPER - port3: - min: 500 - max: 2500 - rom: 180 \ No newline at end of file + servo3: + name: "gripper" + min: 500.0 + max: 2500.0 + rom: 3.1415 \ No newline at end of file diff --git a/src/servo_pkg/launch/i2c_servo_launch.launch.py b/src/servo_pkg/launch/i2c_servo_launch.launch.py index e053e141..69c52aec 100644 --- a/src/servo_pkg/launch/i2c_servo_launch.launch.py +++ b/src/servo_pkg/launch/i2c_servo_launch.launch.py @@ -3,12 +3,15 @@ def generate_launch_description(): + parent_params = os.path.join(pkg_servo, "config", "parent_config.yaml") + return launch.LaunchDescription( [ launch_ros.actions.Node( package="servo_pkg", executable="i2c_Servo", name="USB_Servo_node", + parameters=[parent_params], ), launch_ros.actions.Node( package="servo_pkg", executable="servo_client", name="servo_client_node" diff --git a/src/servo_pkg/launch/pi_servo_launch.launch.py b/src/servo_pkg/launch/pi_servo_launch.launch.py index 46a921d1..4963c4fd 100644 --- a/src/servo_pkg/launch/pi_servo_launch.launch.py +++ b/src/servo_pkg/launch/pi_servo_launch.launch.py @@ -6,7 +6,8 @@ def generate_launch_description(): pkg_servo = get_package_share_directory("servo_pkg") - parameters_file = os.path.join(pkg_servo, "config", "pi_controller.yaml") + child_params = os.path.join(pkg_servo, "config", "pi_controller.yaml") + parent_params = os.path.join(pkg_servo, "config", "parent_config.yaml") return launch.LaunchDescription( [ @@ -14,7 +15,7 @@ def generate_launch_description(): package="servo_pkg", executable="pi_Servo", name="pi_Servo_node", - parameters=[parameters_file], + parameters=[parent_params, child_params], remappings=[("servo_service", "science_servo_service")], ), # for example client diff --git a/src/servo_pkg/launch/usb_servo_launch.launch.py b/src/servo_pkg/launch/usb_servo_launch.launch.py index eb8eeea2..9cf53288 100644 --- a/src/servo_pkg/launch/usb_servo_launch.launch.py +++ b/src/servo_pkg/launch/usb_servo_launch.launch.py @@ -6,7 +6,9 @@ def generate_launch_description(): pkg_servo = get_package_share_directory("servo_pkg") - parameters_file = os.path.join(pkg_servo, "config", "usb_controller.yaml") + child_params = os.path.join(pkg_servo, "config", "usb_controller.yaml") + parent_params = os.path.join(pkg_servo, "config", "parent_config.yaml") + client_params = os.path.join(pkg_servo, "config", "servo_client.yaml") return launch.LaunchDescription( [ @@ -14,13 +16,13 @@ def generate_launch_description(): package="servo_pkg", executable="USB_Servo", name="USB_Servo_node", - parameters=[parameters_file], + parameters=[parent_params, child_params], + ), + launch_ros.actions.Node( + package="servo_pkg", + executable="servo_client", + name="Servo_Client_node", + parameters=[client_params], ), - # for example client - # launch_ros.actions.Node( - # package="servo_pkg", - # executable="servo_client", - # name="servo_client_node", - # ), ] ) diff --git a/src/servo_pkg/servo_pkg/USB_Servo.py b/src/servo_pkg/servo_pkg/USB_Servo.py index edb9b670..8e457f18 100644 --- a/src/servo_pkg/servo_pkg/USB_Servo.py +++ b/src/servo_pkg/servo_pkg/USB_Servo.py @@ -1,121 +1,81 @@ import rclpy -from rclpy.node import Node -from interfaces.srv import MoveServo +from std_msgs.msg import Float32 from servo_pkg import maestro +from servo_pkg.parent_config import Parent_Config +from servo_pkg.parent_config import Servo_Info -NUM_PORTS = 12 -DEFAULT_MIN = 512 -DEFAULT_MAX = 2400 -DEFAULT_MAX_DEGREES = 180 - -class Servo_Info: - def __init__(self, port: int, min_us: int, max_us: int, max_deg: int): - self.port = port - self.min = min_us - self.max = max_us - self.rom = max_deg - - -def convert_from_degrees(degrees: int, servo_info: Servo_Info) -> int: +def convert_from_radians(angle, servo_info): total_range = servo_info.max - servo_info.min - return int(servo_info.min + (total_range * degrees / servo_info.rom)) + return int(servo_info.min + (total_range * angle / servo_info.rom)) -def convert_to_degrees(value: int, servo_info: Servo_Info) -> int: +def convert_to_radians(value, servo_info): total_range = servo_info.max - servo_info.min - return int(servo_info.rom * (value - servo_info.min) / total_range) + return servo_info.rom * (value - servo_info.min) / total_range -class USB_Servo(Node): +class USB_Servo(Parent_Config): def __init__(self): super().__init__("usb_servo") + # port parameter self.declare_parameter("serial_port", "/dev/ttyACM0") serial_port = ( self.get_parameter("serial_port").get_parameter_value().string_value ) - self.servo = maestro.Controller(serial_port) - self.srv = self.create_service(MoveServo, "servo_service", self.set_position) - - self.servo_ranges = {} - self.load_port_config() - - for port, servo in self.servo_ranges.items(): - self.servo.setRange(port, servo.min, servo.max) - - def load_port_config(self): - for port_number in range(NUM_PORTS): - self.declare_parameter(f"port{port_number}.min", DEFAULT_MIN) - min_us = ( - self.get_parameter(f"port{port_number}.min") - .get_parameter_value() - .integer_value - ) - self.declare_parameter(f"port{port_number}.max", DEFAULT_MAX) - max_us = ( - self.get_parameter(f"port{port_number}.max") - .get_parameter_value() - .integer_value - ) - self.declare_parameter(f"port{port_number}.rom", DEFAULT_MAX_DEGREES) - rom = ( - self.get_parameter(f"port{port_number}.rom") - .get_parameter_value() - .integer_value - ) - # Convert microseconds to quarter-microseconds - min_qus = min_us * 4 - max_qus = max_us * 4 + self.servo_controller = maestro.Controller(serial_port) + self.get_logger().info(f"{self.servo_info[self.servo_num].motor_name}") + self.sub = self.create_subscription( + Float32, + f"{self.servo_info[self.servo_num].motor_name}", + self.set_position, + 3, + ) - self.servo_ranges[port_number] = Servo_Info( - port_number, min_qus, max_qus, rom - ) - self.get_logger().info( - f"Port {port_number} -> Min: {min_qus}, Max: {max_qus}" - ) + self.set_range() + for port, servo in self.servo_info.items(): + self.servo_controller.setRange(port, servo.min, servo.max) - def set_position(self, request: MoveServo, response: MoveServo) -> MoveServo: - port = request.port - if port not in self.servo_ranges: - response.status = False - response.status_msg = f"Invalid port: {port}" - return response - - servo_info = self.servo_ranges[port] - - # Update range if explicitly given - if ( - request.min is not None - and request.min >= 0 - and request.min < request.max - and request.max is not None - and request.max > 0 - ): - servo_info.min = request.min - self.servo.setRange(port, servo_info.min, servo_info.max) - self.get_logger().info( - f"Updated range for port {port}: Min: {servo_info.min}, Max: {servo_info.max}" - ) + def set_range(self): + for port in self.servo_info: + # Convert microseconds to quarter-microseconds + min_qus = self.servo_info[port].min * 4 + max_qus = self.servo_info[port].max * 4 + self.get_logger().info(f"Port {port} -> Min: {min_qus}, Max: {max_qus}") + self.servo_controller.setRange(port, min_qus, max_qus) + + def set_position(self, msg): + port = self.servo_num + self.get_logger().info(f"Port is {port}") + self.check_valid_servo(port) + servo_info = self.servo_info[port] + total_range = servo_info.max - servo_info.min + self.get_logger().info(f"Float is {msg.data}") + self.get_logger().info(f"Total Range: {total_range}") + target_value = convert_from_radians(msg.data, servo_info) + self.get_logger().info(f"PWM target is {target_value}") + self.get_logger().info(f"Target value: {target_value}") + current_position = convert_to_radians( + self.servo_controller.getPosition(port), servo_info + ) - target_value = convert_from_degrees(request.pos, servo_info) - current_position = convert_to_degrees(self.servo.getPosition(port), servo_info) + self.get_logger().info(f"Total Range: {total_range}") + self.get_logger().info(f"Radian value: {current_position}") if not (servo_info.min <= target_value <= servo_info.max): - response.status = False - response.status_msg = f"Servo {port} input out of range.\nCurrent position: {current_position}" + self.get_logger().warning( + f"Servo {port} input out of range.\nCurrent position: {current_position}" + ) else: self.get_logger().debug( - f"Received request for port {port}: {request.pos} degrees -> {target_value}" + f"Received request for port {port}: {msg.data} angle -> {target_value}" ) - self.servo.setTarget(port, target_value) - response.status = True - current_position = convert_to_degrees( - self.servo.getPosition(port), servo_info + self.servo_controller.setTarget(port, target_value) + current_position = convert_to_radians( + self.servo_controller.getPosition(port), servo_info ) - response.status_msg = f"Servo {port} moved to: {current_position} degrees" - - return response + self.get_logger().info(f"Servo {port} moved to angle: {current_position}") def main(args=None): diff --git a/src/servo_pkg/servo_pkg/i2c_Servo.py b/src/servo_pkg/servo_pkg/i2c_Servo.py index 6473c6eb..f61b1bf1 100644 --- a/src/servo_pkg/servo_pkg/i2c_Servo.py +++ b/src/servo_pkg/servo_pkg/i2c_Servo.py @@ -1,41 +1,36 @@ import rclpy -import rclpy.logging -from rclpy.node import Node -import rclpy.time -from interfaces.srv import MoveServo - +from std_msgs.msg import Float32 +import math import board from adafruit_motor import servo from adafruit_pca9685 import PCA9685 +from servo_pkg.parent_config import Parent_Config -class i2c_Servo(Node): +class i2c_Servo(Parent_Config): def __init__(self): super().__init__("i2c_servo") - self.srv = self.create_service(MoveServo, "servo_service", self.set_position) + + self.sub = self.create_subscription( + Float32, f"servo{self.servo_num}.name", self.set_position, 3 + ) self.i2c = board.I2C() self.pca = PCA9685(self.i2c) self.pca.frequency = 50 - self.maxrom = 180 # max range of motion of the servo, default 180 - self.servo_list = [None] * 16 + self.maxrom = math.pi # max range of motion of the servo, default pi - def set_position(self, request, response) -> MoveServo: - if request.max != None: - self.maxrom = request.max - if self.servo_list[request.port - 1] == None: - self.servo_list[request.port] = servo.Servo( - self.pca.channels[request.port], actuation_range=self.maxrom + def set_position(self, msg): + if self.servo_list[self.servo_num - 1] == None: + self.servo_list[self.servo_num] = servo.Servo( + self.pca.channels[self.servo_num], actuation_range=self.maxrom ) - - s = self.servo_list[request.port] - s.angle = request.pos - - response.status = True - response.status_msg = f"Servo {request.port} moving to {request.pos} degrees" - - return response + cur_servo = self.servo_list[self.servo_num] + cur_servo.angle = msg.data + self.get_logger().info( + f"Servo {self.servo_num} moving to {cur_servo.angle} degrees" + ) def main(args=None): diff --git a/src/servo_pkg/servo_pkg/parent_config.py b/src/servo_pkg/servo_pkg/parent_config.py new file mode 100644 index 00000000..78475a3f --- /dev/null +++ b/src/servo_pkg/servo_pkg/parent_config.py @@ -0,0 +1,69 @@ +from rclpy.node import Node + +DEFAULT_MIN = 512.0 +DEFAULT_MAX = 2400.0 +DEFAULT_MAX_ANGLE = 3.1415 + + +class Servo_Info: + def __init__( + self, motor_name: str, min_pwm: float, max_pwm: float, max_angle: float + ): + self.motor_name = motor_name + self.min = min_pwm + self.max = max_pwm + self.rom = max_angle + + +# Parent class for all 3 types of servos +class Parent_Config(Node): + def __init__(self, name): + super().__init__(name) + self.servo_info = {} + self.load_config() + + # Set class attributes based on yaml config + def load_config(self): + self.declare_parameter("servo_num", 0) + self.servo_num = ( + self.get_parameter("servo_num").get_parameter_value().integer_value + ) + # This should be the highest number servo + self.declare_parameter("max_num_servo", 0) + self.max_num_servo = ( + self.get_parameter("max_num_servo").get_parameter_value().integer_value + ) + for servo in range(self.max_num_servo + 1): + self.declare_parameter(f"servo{servo}.name", f"{servo}") + motor_name = ( + self.get_parameter(f"servo{servo}.name") + .get_parameter_value() + .string_value + ) + self.declare_parameter(f"servo{servo}.min", DEFAULT_MIN) + min_pwm = ( + self.get_parameter(f"servo{servo}.min") + .get_parameter_value() + .double_value + ) + self.declare_parameter(f"servo{servo}.max", DEFAULT_MAX) + max_pwm = ( + self.get_parameter(f"servo{servo}.max") + .get_parameter_value() + .double_value + ) + self.declare_parameter(f"servo{servo}.rom", DEFAULT_MAX_ANGLE) + rom = ( + self.get_parameter(f"servo{servo}.rom") + .get_parameter_value() + .double_value + ) + self.servo_info[servo] = Servo_Info(motor_name, min_pwm, max_pwm, rom) + + def check_valid_servo(self, channel): + if self.max_num_servo < 0: + raise ValueError("Invalid max servo number") + if channel not in self.servo_info: + self.get_logger().error("Invalid servo") + return False + return True diff --git a/src/servo_pkg/servo_pkg/pi_Servo.py b/src/servo_pkg/servo_pkg/pi_Servo.py index fef0aac7..b4ee241a 100644 --- a/src/servo_pkg/servo_pkg/pi_Servo.py +++ b/src/servo_pkg/servo_pkg/pi_Servo.py @@ -1,11 +1,11 @@ import rclpy -from rclpy.node import Node -from interfaces.srv import MoveServo - from rpi_hardware_pwm import HardwarePWM +from servo_pkg.parent_config import Parent_Config +from std_msgs.msg import Float32 +from servo_pkg.parent_config import Servo_Info -def to_channel(pin: int) -> int: +def to_channel(pin): if pin == 18: return 0 elif pin == 19: @@ -13,110 +13,80 @@ def to_channel(pin: int) -> int: raise ValueError(f"Entered non PWM pin: {pin}") -class Servo: - def __init__( - self, channel: int, min_pos: float, max_pos: float, frequency: int, rom: int - ): +class pi_Servo_info: + def __init__(self, channel, servo_info, frequency): + self.servo_info = servo_info self.channel = channel - self.min_pos = min_pos - self.max_pos = max_pos self.frequency = frequency - self.rom = rom - self.pwm_pin = HardwarePWM(pwm_channel=self.channel, hz=self.frequency, chip=0) self.pwm_pin.start(0) - def set_position(self, degree: int): - if degree < 0 or degree > self.rom: - raise ValueError(f"Degree out of range: {degree}") + def set_position(self, angle: float): + if angle < 0 or angle > self.servo_info.rom: + raise ValueError(f"Angle out of range: {angle}") - duty_cycle = self.convert_to_pwm(degree) + duty_cycle = self.convert_to_pwm(angle) self.pwm_pin.change_duty_cycle(duty_cycle) - def convert_to_pwm(self, degree: int) -> float: - return float(degree / (self.rom / (self.max_pos - self.min_pos)) + self.min_pos) + def convert_to_pwm(self, angle): + return float( + angle / (self.servo_info.rom / (self.max_pos - self.min_pos)) + self.min_pos + ) def stop(self): self.pwm_pin.stop() -class pi_Servo(Node): +class pi_Servo(Parent_Config): def __init__(self): super().__init__("pi_servo") - self.srv = self.create_service(MoveServo, "servo_service", self.set_position) - self.servos = {} + self.get_logger().info(f"{self.servo_num}") + self.get_logger().info(self.servo_info[self.servo_num].motor_name) + self.sub = self.create_subscription( + Float32, + f"servo{self.servo_info[self.servo_num].motor_name}", + self.set_position, + 3, + ) + self.servo_list = {} self.load_params() def load_params(self): - self.declare_parameter("servos_used", 0) - num_servos = ( - self.get_parameter("servos_used").get_parameter_value().integer_value - ) - if num_servos <= 0: - self.get_logger().error("Invalid number of ports") - raise ValueError("Invalid number of ports") - - for i in range(num_servos): - self.declare_parameter(f"servo{i}.frequency", 50) - self.declare_parameter(f"servo{i}.min", 0.0) - self.declare_parameter(f"servo{i}.max", 100.0) - self.declare_parameter(f"servo{i}.rom", 180) - self.declare_parameter(f"servo{i}.out_pin", 0) + for servo in self.servo_info: + self.declare_parameter(f"servo{servo}.frequency", 50) frequency = ( - self.get_parameter(f"servo{i}.frequency") + self.get_parameter(f"servo{servo}.frequency") .get_parameter_value() .integer_value ) - min_pos = ( - self.get_parameter(f"servo{i}.min").get_parameter_value().double_value - ) - max_pos = ( - self.get_parameter(f"servo{i}.max").get_parameter_value().double_value - ) - rom = ( - self.get_parameter(f"servo{i}.rom").get_parameter_value().integer_value - ) + self.declare_parameter(f"servo{servo}.out_pin", 0) outpin = ( - self.get_parameter(f"servo{i}.out_pin") + self.get_parameter(f"servo{servo}.out_pin") .get_parameter_value() .integer_value ) if outpin < 0: - self.get_logger().error(f"Invalid pin number for port {i}") - raise ValueError(f"Invalid pin number for port {i}") + self.get_logger().error(f"Invalid pin number for port {servo}") + raise ValueError(f"Invalid pin number for port {servo}") - self.servos[i] = Servo( + self.servo_list[servo] = pi_Servo_info( channel=to_channel(outpin), - min_pos=min_pos, - max_pos=max_pos, + servo_info=self.servo_info[servo], frequency=frequency, - rom=rom, ) - def set_position(self, request, response) -> MoveServo: - port = request.port - if port not in self.servos: - response.status = False - response.status_msg = f"Invalid port: {port}" - self.get_logger().error(response.status_msg) - return response - - servo = self.servos[port] - degree = request.pos + def set_position(self, msg): + port = self.servo + servo = self.servo_info[port] + angle = msg.data try: - servo.set_position(degree) + servo.set_position(angle) except ValueError as e: - response.status = False - response.status_msg = str(e) self.get_logger().error(f"Error setting position {str(e)}") - return response - response.status = True - response.status_msg = f"Moved to {degree} degrees" - self.get_logger().info(response.status_msg) - return response + self.get_logger().info(f"Moved to angle: {angle}") def destroy_node(self): - for servo in self.servos.values(): + for servo in self.servo_info.values(): servo.stop() super().destroy_node() diff --git a/src/servo_pkg/servo_pkg/servo_client.py b/src/servo_pkg/servo_pkg/servo_client.py index b5dc6fb3..a31e083d 100644 --- a/src/servo_pkg/servo_pkg/servo_client.py +++ b/src/servo_pkg/servo_pkg/servo_client.py @@ -1,36 +1,47 @@ import rclpy from rclpy.node import Node -from interfaces.srv import MoveServo - +import rclpy.logging +import rclpy.time +from std_msgs.msg import Float32 import random +import math class Servo_Client(Node): def __init__(self): super().__init__("servo_Client") - self.declare_parameter("port", 0) - self.port = self.get_parameter("port").get_parameter_value().integer_value - self.get_logger().info(f"{self.port}") - - self.cli = self.create_client(MoveServo, "servo_service") - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info("service not available, waiting again...") - self.timer = self.create_timer(2, self.servo_tester) - def send_request(self, port: int, pos: int) -> MoveServo: - req = MoveServo.Request() - req.port = port - req.pos = pos - future = self.cli.call_async(req) - - def servo_request(self, req_port, req_pos) -> None: - Servo_Client.get_logger(self).info("Sending Request for: %s" % (req_pos)) - self.send_request(port=req_port, pos=req_pos) + self.declare_parameter("servo_num", 0) + self.servo_num = ( + self.get_parameter("servo_num").get_parameter_value().integer_value + ) + self.declare_parameter(f"servo{self.servo_num}.name", f"servo{self.servo_num}") + self.motor_name = ( + self.get_parameter(f"servo{self.servo_num}.name") + .get_parameter_value() + .string_value + ) + self.get_logger().info(f"motor name is {self.motor_name}") + + # publish angle with topic as motor name + self.pub = self.create_publisher(Float32, f"{self.motor_name}", 3) + timer_period = 0.5 + self.timer = self.create_timer(timer_period, self.servo_tester) + + def servo_pub(self, req_pos) -> None: + self.get_logger().info(f"Publishing: {req_pos} to {self.motor_name}") + self.pub.publish(req_pos) def servo_tester(self) -> None: - random_pos = random.randint(0, 180) - - self.servo_request(self.port, random_pos) + msg = Float32() + # random value + msg.data = random.uniform(0, math.pi) + # set value + # msg.data = math.pi / 4 + self.get_logger().info( + f"Sending position: {msg.data} to {self.motor_name}" + ) + self.servo_pub(msg) def main(args=None):