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

Gripper control works. Needs firmware tuning for limits and readouts on GUI

Browse Source
This commit is contained in:
Corwin Perren
2018-08-07 05:18:50 -07:00
parent dfd9bb3854
commit 501b24e795
9 changed files with 266 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
software/environment/rover/UDEV_rules/99-rover-usb-serial.rules
View File

@@ -20,7 +20,7 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyOdometry"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyChassisPanTilt"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_2_0"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyRDF_SoilProbe"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_2_1"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyExtraUART"

110
software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py
View File

@@ -9,7 +9,7 @@ from time import time
import rospy
from rover_arm.msg import ArmControlMessage
from rover_control.msg import MiningControlMessage
from rover_control.msg import MiningControlMessage, GripperControlMessage
#####################################
# Global Variables
@@ -18,6 +18,7 @@ GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360"
DRIVE_COMMAND_HERTZ = 20
GRIPPER_CONTROL_TOPIC = "/rover_control/gripper/control"
RELATIVE_ARM_CONTROL_TOPIC = "/rover_arm/control/relative"
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
@@ -28,6 +29,8 @@ ROLL_SCALAR = 0.003
WRIST_PITCH_SCALAR = 0.003
WRIST_ROLL_SCALAR = 0.006
GRIPPER_MOVEMENT_SCALAR = 300
LEFT_X_AXIS_DEADZONE = 1500
LEFT_Y_AXIS_DEADZONE = 1500
@@ -39,7 +42,6 @@ THUMB_STICK_MAX = 32768.0
MINING_LIFT_SCALAR = 5
MINING_TILT_SCALAR = 5
COLOR_GREEN = "background-color:darkgreen; border: 1px solid black;"
COLOR_NONE = "border: 1px solid black;"
@@ -158,11 +160,18 @@ class XBOXController(QtCore.QThread):
# Controller Class Definition
#####################################
class XBOXControllerControlSender(QtCore.QThread):
STATES = [
XBOX_CONTROL_STATES = [
"ARM",
"MINING"
]
GRIPPER_CONTROL_MODES = {
"NORMAL": 1,
"TWO_FINGER_PINCH": 2,
"WIDE": 3,
"SCISSOR": 4
}
xbox_control_arm_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
xbox_control_mining_stylesheet_update_ready__signal = QtCore.pyqtSignal(str)
@@ -190,14 +199,20 @@ class XBOXControllerControlSender(QtCore.QThread):
self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
self.gripper_control_publisher = rospy.Publisher(GRIPPER_CONTROL_TOPIC, GripperControlMessage, queue_size=1)
self.relative_arm_control_publisher = rospy.Publisher(RELATIVE_ARM_CONTROL_TOPIC, ArmControlMessage,
queue_size=1)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.current_state = self.STATES.index("ARM")
self.state_just_changed = False
self.current_state = self.XBOX_CONTROL_STATES.index("ARM")
self.xbox_control_state_just_changed = False
self.last_xbox_button_state = 0
self.last_left_bumper_state = 0
self.last_right_bumper_state = 0
self.gripper_control_mode = self.GRIPPER_CONTROL_MODES["NORMAL"]
def run(self):
self.logger.debug("Starting Joystick Thread")
@@ -207,9 +222,9 @@ class XBOXControllerControlSender(QtCore.QThread):
self.change_control_state_if_needed()
if self.current_state == self.STATES.index("ARM"):
if self.current_state == self.XBOX_CONTROL_STATES.index("ARM"):
self.process_and_send_arm_control()
elif self.current_state == self.STATES.index("MINING"):
elif self.current_state == self.XBOX_CONTROL_STATES.index("MINING"):
self.send_mining_commands()
time_diff = time() - start_time
@@ -223,31 +238,51 @@ class XBOXControllerControlSender(QtCore.QThread):
self.xbox_control_mining_stylesheet_update_ready__signal.connect(self.xbox_mode_mining_label.setStyleSheet)
def change_control_state_if_needed(self):
if self.state_just_changed:
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.state_just_changed = False
xbox_state = self.controller.controller_states["xbox_button"]
left_bumper_state = self.controller.controller_states["left_bumper"]
right_bumper_state = self.controller.controller_states["right_bumper"]
if self.last_xbox_button_state == 0 and xbox_state == 1:
self.current_state += 1
self.current_state = self.current_state % len(self.STATES)
self.state_just_changed = True
self.current_state = self.current_state % len(self.XBOX_CONTROL_STATES)
self.xbox_control_state_just_changed = True
self.last_xbox_button_state = 1
elif self.last_xbox_button_state == 1 and xbox_state == 0:
self.last_xbox_button_state = 0
if self.last_left_bumper_state == 0 and left_bumper_state == 1:
self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode += 1
self.last_left_bumper_state = 1
elif self.last_left_bumper_state == 1 and left_bumper_state == 0:
self.last_left_bumper_state = 0
if self.last_right_bumper_state == 0 and right_bumper_state == 1:
self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode += 1
self.last_right_bumper_state = 1
elif self.last_right_bumper_state == 1 and right_bumper_state == 0:
self.last_right_bumper_state = 0
if self.xbox_control_state_just_changed:
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_state_just_changed = False
def process_and_send_arm_control(self):
if self.state_just_changed:
if self.xbox_control_state_just_changed:
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_NONE)
self.state_just_changed = False
self.xbox_control_state_just_changed = False
message = ArmControlMessage()
arm_control_message = ArmControlMessage()
should_publish = False
gripper_control_message = GripperControlMessage()
gripper_control_message.gripper_position_absolute = -1
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
should_publish_arm = False
should_publish_gripper = False
left_trigger = self.controller.controller_states["left_trigger"]
right_trigger = self.controller.controller_states["right_trigger"]
@@ -266,29 +301,36 @@ class XBOXControllerControlSender(QtCore.QThread):
left_trigger_ratio = left_trigger / 255.0
right_trigger_ratio = right_trigger / 255.0
if left_trigger > 25:
should_publish = True
message.base = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * left_trigger_ratio
message.shoulder = ((left_y_axis / THUMB_STICK_MAX) * SHOULDER_SCALAR) * left_trigger_ratio
message.elbow = (-(right_y_axis / THUMB_STICK_MAX) * ELBOW_SCALAR) * left_trigger_ratio
message.roll = (-(right_x_axis / THUMB_STICK_MAX) * ROLL_SCALAR) * left_trigger_ratio
if left_trigger > 0:
should_publish_arm = True
arm_control_message.base = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * left_trigger_ratio
arm_control_message.shoulder = ((left_y_axis / THUMB_STICK_MAX) * SHOULDER_SCALAR) * left_trigger_ratio
arm_control_message.elbow = (-(right_y_axis / THUMB_STICK_MAX) * ELBOW_SCALAR) * left_trigger_ratio
arm_control_message.roll = (-(right_x_axis / THUMB_STICK_MAX) * ROLL_SCALAR) * left_trigger_ratio
elif right_trigger > 25:
should_publish = True
message.wrist_roll = ((right_x_axis / THUMB_STICK_MAX) * WRIST_ROLL_SCALAR) * right_trigger_ratio
message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio
elif right_trigger > 0:
should_publish_arm = True
should_publish_gripper = True
if should_publish:
self.relative_arm_control_publisher.publish(message)
arm_control_message.wrist_roll = ((right_x_axis / THUMB_STICK_MAX) * WRIST_ROLL_SCALAR) * right_trigger_ratio
arm_control_message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio
gripper_control_message.gripper_position_relative = (-(right_y_axis / THUMB_STICK_MAX) * GRIPPER_MOVEMENT_SCALAR) * right_trigger_ratio
if should_publish_arm:
self.relative_arm_control_publisher.publish(arm_control_message)
if should_publish_gripper:
self.gripper_control_publisher.publish(gripper_control_message)
def send_mining_commands(self):
left_y_axis = self.controller.controller_states["left_y_axis"] if abs(self.controller.controller_states["left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0
left_y_axis = self.controller.controller_states["left_y_axis"] if abs(
self.controller.controller_states["left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0
right_y_axis = self.controller.controller_states["right_y_axis"] if abs(self.controller.controller_states[
"right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0
"right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0
if left_y_axis or right_y_axis:
message = MiningControlMessage()
message.lift_set_absolute = 1024

2
software/ros_packages/rover_control/CMakeLists.txt
View File

@@ -57,6 +57,8 @@ find_package(catkin REQUIRED COMPONENTS
TowerPanTiltControlMessage.msg
MiningControlMessage.msg
MiningStatusMessage.msg
GripperControlMessage.msg
GripperStatusMessage.msg
)
## Generate services in the 'srv' folder

7
software/ros_packages/rover_control/msg/GripperControlMessage.msg Normal file
View File

@@ -0,0 +1,7 @@
uint8 gripper_mode
int32 gripper_position_absolute
int16 gripper_position_relative
bool should_home
bool toggle_light

13
software/ros_packages/rover_control/msg/GripperStatusMessage.msg Normal file
View File

@@ -0,0 +1,13 @@
uint16 pinch_position_raw
uint16 forefinger_position_raw
uint16 thumb_position_raw
uint16 middlefinger_position_raw
uint16 pinch_current
uint16 forefinger_current
uint16 thumb_current
uint16 middlefinger_current
bool light_on
uint8 current_mode
uint16 current_finger_position

108
software/ros_packages/rover_control/src/effectors_control/effectors_control.py
View File

@@ -12,7 +12,7 @@ import minimalmodbus
# from std_msgs.msg import UInt8, UInt16
# Custom Imports
from rover_control.msg import MiningControlMessage, MiningStatusMessage
from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage
#####################################
# Global Variables
@@ -21,7 +21,7 @@ NODE_NAME = "effectors_control"
# ##### Communication Defines #####
# DEFAULT_PORT = "/dev/rover/ttyEffectors"
DEFAULT_PORT = "/dev/ttyUSB0"
DEFAULT_PORT = "/dev/ttyUSB1"
DEFAULT_BAUD = 115200
GRIPPER_NODE_ID = 1
@@ -38,13 +38,47 @@ TX_DELAY = 0.01
DEFAULT_HERTZ = 40
GRIPPER_CONTROL_SUBSCRIBER_TOPIC = "gripper/control"
GRIPPER_STATUS_PUBLISHER_TOPIC = "gripper/status"
MINING_CONTROL_SUBSCRIBER_TOPIC = "mining/control"
MINING_STATUS_PUBLISHER_TOPIC = "mining/status"
SCIENCE_CONTROL_SUBSCRIBER_TOPIC = "science/control"
# ##### Arm Defines #####
# ##### Gripper Defines #####
GRIPPER_MODBUS_REGISTERS = {
"MODE": 0,
"FINGER_POSITION": 1,
"HOME": 2,
"LIGHT_STATE": 3,
"CURRENT_PINCH": 4,
"CURRENT_FOREFINGER": 5,
"CURRENT_THUMB": 6,
"CURRENT_MIDDLEFINGER": 7,
"POSITION_PINCH": 8,
"POSITION_FOREFINGER": 9,
"POSITION_THUMB": 10,
"POSITION_MIDDLEFINGER": 11,
"LIGHT_STATE_OUPUT": 12,
"MODE_OUTPUT": 13,
"FINGER_POSITION_OUTPUT": 14
}
GRIPPER_MODES = {
"NO_CHANGE": 0,
"NORMAL": 1,
"TWO_FINGER_PINCH": 2,
"WIDE ": 3,
"SCISSOR": 4
}
DEFAULT_GRIPPER_REGISTERS = [
0, # No change
0, # No positional update
0, # Do not home
0, # Light off
]
# ##### Mining Defines #####
MINING_MODBUS_REGISTERS = {
@@ -89,6 +123,8 @@ class EffectorsControl(object):
self.gripper_control_subscriber_topic = rospy.get_param("~gripper_control_subscriber_topic",
GRIPPER_CONTROL_SUBSCRIBER_TOPIC)
self.gripper_status_publisher_topic = rospy.get_param("~gripper_status_publisher_topic",
GRIPPER_STATUS_PUBLISHER_TOPIC)
self.mining_control_subscriber_topic = rospy.get_param("~mining_control_subscriber_topic",
MINING_CONTROL_SUBSCRIBER_TOPIC)
@@ -113,22 +149,30 @@ class EffectorsControl(object):
# self.check_which_nodes_present()
# ##### Subscribers #####
self.gripper_control_subscriber = rospy.Subscriber(self.gripper_control_subscriber_topic, GripperControlMessage, self.gripper_control_message_received__callback)
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage,
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningStatusMessage,
self.mining_control_message_received__callback)
# ##### Publishers #####
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, queue_size=1)
self.gripper_status_publisher = rospy.Publisher(self.gripper_status_publisher_topic, GripperStatusMessage, queue_size=1)
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage,
queue_size=1)
# ##### Misc #####
self.modbus_nodes_seen_time = time()
# ##### Mining Variables #####
self.mining_registers = [0 for _ in MINING_MODBUS_REGISTERS]
self.gripper_registers = [0 for _ in GRIPPER_MODBUS_REGISTERS]
self.mining_control_message = None # type:MiningControlMessage
self.new_mining_control_message = False
self.gripper_control_message = None
self.new_gripper_control_message = False
self.run()
def __setup_minimalmodbus_for_485(self):
@@ -150,6 +194,11 @@ class EffectorsControl(object):
def run(self):
# self.initialize_mining_system()
try:
self.gripper_registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
except:
pass
while not rospy.is_shutdown():
# try:
# self.process_mining_control_message()
@@ -172,7 +221,8 @@ class EffectorsControl(object):
# pass
try:
print self.gripper_node.read_registers(0, 15)
self.process_gripper_control_message()
self.send_gripper_status_message()
except Exception, error:
print error
@@ -253,6 +303,52 @@ class EffectorsControl(object):
self.modbus_nodes_seen_time = time()
def process_gripper_control_message(self):
if self.new_gripper_control_message:
if self.gripper_control_message.toggle_light:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = not self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]]
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["HOME"]] = self.gripper_control_message.should_home
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["MODE"]] = self.gripper_control_message.gripper_mode
gripper_absolute = self.gripper_control_message.gripper_position_absolute
gripper_relative = self.gripper_control_message.gripper_position_relative
if -1 < gripper_absolute < UINT16_MAX:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = gripper_absolute
else:
new_position = self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] + gripper_relative
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION"]] = min(max(new_position, 0), 10000)
self.gripper_node.write_registers(0, self.gripper_registers)
self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS)
self.new_gripper_control_message = False
def send_gripper_status_message(self):
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
message = GripperStatusMessage()
message.pinch_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_PINCH"]]
message.forefinger_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_FOREFINGER"]]
message.thumb_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_THUMB"]]
message.middlefinger_position_raw = registers[GRIPPER_MODBUS_REGISTERS["POSITION_MIDDLEFINGER"]]
message.pinch_current = registers[GRIPPER_MODBUS_REGISTERS["CURRENT_PINCH"]]
message.forefinger_current = registers[GRIPPER_MODBUS_REGISTERS["CURRENT_FOREFINGER"]]
message.thumb_current = registers[GRIPPER_MODBUS_REGISTERS["CURRENT_THUMB"]]
message.middlefinger_current = registers[GRIPPER_MODBUS_REGISTERS["CURRENT_MIDDLEFINGER"]]
message.light_on = registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE_OUPUT"]]
message.current_mode = registers[GRIPPER_MODBUS_REGISTERS["MODE_OUTPUT"]]
message.current_finger_position = registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]]
self.gripper_status_publisher.publish(message)
def gripper_control_message_received__callback(self, control_message):
self.gripper_control_message = control_message
self.new_gripper_control_message = True
def mining_control_message_received__callback(self, control_message):
self.mining_control_message = control_message
self.new_mining_control_message = True

