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)
rc.ReadEncM1(0x80) rc.ResetEncoders(0x80) rc.ReadEncM1(0x80) p = 15000 i = 1000 d = 500 qpps = 3000 rc.SetM1VelocityPID(0x80, p, i, d, qpps) rc.ReadM1VelocityPID(0x80) rc.SpeedM1(0x80, 250) rc.ReadM1MaxCurrent(0x80) rc.ReadCurrents(0x80) error = rc.ReadError(0x80)[1] format(error, "08b") # "Position" API appears to be for times when we want to act like # a servo motor. Probably not applicable here. """ P = 600 I = 0 D = 400 maxI = ? deadZone = 10 minPos = 0
