mirror of
https://github.com/caperren/school_archives.git
synced 2025-11-09 21:51:15 +00:00
130 lines
3.8 KiB
Python
130 lines
3.8 KiB
Python
#!/usr/bin/env python
|
|
#####################################
|
|
# Imports
|
|
#####################################
|
|
# Python native imports
|
|
|
|
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
|
|
|
|
#####################################
|
|
# Global Variables
|
|
#####################################
|
|
NODE_NAME = "chassis_pan_tilt_control"
|
|
|
|
DEFAULT_PORT = "/dev/rover/ttyEffectors"
|
|
DEFAULT_BAUD = 115200
|
|
|
|
DEFAULT_INVERT = False
|
|
|
|
DEFAULT_PAN_TILT_CONTROL_TOPIC = "chassis/pan_tilt/control"
|
|
|
|
PAN_TILT_NODE_ID = 1
|
|
|
|
COMMUNICATIONS_TIMEOUT = 0.15
|
|
# Seconds
|
|
|
|
RX_DELAY = 0.01
|
|
TX_DELAY = 0.01
|
|
|
|
DEFAULT_HERTZ = 20
|
|
|
|
MINING_MODBUS_REGISTERS = {
|
|
"LIFT_SET": 0,
|
|
"TILT_SET": 1,
|
|
"TARE": 2,
|
|
"CAL_FACTOR": 3,
|
|
|
|
"LIFT_POSITION": 4,
|
|
"TILT_POSITION": 5,
|
|
"MEASURED_WEIGHT": 6
|
|
}
|
|
|
|
POSITIONAL_THRESHOLD = 20
|
|
|
|
NODE_LAST_SEEN_TIMEOUT = 2 # seconds
|
|
|
|
|
|
#####################################
|
|
# DriveControl Class Definition
|
|
#####################################
|
|
class MiningControl(object):
|
|
def __init__(self):
|
|
self.port = DEFAULT_PORT
|
|
self.baud = 115200
|
|
|
|
self.mining_node = None
|
|
self.tower_node = None
|
|
|
|
self.connect_to_pan_tilt_and_tower()
|
|
|
|
self.pan_tilt_control_message = None
|
|
self.new_pan_tilt_control_message = False
|
|
|
|
self.modbus_nodes_seen_time = time()
|
|
|
|
self.mining_registers = [
|
|
0,
|
|
0,
|
|
0,
|
|
0,
|
|
0,
|
|
0,
|
|
0
|
|
]
|
|
|
|
self.run()
|
|
|
|
def __setup_minimalmodbus_for_485(self):
|
|
self.mining_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_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)
|
|
|
|
def run(self):
|
|
self.initialize_mining_system()
|
|
while True:
|
|
try:
|
|
print self.mining_node.read_registers(0, 7)
|
|
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]] = int(input("Enter new tilt value:"))
|
|
self.mining_node.write_registers(0, self.mining_registers)
|
|
|
|
except Exception, e:
|
|
print e
|
|
|
|
def connect_to_pan_tilt_and_tower(self):
|
|
self.mining_node = minimalmodbus.Instrument(self.port, int(2))
|
|
self.__setup_minimalmodbus_for_485()
|
|
|
|
def initialize_mining_system(self):
|
|
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"]]) > POSITIONAL_THRESHOLD or \
|
|
abs(self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]] - self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]]) > 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
|
|
|
|
|
|
if __name__ == "__main__":
|
|
MiningControl()
|