def init_rc():
global rc
global rc_address
# Initialise the roboclaw motorcontroller
print("Initialising roboclaw driver...")
from roboclaw import Roboclaw
rc_address = 0x80
rc = Roboclaw("/dev/roboclaw", 115200)
rc.Open()
# Get roboclaw version to test if is attached
version = rc.ReadVersion(rc_address)
if version[0] is False:
print("Roboclaw get version failed")
sys.exit()
else:
print(repr(version[1]))
# Set motor controller variables to those required by K9
rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS)
rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS)
rc.SetMainVoltages(rc_address,232,290) # 23.2V min, 29V max
rc.SetPinFunctions(rc_address,2,0,0)
# Zero the motor encoders
rc.ResetEncoders(rc_address)
# Print Motor PID Settings
m1pid = rc.ReadM1VelocityPID(rc_address)
m2pid = rc.ReadM2VelocityPID(rc_address)
print("M1 P: " + str(m1pid[1]) + ", I:" + str(m1pid[2]) + ", D:" + str(m1pid[3]))
print("M2 P: " + str(m2pid[1]) + ", I:" + str(m2pid[2]) + ", D:" + str(m2pid[3]))
# Print Min and Max Main Battery Settings
minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts
print ("Min Main Battery: " + str(int(minmaxv[1])/10) + "V")
print ("Max Main Battery: " + str(int(minmaxv[2])/10) + "V")
# Print S3, S4 and S5 Modes
S3mode=['Default','E-Stop (latching)','E-Stop','Voltage Clamp','Undefined']
S4mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M1 Home']
S5mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M2 Home']
pinfunc = rc.ReadPinFunctions(rc_address)
print ("S3 pin: " + S3mode[pinfunc[1]])
print ("S4 pin: " + S4mode[pinfunc[2]])
print ("S5 pin: " + S5mode[pinfunc[3]])
print("Roboclaw motor controller initialised...")
class Agitator(object):
'''Class for controlling the EXPRES fiber agitator
Inputs
------
comport : str
The hardware COM Port for the Roboclaw motor controller
Public Methods
--------------
start(exp_time, timeout, rot1, rot2):
Threaded agitation
start_agitation(exp_time, rot1, rot2):
Unthreaded agitation
stop():
Stop either threaded or unthreaded agitation
stop_agitation():
Hard-stop agitation but will not close thread
'''
def __init__(self, comport=__DEFAULT_PORT__):
self._rc = Roboclaw(comport=comport,
rate=__DEFAULT_BAUD_RATE__,
addr=__DEFAULT_ADDR__,
timeout=__DEFAULT_TIMEOUT__,
retries=__DEFAULT_RETRIES__,
inter_byte_timeout=__DEFAULT_INTER_BYTE_TIMEOUT__)
# Create a logger for the agitator
self.logger = logging.getLogger('expres_agitator')
self.logger.setLevel(logging.DEBUG)
# Create file handler to log all messages
try:
fh = logging.handlers.TimedRotatingFileHandler(
'C:/Users/admin/agitator_logs/agitator.log',
when='D',
interval=1,
utc=True,
backupCount=5)
except FileNotFoundError:
fh = logging.handlers.TimedRotatingFileHandler('agitator.log',
when='D',
interval=1,
utc=True,
backupCount=5)
fh.setLevel(logging.DEBUG)
# Create console handler to log info messages
ch = logging.StreamHandler()
ch.setLevel(logging.INFO)
# create formatter and add it to the handlers
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s')
fh.setFormatter(formatter)
ch.setFormatter(formatter)
# add the handlers to the logger
self.logger.addHandler(fh)
self.logger.addHandler(ch)
self.thread = None # In case stop() is called before a thread is created
self.stop_event = Event() # Used for stopping threads
self.stop_agitation() # Just to make sure
def __del__(self):
'''
When the object is deleted, make sure all threads are closed and
agitator is stopped. Unfortunately, these actions cannot be logged
because the logger closes by the time __del__ is called...
'''
self.stop(verbose=False)
self.stop_agitation(verbose=False)
def threaded_agitation(self, exp_time, timeout, **kwargs):
'''Threadable function allowing stop event'''
self.logger.info(
'Starting agitator thread for {}s exposure with {}s timeout'.
format(exp_time, timeout))
self.start_agitation(exp_time, **kwargs)
t = 0
start_time = time.time()
while not self.stop_event.is_set() and t < timeout:
time.sleep(1)
t = time.time() - start_time
self.logger.info('{}/{}s for {}s exposure. I1: {}, I2: {}'.format(
round(t, 1), timeout, exp_time, self.current1, self.current2))
self.stop_agitation()
self.stop_event.clear() # Allow for future agitation events
def start(self, exp_time=60.0, timeout=None, **kwargs):
'''
Start a thread that starts agitation and stops if a stop event is
called or if the timeout is reached
'''
self.stop() # To close any previously opened threads
if timeout is None: # Allow for some overlap time
timeout = exp_time + 10.0
self.thread = Thread(target=self.threaded_agitation,
args=(exp_time, timeout),
kwargs=kwargs)
self.thread.start()
def stop(self, verbose=True):
'''Stop the agitation thread if it is running'''
if self.thread is not None and self.thread.is_alive():
while self.voltage1 > 0 or self.voltage2 > 0:
if verbose:
self.logger.info('Attempting to stop threaded agitation')
self.stop_event.set()
self.thread.join(2)
if self.voltage1 > 0 or self.voltage2 > 0:
# As a backup in case something went wrong
if verbose:
self.logger.error(
'Something went wrong when trying to stop threaded agitation. Forcing agitator to stop.'
)
self.stop_agitation()
def start_agitation(self, exp_time=60.0, rot=None):
'''Set the motor voltages for the given number of rotations in exp_time'''
if exp_time <= 0:
self.logger.warning(
'Non-positive exposure time given to agitator object')
self.stop_agitation()
return
if rot is None:
rot = 0.5 * exp_time
if rot <= 0:
self.logger.warning(
'Non-positive rotation number given to agitator object')
self.stop_agitation()
return
freq1 = rot / exp_time
freq2 = 0.9 * rot / exp_time
self._freq = freq1
self.logger.info('Starting agitation at approximately {} Hz'.format(
self._freq))
self.set_voltage1(Motor1.calc_voltage(self.battery_voltage, freq1))
self.set_voltage2(Motor2.calc_voltage(self.battery_voltage, freq2))
def stop_agitation(self, verbose=True):
'''Set both motor voltages to 0'''
if verbose:
self.logger.info('Stopping agitation')
self.set_voltage(0)
self._freq = 0
def set_voltage(self, voltage):
'''Set both motor voltages to the given voltage'''
self.set_voltage1(voltage)
self.set_voltage2(voltage)
# Getter for the frequency
def get_freq(self):
self.logger.info('Requesting frequency')
return self._freq
# Getters and setters for the motor voltages
def get_voltage1(self):
return self._voltage1
def set_voltage1(self, voltage):
battery_voltage = self.battery_voltage
if abs(voltage) > battery_voltage:
voltage = np.sign(voltage) * battery_voltage
if voltage >= 0:
self._rc.ForwardM1(int(voltage / battery_voltage * 127))
else:
self._rc.BackwardM1(int(-voltage / battery_voltage * 127))
self._voltage1 = voltage
voltage1 = property(get_voltage1, set_voltage1)
def get_voltage2(self):
return self._voltage2
def set_voltage2(self, voltage):
battery_voltage = self.battery_voltage
if abs(voltage) > battery_voltage:
voltage = np.sign(voltage) * battery_voltage
if voltage >= 0:
self._rc.ForwardM2(int(voltage / battery_voltage * 127))
else:
self._rc.BackwardM2(int(-voltage / battery_voltage * 127))
self._voltage2 = voltage
voltage2 = property(get_voltage2, set_voltage2)
def get_max_voltage(self):
return self._rc.ReadMinMaxMainVoltages()[2] / 10
def set_max_voltage(self, voltage):
self._rc.SetMainVoltages(int(self.min_voltage * 10), int(voltage * 10))
max_voltage = property(get_max_voltage, set_max_voltage)
def get_min_voltage(self):
return self._rc.ReadMinMaxMainVoltages()[1] / 10
def set_min_voltage(self, voltage):
self._rc.SetMainVoltages(int(voltage * 10), int(self.max_voltage * 10))
min_voltage = property(get_min_voltage, set_min_voltage)
# Getter for the motor controller power source voltage
def get_battery_voltage(self):
return self._rc.ReadMainBatteryVoltage()[1] / 10
battery_voltage = property(get_battery_voltage)
# Getters and setters for the motor currents
def get_current1(self):
return self._rc.ReadCurrents()[1] / 100
current1 = property(get_current1)
def get_current2(self):
return self._rc.ReadCurrents()[2] / 100
current2 = property(get_current2)
def get_max_current1(self):
return self._rc.ReadM1MaxCurrent()[1] / 100
def set_max_current1(self, current):
self._rc.SetM1MaxCurrent(current)
max_current1 = property(get_max_current1, set_max_current1)
def get_max_current2(self):
return self._rc.ReadM2MaxCurrent()[1] / 100
def set_max_current2(self, current):
self._rc.SetM2MaxCurrent(current)
max_current2 = property(get_max_current2, set_max_current2)
# Set motor controller variables to those required by K9
rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS)
rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS)
rc.SetMainVoltages(rc_address, 232, 290) # 23.2V min, 29V max
rc.SetPinFunctions(rc_address, 2, 0, 0)
# Zero the motor encoders
rc.ResetEncoders(rc_address)
# Print Motor PID Settings
m1pid = rc.ReadM1VelocityPID(rc_address)
m2pid = rc.ReadM2VelocityPID(rc_address)
print("M1 P: " + str(m1pid[1]) + ", I:" + str(m1pid[2]) + ", D:" +
str(m1pid[3]))
print("M2 P: " + str(m2pid[1]) + ", I:" + str(m2pid[2]) + ", D:" +
str(m2pid[3]))
# Print Min and Max Main Battery Settings
minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts
print("Min Main Battery: " + str(int(minmaxv[1]) / 10) + "V")
print("Max Main Battery: " + str(int(minmaxv[2]) / 10) + "V")
# Print S3, S4 and S5 Modes
S3mode = [
'Default', 'E-Stop (latching)', 'E-Stop', 'Voltage Clamp',
'Undefined'
]
S4mode = [
'Disabled', 'E-Stop (latching)', 'E-Stop', 'Voltage Clamp',
'M1 Home'
]
S5mode = [
'Disabled', 'E-Stop (latching)', 'E-Stop', 'Voltage Clamp',
'M2 Home'
]
from roboclaw import Roboclaw
rc = Roboclaw("/dev/ttyS3", 115200)
rc.Open()
rc.ReadVersion(0x80)
# This is the old API, use the new one below. rc.SetMinVoltageMainBattery(0x80, 110) # 11 Volts
rc.ReadMinMaxMainVoltages(0x80)
rc.SetMainVoltages(0x80, 110, 340) # Allowed range: 11 V - 34 V
rc.SetM1MaxCurrent(0x80, 500) # 5 Amps
rc.SetPWMMode(0x80, 0) # Locked Antiphase
#rc.ReadPWMMode(0x80)
rc.SetM1EncoderMode(0x80, 0) # No RC/Analog support + Quadrature encoder
#rc.ReadEncoderModes(0x80)
getConfig = rc.GetConfig(0x80)
config = getConfig[1] # index zero is 1 for success, 0 for failure.
config = config | 0x0003 # Packet serial mode
config = config | 0x8000 # Multi-Unit mode
rc.SetConfig(0x80, config)
rc.SetPinFunctions(0x80, 2, 0, 0) # S3 = E-Stop, S4 = Disabled, S5 = Disabled
rc.WriteNVM(0x80)
rc.ReadEncM1(0x80)
rc.ResetEncoders(0x80)
rc.ReadEncM1(0x80)
p = 15000
i = 1000