From 303099fc07d7cb4bb9b6b059256e7e3eb7fe2fe5 Mon Sep 17 00:00:00 2001 From: Josh Newans Date: Sat, 7 May 2022 15:35:26 +1000 Subject: [PATCH] Improve driver functionality --- serial_motor_demo/serial_motor_demo/driver.py | 145 ++++++++++++++++-- serial_motor_demo_msgs/CMakeLists.txt | 2 + serial_motor_demo_msgs/msg/EncoderVals.msg | 2 + 3 files changed, 132 insertions(+), 17 deletions(-) create mode 100644 serial_motor_demo_msgs/msg/EncoderVals.msg diff --git a/serial_motor_demo/serial_motor_demo/driver.py b/serial_motor_demo/serial_motor_demo/driver.py index d7a6606..6b2018a 100644 --- a/serial_motor_demo/serial_motor_demo/driver.py +++ b/serial_motor_demo/serial_motor_demo/driver.py @@ -1,9 +1,12 @@ -from matplotlib.pyplot import title import rclpy from rclpy.node import Node from serial_motor_demo_msgs.msg import MotorCommand +from serial_motor_demo_msgs.msg import MotorVels +from serial_motor_demo_msgs.msg import EncoderVals import time import math +import serial +from threading import Lock @@ -12,26 +15,84 @@ class MotorDriver(Node): def __init__(self): super().__init__('motor_driver') - self.subscription = self.create_subscription( - MotorCommand, - 'motor_command', - self.motor_command_callback, - 10) + + # Setup parameters self.declare_parameter('encoder_cpr', value=0) if (self.get_parameter('encoder_cpr').value == 0): print("WARNING! ENCODER CPR SET TO 0!!") + self.declare_parameter('loop_rate', value=0) if (self.get_parameter('loop_rate').value == 0): print("WARNING! LOOP RATE SET TO 0!!") + + self.declare_parameter('serial_port', value="/dev/ttyUSB0") + self.serial_port = self.get_parameter('serial_port').value + + + self.declare_parameter('baud_rate', value=57600) + self.baud_rate = self.get_parameter('baud_rate').value + + + self.declare_parameter('serial_debug', value=False) + self.debug_serial_cmds = self.get_parameter('serial_debug').value + if (self.debug_serial_cmds): + print("Serial debug enabled") + + + + # Setup topics & services + + self.subscription = self.create_subscription( + MotorCommand, + 'motor_command', + self.motor_command_callback, + 10) + + self.speed_pub = self.create_publisher(MotorVels, 'motor_vels', 10) + + self.encoder_pub = self.create_publisher(EncoderVals, 'encoder_vals', 10) + + + # Member Variables + + self.last_enc_read_time = time.time() + self.last_m1_enc = 0 + self.last_m2_enc = 0 + self.m1_spd = 0.0 + self.m2_spd = 0.0 + + self.mutex = Lock() + + + # Open serial comms + + print(f"Connecting to port {self.serial_port} at {self.baud_rate}.") + self.conn = serial.Serial(self.serial_port, self.baud_rate, timeout=1.0) + print(f"Connected to {self.conn}") + + + + + + # Raw serial commands def send_pwm_motor_command(self, mot_1_pwm, mot_2_pwm): - self.send_command(f"o {mot_1_pwm} {mot_2_pwm}") + self.send_command(f"o {int(mot_1_pwm)} {int(mot_2_pwm)}") def send_feedback_motor_command(self, mot_1_ct_per_loop, mot_2_ct_per_loop): - self.send_command(f"m {mot_1_ct_per_loop} {mot_2_ct_per_loop}") + self.send_command(f"m {int(mot_1_ct_per_loop)} {int(mot_2_ct_per_loop)}") + + def send_encoder_read_command(self): + resp = self.send_command(f"e") + if resp: + return [int(raw_enc) for raw_enc in resp.split()] + return [] + + + # More user-friendly functions def motor_command_callback(self, motor_command): if (motor_command.is_pwm): @@ -43,12 +104,67 @@ def motor_command_callback(self, motor_command): mot2_ct_per_loop = motor_command.mot_2_req_rad_sec * scaler self.send_feedback_motor_command(mot1_ct_per_loop, mot2_ct_per_loop) - def send_command(self, cmd_string): - print("Sent: " + cmd_string) + def check_encoders(self): + resp = self.send_encoder_read_command() + if (resp): + + new_time = time.time() + time_diff = new_time - self.last_enc_read_time + self.last_enc_read_time = new_time + + m1_diff = resp[0] - self.last_m1_enc + self.last_m1_enc = resp[0] + m2_diff = resp[1] - self.last_m2_enc + self.last_m2_enc = resp[1] + rads_per_ct = 2*math.pi/self.get_parameter('encoder_cpr').value + self.m1_spd = m1_diff*rads_per_ct/time_diff + self.m2_spd = m2_diff*rads_per_ct/time_diff + spd_msg = MotorVels() + spd_msg.mot_1_rad_sec = self.m1_spd + spd_msg.mot_2_rad_sec = self.m2_spd + self.speed_pub.publish(spd_msg) + enc_msg = EncoderVals() + enc_msg.mot_1_enc_val = self.last_m1_enc + enc_msg.mot_2_enc_val = self.last_m2_enc + self.encoder_pub.publish(enc_msg) + + + + # Utility functions + + def send_command(self, cmd_string): + self.mutex.acquire() + try: + cmd_string += "\r" + self.conn.write(cmd_string.encode("utf-8")) + if (self.debug_serial_cmds): + print("Sent: " + cmd_string) + + ## Adapted from original + c = '' + value = '' + while c != '\r': + c = self.conn.read(1).decode("utf-8") + if (c == ''): + print("Error: Serial timeout on command: " + cmd_string) + return '' + value += c + + value = value.strip('\r') + + if (self.debug_serial_cmds): + print("Received: " + value) + return value + finally: + self.mutex.release() + + def close_conn(self): + self.conn.close() + def main(args=None): @@ -57,18 +173,13 @@ def main(args=None): motor_driver = MotorDriver() - rate = motor_driver.create_rate(2) while rclpy.ok(): rclpy.spin_once(motor_driver) - - # motor_gui.update_image() - - # cv2.waitKey(2) - time.sleep(0.01) + motor_driver.check_encoders() - # tk.mainloop() + motor_driver.close_conn() motor_driver.destroy_node() rclpy.shutdown() diff --git a/serial_motor_demo_msgs/CMakeLists.txt b/serial_motor_demo_msgs/CMakeLists.txt index 4df6eaa..f16d50d 100644 --- a/serial_motor_demo_msgs/CMakeLists.txt +++ b/serial_motor_demo_msgs/CMakeLists.txt @@ -41,6 +41,8 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCommand.msg" + "msg/MotorVels.msg" + "msg/EncoderVals.msg" ) diff --git a/serial_motor_demo_msgs/msg/EncoderVals.msg b/serial_motor_demo_msgs/msg/EncoderVals.msg new file mode 100644 index 0000000..cb625a2 --- /dev/null +++ b/serial_motor_demo_msgs/msg/EncoderVals.msg @@ -0,0 +1,2 @@ +int32 mot_1_enc_val +int32 mot_2_enc_val \ No newline at end of file