def test_clear_state(self):
s = AxisState("READY")
s.clear()
self.assertEquals(s, "UNKNOWN")
s.set("MOVING")
self.assertEquals(s, "MOVING")
def test_clear_state():
s = AxisState("READY")
s.clear()
assert s == "UNKNOWN"
s.set("MOVING")
assert s == "MOVING"
class IcePAP(Controller):
"""Implement IcePAP stepper motor controller access"""
default_group = None
default_groupenc = None
def __init__(self, name, config, axes, encoders):
"""Contructor"""
Controller.__init__(self, name, config, axes, encoders)
self.libdevice = None
def initialize(self):
"""Controller initialization"""
self.log_info("initialize() called")
# Get controller config from bliss config
# Mandatory parameters (port number is not needed)
self.host = self.config.get("host")
# Optional parameters
try:
self.libdebug = int(self.config.get("libdebug"))
except:
self.libdebug = 1
# Create an IcePAP lib object to access the MASTER
self.libdevice = libicepap.System(
self.host,
"verb=%d" %
self.libdebug)
# Create an IcePAP lib object as default group
if IcePAP.default_group is None:
IcePAP.default_group = libicepap.Group("default")
self.libgroup = IcePAP.default_group
# Create an IcePAP lib object as default group for encoders
if IcePAP.default_groupenc is None:
IcePAP.default_groupenc = libicepap.Group("default_enc")
self.libgroupenc = IcePAP.default_groupenc
# Create custom EMotion states to map IcePAP status bits
self.icestate = AxisState()
self.icestate.create_state("POWEROFF", "motor power is off")
for code in libicepap.STATUS_DISCODE_STR:
self.icestate.create_state(
libicepap.STATUS_DISCODE_STR[code],
libicepap.STATUS_DISCODE_DSC[code])
for code in libicepap.STATUS_MODCODE_STR:
self.icestate.create_state(
libicepap.STATUS_MODCODE_STR[code],
libicepap.STATUS_MODCODE_DSC[code])
for code in libicepap.STATUS_STOPCODE_STR:
self.icestate.create_state(
libicepap.STATUS_STOPCODE_STR[code],
libicepap.STATUS_STOPCODE_DSC[code])
def finalize(self):
"""Controller no more needed"""
self.log_info("finalize() called")
# Remove any group in the IcePAP lib
try:
self.libgroup.delete()
self.libgroupenc.delete()
except:
pass
# Close IcePAP lib socket/threads
if self.libdevice is not None:
self.libdevice.close()
def initialize_axis(self, axis):
"""Axis initialization"""
self.log_info("initialize_axis() called for axis %r" % axis.name)
# Get axis config from bliss config
# address form is XY : X=rack {0..?} Y=driver {1..8}
axis.address = axis.config.get("address", int)
# Create an IcePAP lib axis object
device = self.libdevice
address = axis.address
name = axis.name
axis.libaxis = libicepap.Axis(device, address, name)
# Add the axis to the default IcePAP lib group
self.libgroup.add_axis(axis.libaxis)
# Initialiaze hardware
# if set_power fails, display exception but let axis
# be created properly
try:
self.libgroup.set_power(libicepap.ON, axis.libaxis)
except:
sys.excepthook(*sys.exc_info())
# Add new axis oject methods
add_axis_method(axis, self.get_identifier, types_info=("None", "str"))
def set_on(self, axis):
"""Switch power on"""
self.libgroup.set_power(libicepap.ON, axis.libaxis)
def set_off(self, axis):
"""Switch power off"""
self.libgroup.set_power(libicepap.OFF, axis.libaxis)
def read_position(self, axis):
"""Returns axis position in motor units"""
self.log_info("position() called for axis %r" % axis.name)
return self.libgroup.pos(axis.libaxis)
def set_position(self, axis, new_pos):
"""Set axis position to a new value given in motor units"""
l = libicepap.PosList()
l[axis.libaxis] = new_pos
self.libgroup.pos(l)
return self.read_position(axis)
def read_velocity(self, axis):
"""Returns axis current velocity in user units/sec"""
#TODO: wouldn't be better in steps/s ?
return self.libgroup.velocity(axis.libaxis)
def set_velocity(self, axis, new_velocity):
"""Set axis velocity given in units/sec"""
self.log_info("set_velocity(%fstps/sec) called for axis %r" %
(new_velocity, axis.name))
l = libicepap.VelList()
l[axis.libaxis] = new_velocity
self.libgroup.velocity(l)
# Always return the current velocity
return self.read_velocity(axis)
def read_acceleration(self, axis):
"""Returns axis current acceleration in steps/sec2"""
acctime = self.libgroup.acctime(axis.libaxis)
velocity = self.read_velocity(axis)
return velocity/acctime
def set_acceleration(self, axis, new_acc):
"""Set axis acceleration given in steps/sec2"""
self.log_info("set_acceleration(%fstps/sec2) called for axis %r" %
(new_acc, axis.name))
velocity = self.read_velocity(axis)
new_acctime = velocity/new_acc
self.log_info("set_acctime(%fsec) called for axis %r" %
(new_acctime, axis.name))
l = libicepap.AcctimeList()
l[axis.libaxis] = new_acctime
self.libgroup.acctime(l)
# Always return the current acceleration
return self.read_acceleration(axis)
def state(self, axis):
"""Returns the current axis state"""
self.log_info("state() called for axis %r" % axis.name)
# The axis can only be accessed through a group in IcePAP lib
# Use the default group
status = self.libgroup.status(axis.libaxis)
# Convert status from icepaplib to bliss format.
self.icestate.clear()
# first all concurrent states
if(libicepap.status_lowlim(status)):
self.icestate.set("LIMNEG")
if(libicepap.status_highlim(status)):
self.icestate.set("LIMPOS")
if(libicepap.status_home(status)):
self.icestate.set("HOME")
modcod, modstr, moddsc = libicepap.status_get_mode(status)
if modcod != None:
self.icestate.set(modstr)
sccod, scstr, scdsc = libicepap.status_get_stopcode(status)
if sccod != None:
self.icestate.set(scstr)
if(libicepap.status_isready(status)):
self.icestate.set("READY")
# if motor is ready then no need to investigate deeper
return self.icestate
if(libicepap.status_ismoving(status)):
self.icestate.set("MOVING")
if(not libicepap.status_ispoweron(status)):
self.icestate.set("POWEROFF")
discod, disstr, disdsc = libicepap.status_get_disable(status)
if discod != None:
self.icestate.set(disstr)
if not self.icestate.MOVING:
# it seems it is not safe to call warning and/or alarm commands
# while homing motor, so let's not ask if motor is moving
if(libicepap.status_warning(status)):
warn_str = self.libgroup.warning(axis.libaxis)
warn_dsc = "warning condition: \n" + str(warn_str)
self.icestate.create_state("WARNING", warn_dsc)
self.icestate.set("WARNING")
alarm_str = self.libgroup.alarm(axis.libaxis)
if alarm_str != 'NO':
alarm_dsc = "alarm condition: " + str(alarm_str)
self.icestate.create_state("ALARMDESC", alarm_dsc)
self.icestate.set("ALARMDESC")
return self.icestate
def prepare_move(self, motion):
"""
Called once before a single axis motion,
positions in motor units
"""
self.log_info("prepare_move(%fstps) called for axis %r" %
(motion.target_pos, motion.axis.name))
pass
def start_one(self, motion):
"""
Called on a single axis motion,
returns immediately,
positions in motor units
"""
self.log_info("start_one(%fsteps) called for axis %r" %
(motion.target_pos, motion.axis.name))
target_positions = libicepap.PosList()
target_positions[motion.axis.libaxis] = motion.target_pos
self.libgroup.move(target_positions)
def start_all(self, *motion_list):
"""
Called once per controller with all the axis to move
returns immediately,
positions in motor units
"""
self.log_info("start_all() called")
target_positions = libicepap.PosList()
for motion in motion_list:
target_positions[motion.axis.libaxis] = motion.target_pos
self.libgroup.move(target_positions)
def stop(self, axis):
"""Stops smoothly an axis motion"""
self.log_info("stop() called for axis %r" % axis.name)
self.libgroup.stop(axis.libaxis)
def stop_all(self, *motion_list):
"""Stops smoothly all the moving axis given"""
self.log_info("stop_all() called")
axis_list = []
for motion in motion_list:
axis_list.append(motion.axis.libaxis)
self.libgroup.stop(axis_list)
def home_search(self, axis, switch):
"""Launch a homing sequence"""
cmd = "HOME " + ("+1" if switch > 0 else "-1")
# TODO: MP17Nov14: missing argin on position to set at home
# TODO: MP17Nov14: missing home search in IcePAP library
self.libgroup.ackcommand(cmd, axis.libaxis)
# IcePAP status is not immediately MOVING after home search command is sent
time.sleep(0.2)
def home_state(self, axis):
"""Returns the current axis state while homing"""
return self.state(axis)
def limit_search(self, axis, limit):
"""
Launch a limitswitch search sequence
the sign of the argin gives the search direction
"""
cmd = "SRCH"
if limit>0:
cmd += " LIM+"
else:
cmd += " LIM-"
# TODO: MP17Nov14: missing limit search in IcePAP library
self.libgroup.ackcommand(cmd, axis.libaxis)
# TODO: MG18Nov14: remove this sleep (state is not immediately MOVING)
time.sleep(0.1)
def initialize_encoder(self, encoder):
"""Encoder initialization"""
self.log_info("initialize_encoder() called for axis %r" % encoder.name)
# Get axis config from bliss config
# address form is XY : X=rack {0..?} Y=driver {1..8}
encoder.address = encoder.config.get("address", int)
# Get optional encoder input to read
try:
enctype = string.upper(encoder.config.get("type"))
except:
enctype = "ENCIN"
# Minium check on encoder input
if enctype not in ['ENCIN', 'ABSENC', 'INPOS', 'MOTOR']:
raise ValueError('Invalid encoder type')
encoder.enctype = enctype
# Create an IcePAP lib axis object for each encoder
# as there is no encoder objects in the lib
device = self.libdevice
address = encoder.address
name = encoder.name
encoder.libaxis = libicepap.Axis(device, address, name)
# Add the axis to the default IcePAP lib group
self.libgroupenc.add_axis(encoder.libaxis)
def read_encoder(self, encoder):
"""Returns encoder position in encoder units"""
self.log_info("read_encoder() called for encoder %r" % encoder.name)
# Prepare the command to acces directly the encoder input
cmd = ' '.join(['?ENC', encoder.enctype])
ret = self.libgroupenc.command(cmd, encoder.libaxis)
# Return encoder steps
return int(ret)
def set_encoder(self, encoder, steps):
"""Set encoder position to a new value given in encoder units"""
self.log_info("set_encoder(%f) called for encoder %r" %
(steps, encoder.name))
# Prepare the command to acces directly the encoder input
cmd = ' '.join(['ENC', encoder.enctype, '%d'%steps])
self.libgroupenc.ackcommand(cmd, encoder.libaxis)
# No need to return the current encoder position
return
def log_level(self, lvl):
"""Changes IcePAP and eMotion libraries verbose level"""
# Change in the eMotion library
log.level(lvl)
# Value mapping between the two libraries
# eMotion == IcePAP
# NOTSET == 0 == 0 == DBG_NONE
# DEBUG == 10 == 4 == DBG_DATA
# INFO == 20 == 2 == DBG_TRACE
# WARNING== 30 ==
# ERROR == 40 == 1 == DBG_ERROR
# CRITIC == 50 ==
#
val = {
log.NOTSET: 0,
log.DEBUG: 4,
log.INFO: 2,
log.ERROR: 1,
}[lvl]
# Change in the IcePAP library
self.libdevice.set_verbose(val)
# Always return the current eMotion level
self.log_info("log_level(%s) called, lib(%d)" %
(log.getLevelName(lvl), val))
return log.level()
def log_error(self, msg):
"""Logging method"""
log.error(_ICEPAP_TAB + msg)
def log_info(self, msg):
"""Logging method"""
log.info(_ICEPAP_TAB + msg)
def get_identifier(self, axis):
"""Returns the unique string identifier of the specified axis"""
self.log_info("get_identifier() called for axis %r" % axis.name)
return self.libgroup.command("?ID", axis.libaxis)
class Mockup(Controller):
def __init__(self, name, config, axes, encoders):
Controller.__init__(self, name, config, axes, encoders)
self._axis_moves = {}
self.__encoders = {}
self.__error_mode = False
self._hw_status = AxisState("READY")
self.__hw_limit = (None, None)
self._hw_status.create_state("PARKED", "mot au parking")
# Access to the config.
try:
self.host = self.config.get("host")
except:
elog.debug("no 'host' defined in config for %s" % name)
# Adds Mockup-specific settings.
self.axis_settings.add('init_count', int)
self.axis_settings.add('atrubi', float)
self.axis_settings.add('round_earth', bool)
self.axis_settings.add('geocentrisme', bool)
"""
Controller initialization actions.
"""
def initialize(self):
# hardware initialization
for axis_name, axis in self.axes.iteritems():
axis.settings.set('init_count', 0)
axis.settings.set('atrubi', 777)
axis.settings.set('round_earth', True)
axis.settings.set('geocentrisme', False)
axis.__vel = None
axis.__acc = None
"""
Axes initialization actions.
"""
def initialize_axis(self, axis):
def set_pos(move_done, axis=axis):
if move_done:
self.set_position(axis, axis.dial()*axis.steps_per_unit)
self._axis_moves[axis] = {
"measured_simul": False,
"measured_noise": 0.0,
"end_t": 0,
"end_pos": 0,
"move_done_cb": set_pos }
event.connect(axis, "move_done", set_pos)
# this is to test axis are initialized only once
axis.settings.set('init_count', axis.settings.get('init_count') + 1)
# Add new axis oject methods as tango commands.
add_axis_method(axis, self.custom_get_twice, types_info=("int", "int"))
add_axis_method(axis, self.custom_get_chapi, types_info=("str", "str"))
add_axis_method(axis, self.custom_send_command, types_info=("str", "None"))
add_axis_method(axis, self.custom_command_no_types, types_info=("None", "None"))
add_axis_method(axis, self.custom_set_measured_noise, types_info=("float", "None"))
add_axis_method(axis, self._set_closed_loop, name = "Set_Closed_Loop", types_info = (bool, None))
add_axis_method(axis, self.put_discrepancy, types_info=("int", "None"))
if axis.encoder:
self.__encoders.setdefault(axis.encoder, {})["axis"] = axis
def initialize_encoder(self, encoder):
self.__encoders.setdefault(encoder, {})["measured_noise"] = 0.0
self.__encoders[encoder]["steps"] = None
"""
Actions to perform at controller closing.
"""
def finalize(self):
pass
def set_hw_limit(self, axis, low_limit, high_limit):
if low_limit is not None:
ll= axis.user2dial(low_limit)*axis.steps_per_unit
else:
ll = None
if high_limit is not None:
hl = axis.user2dial(high_limit)*axis.steps_per_unit
else:
hl = None
if hl is not None and hl < ll:
self.__hw_limit = (hl, ll)
else:
self.__hw_limit = (ll, hl)
def start_all(self, *motion_list):
if self.__error_mode:
raise RuntimeError("Cannot start because error mode is set")
t0 = time.time()
for motion in motion_list:
self.start_one(motion, t0=t0)
def start_one(self, motion, t0=None):
if self.__error_mode:
raise RuntimeError("Cannot start because error mode is set")
axis = motion.axis
t0 = t0 or time.time()
pos = self.read_position(axis)
v = self.read_velocity(axis)
ll, hl = self.__hw_limit
if hl is not None and motion.target_pos > hl:
d=hl-motion.target_pos
motion.delta -= d
motion.target_pos = hl
if ll is not None and motion.target_pos < ll:
d=ll-motion.target_pos
motion.delta -= d
motion.target_pos = ll
self._axis_moves[axis].update({
"start_pos": pos,
"delta": motion.delta,
"end_pos": motion.target_pos,
"end_t": t0 +
math.fabs(
motion.delta) /
float(v),
"t0": t0})
def read_position(self, axis):
"""
Returns the position (measured or desired) taken from controller
in controller unit (steps).
"""
# handle read out during a motion
if self._axis_moves[axis]["end_t"]:
# motor is moving
t = time.time()
v = self.read_velocity(axis)
d = math.copysign(1, self._axis_moves[axis]["delta"])
dt = t - self._axis_moves[axis]["t0"] # t0=time at start_one.
pos = self._axis_moves[axis]["start_pos"] + d * dt * v
else:
pos = self._axis_moves[axis]["end_pos"]
return int(round(pos))
def read_encoder(self, encoder):
"""
returns encoder position.
unit : 'encoder steps'
"""
axis = self.__encoders[encoder]["axis"]
if self.__encoders[encoder]["measured_noise"] != 0.0:
# Simulates noisy encoder.
amplitude = self.__encoders[encoder]["measured_noise"]
noise_mm = random.uniform(-amplitude, amplitude)
_pos = self.read_position(axis) / axis.steps_per_unit
_pos += noise_mm
self.__encoders[encoder]["steps"] = _pos * encoder.steps_per_unit
else:
# "Perfect encoder"
if self.__encoders[encoder]["steps"] is None:
self.__encoders[encoder]["steps"] = self.read_position(axis) / axis.steps_per_unit
return self.__encoders[encoder]["steps"]
def set_encoder(self, encoder, encoder_steps):
self.__encoders[encoder]["steps"]=encoder_steps
"""
VELOCITY
"""
def read_velocity(self, axis):
"""
Returns the current velocity taken from controller
in motor units.
"""
return axis.__vel
def set_velocity(self, axis, new_velocity):
"""
<new_velocity> is in motor units
"""
axis.__vel = new_velocity
"""
ACCELERATION
"""
def read_acceleration(self, axis):
"""
must return acceleration in controller units / s2
"""
return axis.__acc
def set_acceleration(self, axis, new_acceleration):
"""
<new_acceleration> is in controller units / s2
"""
axis.__acc = new_acceleration
"""
ON / OFF
"""
def set_on(self, axis):
self._hw_status = "READY"
def set_off(self, axis):
self._hw_status = "OFF"
"""
Hard limits
"""
def _check_hw_limits(self, axis):
ll, hl = self.__hw_limit
pos = self.read_position(axis)
if ll is not None and pos <= ll:
return AxisState("READY", "LIMNEG")
elif hl is not None and pos >= hl:
return AxisState("READY", "LIMPOS")
return AxisState("READY")
"""
STATE
"""
def state(self, axis):
if self._hw_status == "PARKED":
return AxisState("PARKED")
if self._hw_status == "OFF":
return AxisState("OFF")
if self._axis_moves[axis]["end_t"] > time.time():
return AxisState("MOVING")
else:
self._axis_moves[axis]["end_t"]=0
return self._check_hw_limits(axis)
"""
Must send a command to the controller to abort the motion of given axis.
"""
def stop(self, axis):
self._axis_moves[axis]["end_pos"] = self.read_position(axis)
self._axis_moves[axis]["end_t"] = 0
def stop_all(self, *motion_list):
for motion in motion_list:
axis = motion.axis
self._axis_moves[axis]["end_pos"] = self.read_position(axis)
self._axis_moves[axis]["end_t"] = 0
"""
HOME and limits search
"""
def home_search(self, axis, switch):
self._axis_moves[axis]["start_pos"] = self._axis_moves[axis]["end_pos"]
self._axis_moves[axis]["end_pos"] = 0
self._axis_moves[axis]["delta"] = 0
self._axis_moves[axis]["end_t"] = 0
self._axis_moves[axis]["t0"] = time.time()
self._axis_moves[axis]["home_search_start_time"] = time.time()
# def home_set_hardware_position(self, axis, home_pos):
# raise NotImplementedError
def home_state(self, axis):
if(time.time() - self._axis_moves[axis]["home_search_start_time"]) > 2:
return AxisState("READY")
else:
return AxisState("MOVING")
def limit_search(self, axis, limit):
self._axis_moves[axis]["start_pos"] = self._axis_moves[axis]["end_pos"]
self._axis_moves[axis]["end_pos"] = 1E6 if limit > 0 else -1E6
self._axis_moves[axis]["delta"] = self._axis_moves[axis]["end_pos"] #this is just for direction sign
self._axis_moves[axis]["end_pos"] *= axis.steps_per_unit
self._axis_moves[axis]["end_t"] = time.time() + 2
self._axis_moves[axis]["t0"] = time.time()
def get_info(self, axis):
return "turlututu chapo pointu : %s" % (axis.name)
def raw_write(self, axis, com):
print ("raw_write: com = %s" % com)
def raw_write_read(self, axis, com):
return com + ">-<" + com
def set_position(self, axis, pos):
self._axis_moves[axis]["end_pos"] = pos
self._axis_moves[axis]["end_t"] = 0
return pos
def put_discrepancy(self, axis, disc):
self._axis_moves[axis]["end_pos"] += disc
"""
Custom axis methods
"""
# VOID VOID
@axis_method
def custom_park(self, axis):
print "parking"
self._hw_status.clear()
self._hw_status.set("PARKED")
# VOID LONG
@axis_method(types_info=("None", "int"))
def custom_get_forty_two(self, axis):
return 42
# LONG LONG
def custom_get_twice(self, axis, LongValue):
return LongValue * 2
# STRING STRING
def custom_get_chapi(self, axis, value):
if value == "chapi":
return "chapo"
elif value == "titi":
return "toto"
else:
return "bla"
# STRING VOID
def custom_send_command(self, axis, value):
print "command=", value
# BOOL NONE
def _set_closed_loop(self, axis, onoff = True):
print "I set the closed loop ", onoff
# Types by default
def custom_command_no_types(self, axis):
print "print with no types"
def custom_set_measured_noise(self, axis, noise):
"""
Custom axis method to add a random noise, given in user units,
to measured positions. Set noise value to 0 to have a measured
position equal to target position.
By the way we add a ref to the coresponding axis.
"""
self.__encoders[axis.encoder]["measured_noise"] = noise
self.__encoders[axis.encoder]["axis"] = axis
def set_error(self, error_mode):
self.__error_mode = error_mode
class Mockup(Controller):
def __init__(self, *args, **kwargs):
Controller.__init__(self, *args, **kwargs)
self._axis_moves = {}
self.__encoders = {}
# Custom attributes.
self.__voltages = {}
self.__cust_attr_float = {}
self.__error_mode = False
self._hw_state = AxisState("READY")
self.__hw_limit = (None, None)
self._hw_state.create_state("PARKED", "mot au parking")
# Access to the config.
try:
self.host = self.config.get("host")
except:
elog.debug("no 'host' defined in config for %s" % self.name)
# Adds Mockup-specific settings.
self.axis_settings.add('init_count', int)
self.axis_settings.add('hw_position', float)
"""
Controller initialization actions.
"""
def initialize(self):
# hardware initialization
for axis_name, axis in self.axes.iteritems():
axis.settings.set('init_count', 0)
"""
Axes initialization actions.
"""
def initialize_axis(self, axis):
# this is to protect position reading,
# indeed the mockup controller uses redis to store
# a 'hardware position', and it is not allowed
# to read a position before it has been written
def set_pos(move_done, axis=axis):
if move_done:
self.set_position(axis, axis.dial()*axis.steps_per_unit)
self._axis_moves[axis] = {
"end_t": None,
"move_done_cb": set_pos }
if axis.settings.get('hw_position') is None:
axis.settings.set('hw_position', 0)
self._axis_moves[axis]['start_pos'] = self.read_position(axis)
self._axis_moves[axis]['target'] = self._axis_moves[axis]['start_pos']
event.connect(axis, "move_done", set_pos)
self.__voltages[axis] = axis.config.get("default_voltage",
int, default=220)
self.__cust_attr_float[axis] = axis.config.get("default_cust_attr",
float, default=3.14)
# this is to test axis are initialized only once
axis.settings.set('init_count', axis.settings.get('init_count') + 1)
if axis.encoder:
self.__encoders.setdefault(axis.encoder, {})["axis"] = axis
def initialize_encoder(self, encoder):
self.__encoders.setdefault(encoder, {})["measured_noise"] = None
self.__encoders[encoder]["steps"] = None
"""
Actions to perform at controller closing.
"""
def finalize(self):
pass
def set_hw_limits(self, axis, low_limit, high_limit):
if low_limit is not None:
ll= axis.user2dial(low_limit)*axis.steps_per_unit
else:
ll = None
if high_limit is not None:
hl = axis.user2dial(high_limit)*axis.steps_per_unit
else:
hl = None
if hl is not None and hl < ll:
self.__hw_limit = (hl, ll)
else:
self.__hw_limit = (ll, hl)
def start_all(self, *motion_list):
if self.__error_mode:
raise RuntimeError("Cannot start because error mode is set")
t0 = time.time()
for motion in motion_list:
self.start_one(motion, t0=t0)
def start_one(self, motion, t0=None):
if self.__error_mode:
raise RuntimeError("Cannot start because error mode is set")
axis = motion.axis
t0 = t0 or time.time()
pos = self.read_position(axis)
v = self.read_velocity(axis)
ll, hl = self.__hw_limit
end_pos = motion.target_pos
if hl is not None and end_pos > hl:
end_pos = hl
if ll is not None and end_pos < ll:
end_pos = ll
delta = motion.delta + end_pos - motion.target_pos
self._axis_moves[axis].update({
"start_pos": pos,
"delta": delta,
"end_t": t0 + math.fabs(delta) / float(v),
"target": end_pos,
"t0": t0})
def start_jog(self, axis, velocity, direction):
t0 = time.time()
pos = self.read_position(axis)
self.set_velocity(axis, velocity)
self._axis_moves[axis].update({
"start_pos": pos,
"delta": direction,
"end_t": t0+1E9,
"t0": t0})
def read_position(self, axis, t=None):
"""
Returns the position (measured or desired) taken from controller
in controller unit (steps).
"""
t = t or time.time()
# handle read out during a motion
end_t = self._axis_moves[axis]["end_t"]
if end_t is not None and t >= end_t:
pos = self._axis_moves[axis]["target"]
axis.settings.set('hw_position', pos)
elif end_t:
# motor is moving
v = self.read_velocity(axis)
a = self.read_acceleration(axis)
d = math.copysign(1, self._axis_moves[axis]["delta"])
dt = t - self._axis_moves[axis]["t0"] # t0=time at start_one.
acctime = min(dt, v/a)
dt -= acctime
pos = self._axis_moves[axis]["start_pos"] + d*a*0.5*acctime**2
if dt > 0:
pos += d * dt * v
axis.settings.set('hw_position', pos)
else:
pos = axis.settings.get('hw_position')
return int(round(pos))
def read_encoder(self, encoder):
"""
returns encoder position.
unit : 'encoder steps'
"""
if self.__encoders[encoder]["steps"] is not None:
enc_steps = self.__encoders[encoder]["steps"]
else:
axis = self.__encoders[encoder]["axis"]
_pos = self.read_position(axis) / float(axis.steps_per_unit)
if self.__encoders[encoder]["measured_noise"] > 0:
# Simulates noisy encoder.
amplitude = self.__encoders[encoder]["measured_noise"]
noise_mm = random.uniform(-amplitude, amplitude)
_pos += noise_mm
enc_steps = _pos * encoder.steps_per_unit
else:
# "Perfect" encoder
enc_steps = _pos * encoder.steps_per_unit
self.__encoders[encoder]["steps"] = None
return enc_steps
def set_encoder(self, encoder, encoder_steps):
self.__encoders[encoder]["steps"] = encoder_steps
"""
VELOCITY
"""
def read_velocity(self, axis):
"""
Returns the current velocity taken from controller
in motor units.
"""
return axis.settings.get('velocity')*abs(axis.steps_per_unit)
def set_velocity(self, axis, new_velocity):
"""
<new_velocity> is in motor units
"""
vel = new_velocity/abs(axis.steps_per_unit)
axis.settings.set('velocity', vel)
return vel
"""
ACCELERATION
"""
def read_acceleration(self, axis):
"""
must return acceleration in controller units / s2
"""
return axis.settings.get('acceleration')*abs(axis.steps_per_unit)
def set_acceleration(self, axis, new_acceleration):
"""
<new_acceleration> is in controller units / s2
"""
acc = new_acceleration/abs(axis.steps_per_unit)
axis.settings.set('acceleration', acc)
return acc
"""
ON / OFF
"""
def set_on(self, axis):
self._hw_state.clear()
self._hw_state.set("READY")
def set_off(self, axis):
self._hw_state.set("OFF")
"""
Hard limits
"""
def _check_hw_limits(self, axis):
ll, hl = self.__hw_limit
pos = self.read_position(axis)
if ll is not None and pos <= ll:
return AxisState("READY", "LIMNEG")
elif hl is not None and pos >= hl:
return AxisState("READY", "LIMPOS")
if self._hw_state == "OFF":
return AxisState("OFF")
else:
s = AxisState(self._hw_state)
s.set("READY")
return s
"""
STATE
"""
def state(self, axis):
if self._axis_moves[axis]["end_t"] > time.time():
return AxisState("MOVING")
else:
self.read_position(axis, t=self._axis_moves[axis]["end_t"])
self._axis_moves[axis]["end_t"] = None
self._axis_moves[axis]["delta"] = 0
return self._check_hw_limits(axis)
"""
Must send a command to the controller to abort the motion of given axis.
"""
def stop(self, axis, t=None):
if self._axis_moves[axis]["end_t"]:
self._axis_moves[axis]["target"] = self.read_position(axis, t=t)
self._axis_moves[axis]["end_t"] = None
def stop_all(self, *motion_list):
t = time.time()
for motion in motion_list:
self.stop(motion.axis, t=t)
"""
HOME and limits search
"""
def home_search(self, axis, switch):
self._axis_moves[axis]["delta"] = switch
self._axis_moves[axis]["end_t"] = None
self._axis_moves[axis]["t0"] = time.time()
self._axis_moves[axis]["home_search_start_time"] = time.time()
# def home_set_hardware_position(self, axis, home_pos):
# raise NotImplementedError
def home_state(self, axis):
if(time.time() - self._axis_moves[axis]["home_search_start_time"]) > 1:
axis.settings.set("hw_position", 0)
return AxisState("READY")
else:
return AxisState("MOVING")
def limit_search(self, axis, limit):
self._axis_moves[axis]["target"] = 1e6*limit*axis.steps_per_unit
self._axis_moves[axis]["delta"] = limit
self._axis_moves[axis]["end_t"] = time.time() + 1
self._axis_moves[axis]["t0"] = time.time()
def get_info(self, axis):
return "turlututu chapo pointu : %s" % (axis.name)
def get_id(self, axis):
return "MOCKUP AXIS %s" % (axis.name)
def set_position(self, axis, pos):
if self._axis_moves[axis]["end_t"]:
raise RuntimeError("Cannot set position while moving !")
axis.settings.set('hw_position', pos)
self._axis_moves[axis]['target'] = pos
self._axis_moves[axis]["end_t"] = None
return pos
def put_discrepancy(self, axis, disc):
self.set_position(axis, self.read_position(axis)+disc)
"""
Custom axis methods
"""
# VOID VOID
@object_method
def custom_park(self, axis):
elog.debug("custom_park : parking")
self._hw_state.set("PARKED")
# VOID LONG
@object_method(types_info=("None", "int"))
def custom_get_forty_two(self, axis):
return 42
# LONG LONG + renaming.
@object_method(name= "CustomGetTwice", types_info=("int", "int"))
def custom_get_twice(self, axis, LongValue):
return LongValue * 2
# STRING STRING
@object_method(types_info=("str", "str"))
def custom_get_chapi(self, axis, value):
if value == "chapi":
return "chapo"
elif value == "titi":
return "toto"
else:
return "bla"
# STRING VOID
@object_method(types_info=("str", "None"))
def custom_send_command(self, axis, value):
elog.debug("custom_send_command(axis=%s value=%r):" % (axis.name, value))
# BOOL NONE
@object_method(name="Set_Closed_Loop", types_info=("bool", "None"))
def _set_closed_loop(self, axis, onoff = True):
pass #print "I set the closed loop ", onoff
# Types by default (None, None)
@object_method
def custom_command_no_types(self, axis):
print "print with no types"
@object_method
def generate_error(self, axis):
# For testing purposes.
raise RuntimeError("Testing Error")
def custom_get_measured_noise(self, axis):
noise = 0.0
if not axis.encoder in self.__encoders:
raise KeyError("cannot read measured noise: %s "
"doesn't have encoder" % axis.name)
noise = self.__encoders[axis.encoder].get("measured_noise", None)
@object_method(types_info=("float", "None"))
def custom_set_measured_noise(self, axis, noise):
"""
Custom axis method to add a random noise, given in user units,
to measured positions. Set noise value to 0 to have a measured
position equal to target position.
By the way we add a ref to the coresponding axis.
"""
self.__encoders[axis.encoder]["measured_noise"] = noise
self.__encoders[axis.encoder]["axis"] = axis
def set_error(self, error_mode):
self.__error_mode = error_mode
"""
Custom attributes methods
"""
@object_attribute_get(type_info="int")
def get_voltage(self, axis):
return self.__voltages.setdefault(axis, 10000)
@object_attribute_set(type_info="int")
def set_voltage(self, axis, voltage):
self.__voltages[axis] = voltage
@object_attribute_get(type_info="float")
def get_cust_attr_float(self, axis):
return self.__cust_attr_float.setdefault(axis, 9.999)
@object_attribute_set(type_info="float")
def set_cust_attr_float(self, axis, value):
self.__cust_attr_float[axis] = value
