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

Mining system completegit add .!

Browse Source
This commit is contained in:
Corwin Perren
2018-07-29 21:46:10 -07:00
parent b79693c855
commit 092318ec10
20 changed files with 1223 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py
View File

@@ -9,10 +9,20 @@ import spnav
import rospy
from rover_control.msg import MiningControlMessage
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 15
THREAD_HERTZ = 100
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
Y_ANGULAR_DEADBAND = 0.05
Z_LINEAR_DEADBAND = 0.15
MINING_LIFT_SCALAR = 5
MINING_TILT_SCALAR = 5
#####################################
@@ -21,7 +31,7 @@ THREAD_HERTZ = 15
class SpaceNavControlSender(QtCore.QThread):
spacenav_state_update__signal = QtCore.pyqtSignal(object)
GUI_MODE = 0
MINING_MODE = 0
ARM_MODE = 1
def __init__(self, shared_objects):
@@ -95,7 +105,10 @@ class SpaceNavControlSender(QtCore.QThread):
5: "f_pressed"
}
self.current_control_mode = self.GUI_MODE
# ##### Mining Control #####
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.current_control_mode = self.MINING_MODE
def run(self):
spnav.spnav_open()
@@ -110,7 +123,7 @@ class SpaceNavControlSender(QtCore.QThread):
time_diff = time() - start_time
self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
# self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
def process_spnav_events(self):
event = spnav.spnav_poll_event()
@@ -132,16 +145,33 @@ class SpaceNavControlSender(QtCore.QThread):
def check_control_mode_change(self):
if self.spnav_states["1_pressed"]:
self.current_control_mode = self.GUI_MODE
self.current_control_mode = self.MINING_MODE
elif self.spnav_states["2_pressed"]:
self.current_control_mode = self.ARM_MODE
def broadcast_control_state(self):
if self.current_control_mode == self.GUI_MODE:
self.spacenav_state_update__signal.emit(self.spnav_states)
if self.current_control_mode == self.MINING_MODE:
self.send_mining_commands()
# self.spacenav_state_update__signal.emit(self.spnav_states)
elif self.current_control_mode == self.ARM_MODE:
pass
def send_mining_commands(self):
linear_z = self.spnav_states["linear_z"]
angular_y = self.spnav_states["angular_y"]
message = MiningControlMessage()
message.lift_set_absolute = 1024
message.tilt_set_absolute = 1024
message.lift_set_relative = linear_z * MINING_LIFT_SCALAR if abs(linear_z) > Z_LINEAR_DEADBAND else 0
message.tilt_set_relative = angular_y * MINING_TILT_SCALAR if abs(angular_y) > Y_ANGULAR_DEADBAND else 0
message.cal_factor = -1
self.mining_control_publisher.publish(message)
# print self.spnav_states["linear_z"], self.spnav_states["angular_y"]
def connect_signals_and_slots(self):
pass

139
software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py Normal file
View File

