From 8b83e846778f9bf9a9195f5b8f3fd4f0797e50ff Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Sun, 4 Mar 2018 16:27:24 -0800 Subject: [PATCH] Some more modifications to drive sender so things are a little smoother. Works well now. Changed update rates for control systems. --- .../scripts/ground_station_launch.sh | 2 +- .../DriveSystems/RoverDriveSender.py | 4 ++-- .../control_coordinators/drive_coordinator.py | 4 ++-- .../src/drive_control/drive_control.py | 21 +++++++++++++++---- .../rover_main/launch/ground_station.launch | 2 +- .../rover_main/launch/rover/control.launch | 2 +- 6 files changed, 24 insertions(+), 11 deletions(-) diff --git a/software/ros_packages/ground_station/scripts/ground_station_launch.sh b/software/ros_packages/ground_station/scripts/ground_station_launch.sh index ba27a66..d5ceac8 100755 --- a/software/ros_packages/ground_station/scripts/ground_station_launch.sh +++ b/software/ros_packages/ground_station/scripts/ground_station_launch.sh @@ -11,7 +11,7 @@ launch_dir_length_without_current_folder=$(($launch_dir_length-$current_folder_n script_launch_path="${launch_dir:0:$launch_dir_length_without_current_folder}/src" cd $script_launch_path -sleep 5 +sleep 3 export DISPLAY=:0 python ground_station.py \ No newline at end of file diff --git a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py b/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py index e98bccb..eebfd87 100644 --- a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py +++ b/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py @@ -19,8 +19,8 @@ DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_dri DRIVE_COMMAND_HERTZ = 15 -Y_AXIS_DEADBAND = 0.025 -X_AXIS_DEADBAND = 0.025 +Y_AXIS_DEADBAND = 0.05 +X_AXIS_DEADBAND = 0.05 THROTTLE_MIN = 0.05 diff --git a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py index 44587f3..3d37fbb 100755 --- a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py +++ b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py @@ -23,7 +23,7 @@ DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right" UINT16_MAX = 65535 -DEFAULT_HERTZ = 20 +DEFAULT_HERTZ = 30 WATCHDOG_TIMEOUT = 0.3 @@ -136,7 +136,7 @@ class DriveCoordinator(object): def iris_drive_command_callback(self, drive_command): self.drive_command_data["iris"]["message"] = drive_command - self.drive_command_data["iris"]["last_time"] = time() + self.drive_command_data["iris"]["last_time"] = time() def ground_station_drive_command_callback(self, drive_command): self.drive_command_data["ground_station"]["message"] = drive_command diff --git a/software/ros_packages/rover_control/src/drive_control/drive_control.py b/software/ros_packages/rover_control/src/drive_control/drive_control.py index 51386d6..8879f4f 100755 --- a/software/ros_packages/rover_control/src/drive_control/drive_control.py +++ b/software/ros_packages/rover_control/src/drive_control/drive_control.py @@ -29,11 +29,13 @@ DEFAULT_DRIVE_CONTROL_STATUS_TOPIC = "drive_status/rear" FIRST_MOTOR_ID = 1 SECOND_MOTOR_ID = 2 -COMMUNICATIONS_TIMEOUT = 0.15 # Seconds +COMMUNICATIONS_TIMEOUT = 0.01 # Seconds RX_DELAY = 0.01 TX_DELAY = 0.01 +DEFAULT_HERTZ = 20 + MODBUS_REGISTERS = { "DIRECTION": 0, "SPEED": 1, @@ -71,6 +73,8 @@ class DriveControl(object): self.drive_control_status_topic = rospy.get_param("~drive_control_status_topic", DEFAULT_DRIVE_CONTROL_STATUS_TOPIC) + self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) + self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID) self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID) self.__setup_minimalmodbus_for_485() @@ -98,9 +102,18 @@ class DriveControl(object): def run(self): while not rospy.is_shutdown(): - self.send_drive_control_message() - self.get_drive_status() - sleep(0.005) + start_time = time() + + try: + self.send_drive_control_message() + self.get_drive_status() + + except Exception, error: + print "Error occurred:", error + + time_diff = time() - start_time + + sleep(max(self.wait_time - time_diff, 0)) def send_drive_control_message(self): if self.new_control_message: diff --git a/software/ros_packages/rover_main/launch/ground_station.launch b/software/ros_packages/rover_main/launch/ground_station.launch index e88ca62..beb98e8 100644 --- a/software/ros_packages/rover_main/launch/ground_station.launch +++ b/software/ros_packages/rover_main/launch/ground_station.launch @@ -4,5 +4,5 @@ - + \ No newline at end of file diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index 540ee4f..b7feb7c 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -2,7 +2,7 @@ - +