caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 18:21:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added watchdog for drive coordinator. Removed background from compass. Tuned some stuff on ground station drive sender.

Browse Source
This commit is contained in:
Corwin Perren
2018-03-01 19:50:45 -08:00
parent 6d9ed5d438
commit 2a37f6fe2e
4 changed files with 234 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py
View File

@@ -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__':