@@ -0,0 +1,139 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
import rospy
from rover_control.msg import MiningStatusMessage, MiningControlMessage
#####################################
# Global Variables
#####################################
MINING_STATUS_TOPIC = "/rover_control/mining/status"
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
TRAVEL_POSITION_LIFT = 110
TRAVEL_POSITION_TILT = 1023
MEASURE_POSITION_LIFT = 350
MEASURE_POSITION_TILT = 1023
SCOOP_POSITION_LIFT = 228
SCOOP_POSITION_TILT = 215
#####################################
# UbiquitiRadioSettings Class Definition
#####################################
class Mining(QtCore.QObject):
lift_position_update_ready__signal = QtCore.pyqtSignal(int)
tilt_position_update_ready__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
super(Mining, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.mining_qlcdnumber = self.left_screen.mining_qlcdnumber # type:QtWidgets.QLCDNumber
self.mining_tare_button = self.left_screen.mining_tare_button # type:QtWidgets.QPushButton
self.mining_measure_button = self.left_screen.mining_measure_button # type:QtWidgets.QPushButton
self.mining_cal_factor_spinbox = self.left_screen.mining_cal_factor_spinbox # type:QtWidgets.QSpinBox
self.mining_set_cal_factor_button = self.left_screen.mining_set_cal_factor_button # type:QtWidgets.QPushButton
self.lift_position_progress_bar = self.left_screen.lift_position_progress_bar # type:QtWidgets.QProgressBar
self.tilt_position_progress_bar = self.left_screen.tilt_position_progress_bar # type:QtWidgets.QProgressBar
self.mining_measure_move_button = self.left_screen.mining_measure_move_button # type:QtWidgets.QPushButton
self.mining_transport_move_button = self.left_screen.mining_transport_move_button # type:QtWidgets.QPushButton
self.mining_scoop_move_button = self.left_screen.mining_scoop_move_button # type:QtWidgets.QPushButton
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
# ########## Get the Pick And Plate instance of the logger ##########
self.logger = logging.getLogger("groundstation")
# ########## Thread Flags ##########
self.run_thread_flag = True
# ########## Class Variables ##########
self.mining_status_subscriber = rospy.Subscriber(MINING_STATUS_TOPIC, MiningStatusMessage,
self.mining_status_message_received__callback)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.connect_signals_and_slots()
def connect_signals_and_slots(self):
self.mining_set_cal_factor_button.clicked.connect(self.on_mining_set_cal_factor_clicked__slot)
self.mining_tare_button.clicked.connect(self.on_mining_tare_clicked__slot)
self.mining_measure_button.clicked.connect(self.on_mining_measure_clicked__slot)
self.mining_measure_move_button.clicked.connect(self.on_mining_move_measure_clicked__slot)
self.mining_transport_move_button.clicked.connect(self.on_mining_move_transport_clicked__slot)
self.mining_scoop_move_button.clicked.connect(self.on_mining_move_scoop_clicked__slot)
self.tilt_position_update_ready__signal.connect(self.tilt_position_progress_bar.setValue)
self.lift_position_update_ready__signal.connect(self.lift_position_progress_bar.setValue)
def on_mining_set_cal_factor_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = 1024
message.lift_set_absolute = 1024
message.cal_factor = self.mining_cal_factor_spinbox.value()
self.mining_control_publisher.publish(message)
def on_mining_tare_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = 1024
message.lift_set_absolute = 1024
message.cal_factor = -1
message.tare = 1
self.mining_control_publisher.publish(message)
def on_mining_measure_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = 1024
message.lift_set_absolute = 1024
message.cal_factor = -1
message.measure = True
self.mining_control_publisher.publish(message)
def on_mining_move_transport_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = TRAVEL_POSITION_TILT
message.lift_set_absolute = TRAVEL_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_measure_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = MEASURE_POSITION_TILT
message.lift_set_absolute = MEASURE_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_scoop_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = SCOOP_POSITION_TILT
message.lift_set_absolute = SCOOP_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def mining_status_message_received__callback(self, status):
status = status # type:MiningStatusMessage
self.tilt_position_update_ready__signal.emit(status.tilt_position)
self.lift_position_update_ready__signal.emit(status.lift_position)
self.mining_qlcdnumber.display(status.measured_weight)

328
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
View File

@@ -1392,6 +1392,334 @@ N/A</string>
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="tab_3">
<attribute name="title">
<string>Mining</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_8">
<item row="5" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_12">
<item>
<widget class="QLabel" name="label_15">
<property name="font">
<font>
<pointsize>12</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Lift Position</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="lift_position_progress_bar">
<property name="maximum">
<number>1023</number>
</property>
<property name="value">
<number>512</number>
</property>
<property name="format">
<string>%v</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="6" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_11">
<item>
<widget class="QLabel" name="label_14">
<property name="font">
<font>
<pointsize>12</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Tilt Position</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="tilt_position_progress_bar">
<property name="maximum">
<number>1023</number>
</property>
<property name="value">
<number>512</number>
</property>
<property name="format">
<string>%v</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_13">
<property name="font">
<font>
<pointsize>20</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Scoop Measurement</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_5">
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="mining_tare_button">
<property name="text">
<string>Tare</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="mining_measure_button">
<property name="text">
<string>Measure</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_5">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item row="3" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_6">
<item>
<spacer name="horizontalSpacer_10">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QSpinBox" name="mining_cal_factor_spinbox">
<property name="maximum">
<number>65535</number>
</property>
<property name="value">
<number>114</number>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="mining_set_cal_factor_button">
<property name="text">
<string>Set Cal Factor</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_6">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_10">
<item>
<spacer name="horizontalSpacer_12">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QLCDNumber" name="mining_qlcdnumber">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>300</width>
<height>150</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>300</width>
<height>150</height>
</size>
</property>
<property name="font">
<font>
<pointsize>9</pointsize>
</font>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="value" stdset="0">
<double>0.000000000000000</double>
</property>
<property name="intValue" stdset="0">
<number>0</number>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_5">
<property name="font">
<font>
<pointsize>19</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>g</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_11">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item row="8" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="4" column="0">
<widget class="Line" name="line_6">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="7" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_13">
<item>
<spacer name="horizontalSpacer_13">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="mining_transport_move_button">
<property name="text">
<string>Transport</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="mining_measure_move_button">
<property name="text">
<string>Measure</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="mining_scoop_move_button">
<property name="text">
<string>Scoop</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_14">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab">
<attribute name="title">
<string>SSH Console</string>

5
software/ros_packages/ground_station/src/ground_station.py
View File

@@ -23,6 +23,7 @@ import Framework.StatusSystems.StatusCore as StatusCore
import Framework.StatusSystems.UbiquitiStatusCore as UbiquitiStatusCore
import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings
import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
import Framework.MiscSystems.MiningCore as MiningCore
#####################################
# Global Variables
@@ -101,6 +102,7 @@ class GroundStation(QtCore.QObject):
rospy.init_node("ground_station")
# ##### Instantiate Regular Classes ######
self.__add_non_thread("Mining System", MiningCore.Mining(self.shared_objects))
# ##### Instantiate Threaded Classes ######
self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
@@ -132,6 +134,9 @@ class GroundStation(QtCore.QObject):
instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal,
self.kill_threads_signal)
def __add_non_thread(self, name, instance):
self.shared_objects["regular_classes"][name] = instance
def __connect_signals_to_slots(self):
self.shared_objects["screens"]["left_screen"].exit_requested_signal.connect(self.on_exit_requested__slot)
self.shared_objects["screens"]["right_screen"].exit_requested_signal.connect(self.on_exit_requested__slot)

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

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