14
software/ros_packages/rover_control/src/effectors_control/effectors_tester.py
View File

@@ -2,20 +2,20 @@
import rospy
import time
from rover_control.msg import MiningControlMessage
from rover_control.msg import GripperControlMessage
DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "/rover_control/mining/control"
TOPIC = "/gripper/control"
rospy.init_node("effectors_tester")
publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
publisher = rospy.Publisher(TOPIC, GripperControlMessage, queue_size=1)
time.sleep(2)
message = MiningControlMessage()
message.lift_set = 200
message.tilt_set = 1023
message.cal_factor = -1
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_position = 0
message.should_home = 0
publisher.publish(message)

3
software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch
View File

@@ -10,7 +10,8 @@
{name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.0},
{name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0},
{name: "/rover_control/mining/control", compress: true, rate: 30.0},
{name: "/rover_arm/control/relative", compress: true, rate: 30.0}]
{name: "/rover_arm/control/relative", compress: true, rate: 30.0},
{name: "/rover_control/gripper/control", compress: true, rate: 30.0}]
</rosparam>
</node>

70
software/ros_packages/rover_science/src/rover_science.py
View File

@@ -20,8 +20,8 @@ from std_msgs.msg import Float64MultiArray
#####################################
NODE_NAME = "science_node"
DEFAULT_PORT = "/dev/rover/ttyIRIS_2_0"
DEFAULT_BAUD = 115200
DEFAULT_PORT = "/dev/rover/ttyRDF_SoilProbe"
DEFAULT_BAUD = 9600
DEFAULT_INVERT = False
@@ -29,13 +29,15 @@ DEFAULT_RDF_PUBLISHER_TOPIC = "rdf/data"
PAN_TILT_NODE_ID = 1
COMMUNICATIONS_TIMEOUT = 0.005 # Seconds
COMMUNICATIONS_TIMEOUT = 0.1 # Seconds
RX_DELAY = 0.01
TX_DELAY = 0.01
DEFAULT_HERTZ = 20
SOIL_PROBE_ADDRESS = "mar"
PAN_TILT_MODBUS_REGISTERS = {
"CENTER_ALL": 0,
@@ -45,6 +47,16 @@ PAN_TILT_MODBUS_REGISTERS = {
"TILT_ADJUST_NEGATIVE": 4
}
SOIL_PROBE_COMMANDS = {
"GET_ADDRESS": "AD=",
"DESCRIPTION": "DS=",
"PROBE_ENABLED": "PE=",
"TAKE_READING": "TR",
"TRANSMIT_READING": "T0",
"QUERY": "?"
}
NODE_LAST_SEEN_TIMEOUT = 2 # seconds
@@ -84,27 +96,57 @@ class RoverScience(object):
self.run()
def __setup_minimalmodbus_for_485(self):
self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) # type: serial.Serial
self.pan_tilt_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.pan_tilt_node.serial.timeout(500)
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["PROBE_ENABLED"] + "1")
self.pan_tilt_node.serial.write(out)
sleep(0.1)
while not rospy.is_shutdown():
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TAKE_READING"])
self.pan_tilt_node.serial.write(out)
sleep(0.1)
out = "%s%s\r\n" % (SOIL_PROBE_ADDRESS, SOIL_PROBE_COMMANDS["TRANSMIT_READING"])
self.pan_tilt_node.serial.write(out)
start_time = time()
try:
registers = self.pan_tilt_node.read_registers(0, 1)
self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()]))
self.modbus_nodes_seen_time = time()
except Exception, Error:
print Error
char = ""
response = ""
while char != '\n' and (time() - start_time) < 2:
# print time() - start_time, (time() - start_time) > 2
if self.pan_tilt_node.serial.inWaiting():
char = self.pan_tilt_node.serial.read()
response += char
# print char
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Science not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting."
return # Exit so respawn can take over
if response:
print response
else:
print "timeout"
# print "No response"
time_diff = time() - start_time
# start_time = time()
# try:
# registers = self.pan_tilt_node.read_registers(0, 1)
# self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()]))
# self.modbus_nodes_seen_time = time()
# except Exception, Error:
# print Error
#
# if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
# print "Science not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting."
# return # Exit so respawn can take over
# time_diff = time() - start_time
# def run(self):
# # self.send_startup_centering_command()