caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-09 02:31:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Got pan/tilt and tower combo node done. Other misc changes.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-14 20:54:50 -07:00
parent de93764849
commit 1b7f5437b9
10 changed files with 271 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
software/ros_packages/rover_control/src/drive_control/drive_control.py
View File

@@ -119,7 +119,7 @@ class DriveControl(object):
self.get_drive_status()
except Exception, error:
print "Error occurred:", error
pass
if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT:
print "Bogie not seen for", BOGIE_LAST_SEEN_TIMEOUT, "seconds. Exiting."

9
software/ros_packages/rover_control/src/iris_controller/iris_controller.py
View File

@@ -70,6 +70,8 @@ REGISTER_STATE_MAPPING = {
"DRIVE_VS_ARM": "SE_SWITCH"
}
IRIS_LAST_SEEN_TIMEOUT = 1 # seconds
#####################################
# IrisController Class Definition
@@ -98,6 +100,8 @@ class IrisController(object):
self.iris_connected = False
self.iris_last_seen_time = time()
self.run()
def __setup_minimalmodbus_for_485(self):
@@ -114,11 +118,16 @@ class IrisController(object):
self.broadcast_drive_if_current_mode()
self.broadcast_arm_if_current_mode()
self.broadcast_iris_status()
self.iris_last_seen_time = time()
except Exception, error:
print "Error occurred:", error
return
if (time() - self.iris_last_seen_time) > IRIS_LAST_SEEN_TIMEOUT:
print "Iris not seen for", IRIS_LAST_SEEN_TIMEOUT, "seconds. Exiting."
return # Exit so respawn can take over
time_diff = time() - start_time
sleep(max(self.wait_time - time_diff, 0))

145
software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py
View File