10
software/ros_packages/rover_control/msg/MiningControlMessage.msg Normal file
View File

@@ -0,0 +1,10 @@
int32 lift_set_relative
int32 tilt_set_relative
uint16 lift_set_absolute
uint16 tilt_set_absolute
bool measure
bool tare
int16 cal_factor

4
software/ros_packages/rover_control/msg/MiningStatusMessage.msg Normal file
View File

@@ -0,0 +1,4 @@
uint16 lift_position
uint16 tilt_position
uint16 measured_weight

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

@@ -96,7 +96,7 @@ class ChassisPanTiltControl(object):
delay_before_tx=TX_DELAY)
def run(self):
self.send_startup_centering_command()
# self.send_startup_centering_command()
while not rospy.is_shutdown():
start_time = time()

280
software/ros_packages/rover_control/src/effectors_control/effectors_control.py Normal file
View File

@@ -0,0 +1,280 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial.rs485
import minimalmodbus
# from std_msgs.msg import UInt8, UInt16
# Custom Imports
from rover_control.msg import MiningControlMessage, MiningStatusMessage
#####################################
# Global Variables
#####################################
NODE_NAME = "effectors_control"
# ##### Communication Defines #####
DEFAULT_PORT = "/dev/rover/ttyEffectors"
DEFAULT_BAUD = 115200
GRIPPER_NODE_ID = 1
MINING_NODE_ID = 2
SCIENCE_NODE_ID = 3
GRIPPER_TIMEOUT = 0.15
MINING_TIMEOUT = 0.3
SCIENCE_TIMEOUT = 0.15
RX_DELAY = 0.01
TX_DELAY = 0.01
DEFAULT_HERTZ = 40
GRIPPER_CONTROL_SUBSCRIBER_TOPIC = "gripper/control"
MINING_CONTROL_SUBSCRIBER_TOPIC = "mining/control"
MINING_STATUS_PUBLISHER_TOPIC = "mining/status"
SCIENCE_CONTROL_SUBSCRIBER_TOPIC = "science/control"
# ##### Arm Defines #####
# ##### Mining Defines #####
MINING_MODBUS_REGISTERS = {
"LIFT_SET_POSITIVE": 0,
"LIFT_SET_NEGATIVE": 1,
"TILT_SET_POSITIVE": 2,
"TILT_SET_NEGATIVE": 3,
"TILT_SET_ABSOLUTE": 4,
"LIFT_SET_ABSOLUTE": 5,
"MEASURE": 6,
"TARE": 7,
"CAL_FACTOR": 8,
"LIFT_POSITION": 9,
"TILT_POSITION": 10,
"MEASURED_WEIGHT": 11
}
MINING_POSITIONAL_THRESHOLD = 20
# ##### Science Defines #####
# ##### Misc Defines #####
NODE_LAST_SEEN_TIMEOUT = 2 # seconds
UINT16_MAX = 65535
#####################################
# DriveControl Class Definition
#####################################
class EffectorsControl(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.port = rospy.get_param("~port", DEFAULT_PORT)
self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
self.gripper_node_id = rospy.get_param("~gripper_node_id", GRIPPER_NODE_ID)
self.mining_node_id = rospy.get_param("~mining_node_id", MINING_NODE_ID)
self.science_node_id = rospy.get_param("~science_node_id", SCIENCE_NODE_ID)
self.gripper_control_subscriber_topic = rospy.get_param("~gripper_control_subscriber_topic",
GRIPPER_CONTROL_SUBSCRIBER_TOPIC)
self.mining_control_subscriber_topic = rospy.get_param("~mining_control_subscriber_topic",
MINING_CONTROL_SUBSCRIBER_TOPIC)
self.mining_status_publisher_topic = rospy.get_param("~mining_status_publisher_topic",
MINING_STATUS_PUBLISHER_TOPIC)
self.science_control_subscriber_topic = rospy.get_param("~science_control_subscriber_topic",
SCIENCE_CONTROL_SUBSCRIBER_TOPIC)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.gripper_node = None # type:minimalmodbus.Instrument
self.mining_node = None # type:minimalmodbus.Instrument
self.science_node = None # type:minimalmodbus.Instrument
self.gripper_node_present = False
self.mining_node_present = True
self.science_node_present = False
self.connect_to_nodes()
# self.check_which_nodes_present()
# ##### Subscribers #####
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage,
self.mining_control_message_received__callback)
# ##### Publishers #####
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.mining_control_message = None # type:MiningControlMessage
self.new_mining_control_message = False
self.run()
def __setup_minimalmodbus_for_485(self):
self.gripper_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=GRIPPER_TIMEOUT)
self.gripper_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)
self.mining_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=MINING_TIMEOUT)
self.mining_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)
self.science_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=SCIENCE_TIMEOUT)
self.science_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.initialize_mining_system()
while not rospy.is_shutdown():
try:
self.process_mining_control_message()
except IOError, e:
print e
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Lost connection to mining system. Exiting for reconnect."
return
except Exception, e:
print e
try:
self.send_mining_status_message()
except IOError, e:
print e
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Lost connection to mining system. Exiting for reconnect."
return
except Exception, e:
print e
def connect_to_nodes(self):
self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id))
self.mining_node = minimalmodbus.Instrument(self.port, int(self.mining_node_id))
self.science_node = minimalmodbus.Instrument(self.port, int(self.science_node_id))
self.__setup_minimalmodbus_for_485()
def check_which_nodes_present(self):
try:
self.gripper_node.read_register(0)
self.gripper_node_present = True
except:
self.gripper_node_present = False
try:
self.mining_node.read_register(0)
self.mining_node_present = True
except:
self.mining_node_present = False
try:
self.science_node.read_register(0)
self.science_node_present = True
except:
self.science_node_present = False
def initialize_mining_system(self):
if self.mining_node_present:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET"]] = 1023
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]] = 350
self.mining_registers[MINING_MODBUS_REGISTERS["CAL_FACTOR"]] = 114
while abs(self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]] - self.mining_registers[
MINING_MODBUS_REGISTERS["LIFT_SET"]]) > MINING_POSITIONAL_THRESHOLD or \
abs(self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]] - self.mining_registers[
MINING_MODBUS_REGISTERS["TILT_SET"]]) > MINING_POSITIONAL_THRESHOLD:
try:
self.mining_node.write_registers(0, self.mining_registers)
self.mining_registers = self.mining_node.read_registers(0, 7)
except Exception, e:
print "Had trouble communicating:", e
try:
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 1
self.mining_node.write_registers(0, self.mining_registers)
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 0
except:
print "Had trouble communicating: no tare: ", e
def process_mining_control_message(self):
if self.new_mining_control_message and self.mining_node_present:
lift_set_relative = self.mining_control_message.lift_set_relative
tilt_set_relative = self.mining_control_message.tilt_set_relative
lift_set_absolute = self.mining_control_message.lift_set_absolute
tilt_set_absolute = self.mining_control_message.tilt_set_absolute
cal_factor = min(self.mining_control_message.cal_factor, UINT16_MAX)
measure = self.mining_control_message.measure
tare = self.mining_control_message.tare
if lift_set_absolute < 1024:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_ABSOLUTE"]] = lift_set_absolute
else:
if lift_set_relative >= 0:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_POSITIVE"]] = lift_set_relative
else:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_NEGATIVE"]] = -lift_set_relative
if tilt_set_absolute < 1024:
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_ABSOLUTE"]] = tilt_set_absolute
else:
if tilt_set_relative >= 0:
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_POSITIVE"]] = tilt_set_relative
else:
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_NEGATIVE"]] = -tilt_set_relative
if cal_factor > -1:
self.mining_registers[MINING_MODBUS_REGISTERS["CAL_FACTOR"]] = cal_factor
self.mining_registers[MINING_MODBUS_REGISTERS["MEASURE"]] = int(measure)
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = int(tare)
self.mining_node.write_registers(0, self.mining_registers)
self.modbus_nodes_seen_time = time()
self.new_mining_control_message = False
def send_mining_status_message(self):
if self.mining_node_present:
self.mining_registers = self.mining_node.read_registers(0, len(MINING_MODBUS_REGISTERS))
message = MiningStatusMessage()
message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]]
message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]]
message.measured_weight = self.mining_registers[MINING_MODBUS_REGISTERS["MEASURED_WEIGHT"]]
self.mining_status_publisher.publish(message)
self.modbus_nodes_seen_time = time()
def mining_control_message_received__callback(self, control_message):
self.mining_control_message = control_message
self.new_mining_control_message = True
if __name__ == "__main__":
EffectorsControl()

