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 @@
-
+