@@ -10,6 +10,8 @@ from time import time, sleep
import serial.rs485
import minimalmodbus
from std_msgs.msg import UInt8, UInt16
# Custom Imports
from rover_control.msg import TowerPanTiltControlMessage
@@ -23,18 +25,21 @@ DEFAULT_BAUD = 115200
DEFAULT_INVERT = False
DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "pan_tilt/control"
DEFAULT_TOWER_LIGHT_CONTROL_TOPIC = "tower/light/control"
DEFAULT_TOWER_CO2_STATUS_TOPIC = "tower/status/co2"
DEFAULT_PAN_TILT_CONTROL_TOPIC = "pan_tilt/control"
NODE_ID = 2
TOWER_NODE_ID = 1
PAN_TILT_NODE_ID = 2
COMMUNICATIONS_TIMEOUT = 0.01 # Seconds
COMMUNICATIONS_TIMEOUT = 0.02 # Seconds
RX_DELAY = 0.01
TX_DELAY = 0.01
RX_DELAY = 0.02
TX_DELAY = 0.02
DEFAULT_HERTZ = 20
MODBUS_REGISTERS = {
PAN_TILT_MODBUS_REGISTERS = {
"CENTER_ALL": 0,
"PAN_ADJUST_POSITIVE": 1,
@@ -43,15 +48,32 @@ MODBUS_REGISTERS = {
"TILT_ADJUST_NEGATIVE": 4
}
TOWER_MODBUS_REGISTERS = {
"LED_CONTROL": 0,
"CO2_READING_PPM": 1
}
PAN_TILT_CONTROL_DEFAULT_MESSAGE = [
0, # No centering
0, # No pan positive adjustment
0, # No pan negative adjustment
0, # No tilt positive adjustment
0 # No tilt negative adjustement
0 # No tilt negative adjustement
]
PAN_TILT_LAST_SEEN_TIMEOUT = 2 # seconds
TOWER_LIGHT_STATES = {
"NO_CHANGE": 0,
"LIGHT_OFF": 1,
"LIGHT_FLASH": 2,
"LIGHT_MED": 3,
"LIGHT_HIGH": 4
}
TOWER_CONTROL_DEFAULT_MESSAGE = [
TOWER_LIGHT_STATES["LIGHT_OFF"] # Light off
]
NODE_LAST_SEEN_TIMEOUT = 2 # seconds
#####################################
@@ -64,23 +86,40 @@ class TowerPanTiltControl(object):
self.port = rospy.get_param("~port", DEFAULT_PORT)
self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
self.node_id = rospy.get_param("~node_id", NODE_ID)
self.tower_node_id = rospy.get_param("~tower_node_id", TOWER_NODE_ID)
self.pan_tilt_node_id = rospy.get_param("~pan_tilt_node_id", PAN_TILT_NODE_ID)
self.pan_tilt_control_subscriber_topic = rospy.get_param("~pan_tilt_control_topic", DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC)
self.tower_light_control_subscriber_topic = rospy.get_param("~tower_light_control_topic",
DEFAULT_TOWER_LIGHT_CONTROL_TOPIC)
self.pan_tilt_control_subscriber_topic = rospy.get_param("~pan_tilt_control_topic",
DEFAULT_PAN_TILT_CONTROL_TOPIC)
self.tower_co2_publisher_topic = rospy.get_param("~tower_co2_status_topic",
DEFAULT_TOWER_CO2_STATUS_TOPIC)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.pan_tilt_node = None
self.tower_node = None
self.connect_to_pan_tilt()
self.connect_to_pan_tilt_and_tower()
self.pan_tilt_control_subscriber = \
rospy.Subscriber(self.pan_tilt_control_subscriber_topic, TowerPanTiltControlMessage, self.pan_tilt_control_callback)
self.pan_tilt_control_subscriber = rospy.Subscriber(self.pan_tilt_control_subscriber_topic,
TowerPanTiltControlMessage,
self.pan_tilt_control_callback)
self.tower_light_control_subscriber = rospy.Subscriber(self.tower_light_control_subscriber_topic, UInt8,
self.tower_light_control_callback)
self.tower_co2_publisher = rospy.Publisher(self.tower_co2_publisher_topic, UInt16, queue_size=1)
self.pan_tilt_control_message = None
self.new_control_message = False
self.tower_light_control_message = None
self.pan_tilt_last_seen = time()
self.new_pan_tilt_control_message = False
self.new_tower_light_control_message = False
self.modbus_nodes_seen_time = time()
self.run()
@@ -90,63 +129,99 @@ class TowerPanTiltControl(object):
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
self.tower_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
self.tower_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
def run(self):
self.send_startup_centering_command()
self.send_startup_centering_and_lights_off_command()
while not rospy.is_shutdown():
start_time = time()
try:
self.send_pan_tilt_control_message()
self.pan_tilt_last_seen = time()
self.modbus_nodes_seen_time = time()
except Exception, error:
print "Error occurred:", error
pass
# print "Error occurred:", error
if (time() - self.pan_tilt_last_seen) > PAN_TILT_LAST_SEEN_TIMEOUT:
print "Tower pan/tilt not seen for", PAN_TILT_LAST_SEEN_TIMEOUT, "seconds. Exiting."
try:
self.send_tower_control_message()
self.broadcast_co2_reading_message()
self.modbus_nodes_seen_time = time()
except Exception, error:
pass
# print "Error occurred:", error
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Tower pan/tilt not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting."
return # Exit so respawn can take over
time_diff = time() - start_time
sleep(max(self.wait_time - time_diff, 0))
def connect_to_pan_tilt(self):
self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.node_id))
def connect_to_pan_tilt_and_tower(self):
self.tower_node = minimalmodbus.Instrument(self.port, int(self.tower_node_id))
self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.pan_tilt_node_id))
self.__setup_minimalmodbus_for_485()
def send_startup_centering_command(self):
registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
registers[MODBUS_REGISTERS["CENTER_ALL"]] = 1
self.pan_tilt_node.write_registers(0, registers)
def send_startup_centering_and_lights_off_command(self):
try:
registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = 1
self.pan_tilt_node.write_registers(0, registers)
self.tower_node.write_register(0, TOWER_LIGHT_STATES["LIGHT_OFF"])
except:
pass
def send_pan_tilt_control_message(self):
if self.new_control_message:
if self.new_pan_tilt_control_message:
pan_tilt_control_message = self.pan_tilt_control_message # type: TowerPanTiltControlMessage
registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
registers[MODBUS_REGISTERS["CENTER_ALL"]] = int(pan_tilt_control_message.should_center)
registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = int(pan_tilt_control_message.should_center)
if pan_tilt_control_message.relative_pan_adjustment >= 0:
registers[MODBUS_REGISTERS["PAN_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_pan_adjustment
registers[
PAN_TILT_MODBUS_REGISTERS["PAN_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_pan_adjustment
else:
registers[MODBUS_REGISTERS["PAN_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_pan_adjustment
registers[PAN_TILT_MODBUS_REGISTERS[
"PAN_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_pan_adjustment
if pan_tilt_control_message.relative_tilt_adjustment >= 0:
registers[MODBUS_REGISTERS["TILT_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_tilt_adjustment
registers[PAN_TILT_MODBUS_REGISTERS[
"TILT_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_tilt_adjustment
else:
registers[MODBUS_REGISTERS["TILT_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_tilt_adjustment
registers[PAN_TILT_MODBUS_REGISTERS[
"TILT_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_tilt_adjustment
self.pan_tilt_node.write_registers(0, registers)
self.new_control_message = False
self.new_pan_tilt_control_message = False
else:
self.pan_tilt_node.write_registers(0, PAN_TILT_CONTROL_DEFAULT_MESSAGE)
def broadcast_co2_reading_message(self):
self.tower_co2_publisher.publish(UInt16(data=self.tower_node.read_register(1)))
def send_tower_control_message(self):
if self.new_tower_light_control_message:
self.tower_node.write_register(0, self.tower_light_control_message.data)
self.new_tower_light_control_message = False
def pan_tilt_control_callback(self, pan_tilt_control):
self.pan_tilt_control_message = pan_tilt_control
self.new_control_message = True
self.new_pan_tilt_control_message = True
def tower_light_control_callback(self, light_control):
self.tower_light_control_message = light_control
self.new_tower_light_control_message = True
if __name__ == "__main__":
TowerPanTiltControl()
TowerPanTiltControl()