31
software/ros_packages/rover_control/src/effectors_control/effectors_tester.py Executable file
View File

@@ -0,0 +1,31 @@
#!/usr/bin/env python
import rospy
import time
from rover_control.msg import MiningControlMessage
DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "/rover_control/mining/control"
rospy.init_node("effectors_tester")
publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
time.sleep(2)
message = MiningControlMessage()
message.lift_set = 200
message.tilt_set = 1023
message.cal_factor = -1
publisher.publish(message)
time.sleep(5)
# message = MiningControlMessage()
# message.lift_set = -200
# message.tilt_set = -100
# message.cal_factor = -1
#
# publisher.publish(message)
#
# time.sleep(2)

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

@@ -172,9 +172,9 @@ class TowerPanTiltControl(object):
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)
# 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:

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

@@ -8,7 +8,8 @@
<rosparam param="topics">
[{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0},
{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/chassis/pan_tilt/control", compress: true, rate: 30.0},
{name: "/rover_control/mining/control", compress: true, rate: 15.0}]
</rosparam>
</node>

2
software/ros_packages/rover_main/launch/rover/control.launch
View File

@@ -34,5 +34,7 @@
<node name="tower_and_pan_tilt" pkg="rover_control" type="tower_and_pan_tilt_control.py" respawn="true" output="screen"/>
<node name="chassis_pan_tilt" pkg="rover_control" type="chassis_pan_tilt_control.py" respawn="true" output="screen"/>
<node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen"/>
</group>
</launch>

6
software/ros_packages/rover_main/launch/rover/test_control.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<group ns="rover_control">
<node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen">
</node>
</group>
</launch>

1
software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch
View File

@@ -155,6 +155,7 @@
{name: "/rover_status/battery_status", compress: false, rate: 1.0},
{name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
{name: "/rover_odometry/imu/data", compress: false, rate: 10.0},
{name: "/rover_control/mining/status", compress: false, rate: 2.0},
]
</rosparam>
</node>