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 8f1551a..e98bccb 100644
--- a/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py
+++ b/software/ros_packages/ground_station/src/Framework/DriveSystems/RoverDriveSender.py
@@ -19,7 +19,7 @@ DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_dri
DRIVE_COMMAND_HERTZ = 15
-Y_AXIS_DEADBAND = 0.015
+Y_AXIS_DEADBAND = 0.025
X_AXIS_DEADBAND = 0.025
THROTTLE_MIN = 0.05
@@ -104,7 +104,6 @@ class LogitechJoystick(QtCore.QThread):
def __setup_controller(self):
for device in devices.gamepads:
- print device
if device.name == GAME_CONTROLLER_NAME:
self.gamepad = device
diff --git a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui
index 889c6df..2eb8b6a 100644
--- a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui
+++ b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui
@@ -85,8 +85,29 @@
+
+ 0
+
+
+ 0
+
+
+ 0
+
+
+ 0
+
+
+ 6
+
-
+
+ 0
+
+
+ 6
+
-
@@ -502,6 +523,13 @@
+ -
+
+
+ Qt::Horizontal
+
+
+
@@ -549,6 +577,13 @@
0
+ -
+
+
+ Qt::Vertical
+
+
+
-
@@ -565,55 +600,6 @@
0
-
-
-
-
-
- 17
- 75
- true
-
-
-
- Heading
-
-
- Qt::AlignCenter
-
-
-
- -
-
-
-
- 14
-
-
-
- 258°
-
-
- Qt::AlignCenter
-
-
-
- -
-
-
-
- 17
- 75
- true
-
-
-
- Next Goal
-
-
- Qt::AlignCenter
-
-
-
-
@@ -642,23 +628,137 @@
+ -
+
+
+
+ 17
+ 75
+ true
+
+
+
+ Heading
+
+
+ Qt::AlignCenter
+
+
+
+ -
+
+
+
+ 17
+ 75
+ true
+
+
+
+ Next Goal
+
+
+ Qt::AlignCenter
+
+
+
+ -
+
+
+
+ 14
+
+
+
+ 258°
+
+
+ Qt::AlignCenter
+
+
+
-
-
-
+
+
+ Qt::Vertical
+
+
- 275
- 275
+ 20
+ 40
-
-
+
+
+ -
+
+
-
+
+
+ Qt::Horizontal
+
+
+
+ 40
+ 20
+
+
+
+
+ -
+
+
+
+ 275
+ 275
+
+
+
+
+ 275
+ 275
+
+
+
+
+
+
+ true
+
+
+ Qt::AlignCenter
+
+
+
+ -
+
+
+ Qt::Horizontal
+
+
+
+ 40
+ 20
+
+
+
+
+
+
+ -
+
+
+ Qt::Vertical
-
- Qt::AlignCenter
+
+
+ 20
+ 40
+
-
+
@@ -687,7 +787,7 @@
- 6
+ 0
6
@@ -720,6 +820,9 @@
-
+
+ 0
+
-
@@ -749,6 +852,19 @@
+ -
+
+
+ Qt::Vertical
+
+
+
+ 20
+ 40
+
+
+
+
-
@@ -791,6 +907,19 @@
+ -
+
+
+ Qt::Vertical
+
+
+
+ 20
+ 40
+
+
+
+
-
@@ -810,6 +939,9 @@
-
+
+ 6
+
-
@@ -876,6 +1008,19 @@
+ -
+
+
+ Qt::Vertical
+
+
+
+ 20
+ 40
+
+
+
+
diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py
index 79cbf26..58cc126 100644
--- a/software/ros_packages/ground_station/src/ground_station.py
+++ b/software/ros_packages/ground_station/src/ground_station.py
@@ -105,7 +105,7 @@ class GroundStation(QtCore.QObject):
self.start_threads_signal.emit()
compass_image = PIL.Image.open("Resources/Images/compass.png").resize((300, 300)) # PIL.Image
- self.shared_objects["screens"]["right_screen"].compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image)))
+ self.shared_objects["screens"]["right_screen"].heading_compass_label.setPixmap(QtGui.QPixmap.fromImage(ImageQt(compass_image)))
def ___ros_master_running(self):
checker = ROSMasterChecker.ROSMasterChecker()
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 6ead8d7..036677a 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
@@ -25,6 +25,8 @@ UINT16_MAX = 65535
DEFAULT_HERTZ = 15
+WATCHDOG_TIMEOUT = 0.3
+
#####################################
# ControlCoordinator Class Definition
@@ -57,11 +59,20 @@ class DriveCoordinator(object):
self.right_bogie_publisher = rospy.Publisher(self.right_bogie_topic, DriveControlMessage, queue_size=1)
# Other Vars TODO: fix this later
- self.drive_commands = {
- "iris": DriveCommandMessage(),
- "ground_station": DriveCommandMessage()
+ self.drive_command_data = {
+ "iris": {
+ "message": DriveCommandMessage(),
+ "last_time": time()
+ },
+
+ "ground_station": {
+ "message": DriveCommandMessage(),
+ "last_time": time()
+ }
}
+ self.last_message_time = time()
+
# ########## Run the Class ##########
self.run()
@@ -79,12 +90,18 @@ class DriveCoordinator(object):
sleep(max(self.wait_time - time_diff, 0))
def process_drive_commands(self):
- if not self.drive_commands["iris"].ignore_drive_control:
- self.send_drive_control_command(self.drive_commands["iris"])
+ if not self.drive_command_data["iris"]["message"].ignore_drive_control:
+ self.send_drive_control_command(self.drive_command_data["iris"])
else:
- self.send_drive_control_command(self.drive_commands["ground_station"])
+ self.send_drive_control_command(self.drive_command_data["ground_station"])
+
+ def send_drive_control_command(self, drive_command_data):
+
+ if (time() - drive_command_data["last_time"]) > WATCHDOG_TIMEOUT:
+ drive_command = DriveCommandMessage()
+ else:
+ drive_command = drive_command_data["message"]
- def send_drive_control_command(self, drive_command):
rear_drive = DriveControlMessage()
left_drive = DriveControlMessage()
right_drive = DriveControlMessage()
@@ -118,12 +135,12 @@ class DriveCoordinator(object):
self.right_bogie_publisher.publish(right_drive)
def iris_drive_command_callback(self, drive_command):
- self.drive_commands["iris"] = drive_command
- return
+ self.drive_command_data["iris"]["message"] = drive_command
+ self.drive_command_data["iris"]["last_time"] = time()
def ground_station_drive_command_callback(self, drive_command):
- self.drive_commands["ground_station"] = drive_command
- return
+ self.drive_command_data["ground_station"]["message"] = drive_command
+ self.drive_command_data["ground_station"]["last_time"] = time()
if __name__ == '__main__':