def state(self):
if self.is_moving:
return AxisState("MOVING")
grp_state = AxisState("READY")
for i, (name, state) in enumerate([
(axis.name, axis.state()) for axis in self._axes.itervalues()
]):
if state.MOVING:
new_state = "MOVING" + " " * i
grp_state.create_state(
new_state,
"%s: %s" % (name, grp_state._state_desc["MOVING"]))
grp_state.set("MOVING")
grp_state.set(new_state)
for axis_state in state._current_states:
if axis_state == "READY":
continue
new_state = axis_state + " " * i
grp_state.create_state(
new_state,
"%s: %s" % (name, state._state_desc[axis_state]))
grp_state.set(new_state)
return grp_state
def test_custom_state():
s = AxisState()
s.set("READY")
s.create_state("PARKED", "here I am")
s.set("PARKED")
assert s.READY
assert s == "PARKED"
def test_states(self):
# empty state
s = AxisState()
self.assertEquals(s, "UNKNOWN")
# moving
s.set("MOVING")
self.assertEquals(s, "MOVING")
# moving => not ready
self.assertFalse(s.READY)
# now ready but no more moving
s.set("READY")
self.assertTrue(s.READY)
self.assertFalse(s.MOVING)
# custom state
s.create_state("PARKED", "c'est ma place !!")
s.set("PARKED")
# still ready
self.assertTrue(s.READY)
self.assertEquals(s, "PARKED")
# Prints string of states.
self.assertTrue(isinstance(s.current_states(), str))
# bad name for a state
self.assertRaises(ValueError, s.create_state, "A bad state")
def state(self):
if self.is_moving:
return AxisState("MOVING")
grp_state = AxisState("READY")
for i, (name, state) in enumerate([(axis.name, axis.state()) for axis in self._axes.itervalues()]):
if state.MOVING:
new_state = "MOVING"+" "*i
grp_state.create_state(new_state, "%s: %s" % (name, grp_state._state_desc["MOVING"]))
grp_state.set("MOVING")
grp_state.set(new_state)
for axis_state in state._current_states:
if axis_state == "READY":
continue
new_state = axis_state+" "*i
grp_state.create_state(new_state, "%s: %s" % (name, state._state_desc[axis_state]))
grp_state.set(new_state)
return grp_state
class Elmo(Controller):
"""
Elmo motor controller
configuration example:
- class: elmo
udp:
url: nscopeelmo
axes:
- name: rot
steps_per_unit: 26222.2
velocity: 377600
acceleration: 755200
control_slave: True
"""
ErrorCode = {
1: 'Do not Update',
2: 'Bad Command',
3: 'Bad Index',
5: 'BEYOND_ALPHA_BET (Has No Interpreter Meaning',
6: 'Program is not running',
7: 'Mode cannot be started - bad initialization data',
8: 'Motion terminated, probably data underflow',
9: 'CAN message was lost',
10: 'Cannot be used by PDO',
11: 'Cannot write to flash memory',
12: 'Command not available in this unit mode',
13: 'Cannot reset communication - UART is busy',
14: 'Cannot perform CAN NMT service',
15: 'CAN Life time exceeded - motor shut down',
16: "The command attribute is array '[]' is expected",
17: 'Format of UL command is not valid - check the command definition',
18: 'Empty Assign',
19: 'Command syntax error',
21: 'Operand Out of Range',
22: 'Zero Division',
23: 'Command cannot be assigned',
24: 'Bad Operation',
25: 'Command Not Valid While Moving',
26: 'Profiler mode not supported in this unit mode (UM)',
28: 'Out Of Limit',
29: 'CAN set object return an abort when called from interpreter',
30: 'No program to continue',
31: 'CAN get object return an abort when called from interpreter',
32: 'Communication overrun, parity, noise, or framing error',
33: 'Bad sensor setting',
34: 'There is a conflict with another command',
36:
'Commutation method (CA[17]) or commutation table does not fit to sensor',
37: 'Two Or More Hall sensors are defined to the same place',
38: 'Motion start specified for the past',
41: 'Command is not supported by this product',
42: 'No Such Label',
43: 'CAN state machine in fault(object 0x6041 in DS-402)',
45: 'Return Error From Subroutine',
46: 'May Not Use Multi- capture Homing Mode With Stop Event',
47: 'Program does not exist or not Compiled',
48: 'Motor cold not start - fault reason in CD',
50: 'Stack Overflow',
51: 'Inhibit OR Abort inputs are active, Cannot start motor',
52: 'PVT Queue Full',
53: 'Only For Current',
54: 'Bad Data Base',
55: 'Bad Context',
56: 'The product grade does not support this command',
57: 'Motor Must be Off',
58: 'Motor Must be On',
60: 'Bad Unit Mode',
61: 'Data Base Reset',
64: 'Cannot set the index of an active table',
65: 'Disabled By SW',
66: 'Amplifier Not Ready',
67: 'Recorder Is Busy',
68: 'Required profiler mode is not supported',
69: 'Recorder Usage Error',
70: 'Recorder data Invalid',
71: 'Homing is busy',
72: 'Modulo range must be even',
73: 'Please Set Position',
74: 'Bad profile database, see 0x2081 for object number (EE[2])',
77: 'Buffer Too Large',
78: 'Out of Program Range',
80: 'ECAM data inconsistent',
81: 'Download failed see specific error in EE[3]',
82: 'Program Is Running',
83: 'Command is not permitted in a program.',
84: 'The System Is Not In Point To Point Mode',
86: 'PVT table is soon going to underflow',
88: 'ECAM last interval is larger than allowed',
90: 'CAN state machine not ready (object 0x6041 in DS-402)',
91: 'Bad PVT Head Pointer',
92: 'PDO not configured',
93: 'There is a wrong initiation value for this command',
95: 'ER[3] Too large for modulo setting applied',
96: 'User program time out',
97: 'RS232 receive buffer overflow',
98: 'Cannot measure current offsets',
99: 'Bad auxiliary sensor configuration',
100: 'The requested PWM value is not supported',
101: 'Absolute encoder setting problem',
105: 'Speed loop KP out of range',
106: 'Position loop KP out of range',
110: 'Too long number',
111: 'KV vector is invalid',
112: 'KV defines scheduled block but scheduling is off',
113: 'Exp task queue is full',
114: 'Exp task queue is empty',
115: 'Exp output queue is full',
116: 'Exp output queue is empty',
117: 'Bad KV setting for sensor filter',
118: 'Bad KV setting',
119: 'Analog Sensor filter out of range',
120: 'Analog Sensor filter may contain 0 or 2 blocks',
121: 'Please wait until Analog Sensor initialized',
122: 'Motion mode is not supported or with initialization conflict',
123: 'Profiler queue is full',
125: 'Personality not loaded',
126: 'User Program failed - variable out of program size',
127: 'Modulo range must be positive',
128: 'Bad variable index in database',
129: 'Variable is not an array',
130: 'Variable name does not exist',
131: 'Cannot record local variable',
132: 'Variable is an array',
133: 'Number of function input arguments is not as expected',
134: 'Cannot run local label/function with the XQ command',
135: 'Frequency identification failed',
136: 'Not a number',
137: 'Program already compiled',
139: 'The number of break points exceeds maximal number',
140: 'An attempt to set/clear break point at the not relevant line',
141: 'Boot Identity parameters section is not clear',
142: 'Checksum of data is not correct',
143: 'Missing boot identity parameters',
144: 'Numeric Stack underflow',
145: 'Numeric stack overflow',
146: 'Expression stack overflow',
147: 'Executable command within math expression',
148: 'Nothing in the expression',
149: 'Unexpected sentence termination',
150: 'Sentence terminator not found',
151: 'Parentheses mismatch',
152: 'Bad operand type',
153: 'Overflow in a numeric operator',
154: 'Address is out of data memory segment',
155: 'Beyond stack range',
156: 'Bad op-code',
157: 'No Available program stack',
158: 'Out of flash memory range',
159: 'Flash memory verification error',
160: 'Program aborted by another thread',
161: 'Program is not halted.',
162: 'Badly formatted number.',
163:
'There is not enough space in the program data segment. Try to reduce variable usage in the user program.',
164: 'EC command (not an error)',
165: 'An attempt was made to access serial flash memory while busy.',
166: 'Out of modulo range.',
167: 'Infinite loop in for loop - zero step',
168: 'Speed too large to start motor.',
169: 'Time out using peripheral.(overflo w or busy)',
170: 'Cannot erase sector in flash memory',
171: 'Cannot read from flash memory',
172: 'Cannot write to flash memory',
173: 'Executable area of program is too large',
174: 'Program has not been loaded',
175: 'Cannot write program checksum - clear program (CP)',
176: 'User code, variables and functions are too large',
181: 'Writing to Flash program area, failed',
182: 'PAL Burn Is In Process or no PAL is burned',
183: 'PAL Burn (PB Command) Is Disabled',
184: 'Capture option already used by other operation',
185:
'This element may be modified only when interpolation is not active',
186: 'Interpolation queue is full',
187: 'Incorrect Interpolation sub-mode',
188: 'Gantry slave is disabled',
}
def __init__(self, *args, **kwargs):
Controller.__init__(self, *args, **kwargs)
self._cnx = None
def initialize(self):
config = self.config.config_dict
if get_comm_type(config) == UDP:
opt = {'port': 5001, 'eol': ';'}
else: # SERIAL
opt = {'baudrate': 115200, 'eol': ';'}
self._cnx = get_comm(config, **opt)
self._elmostate = AxisState()
for state, human in (('SLAVESWITCH', 'Slave switch activate'),
('INHIBITSWITCH', 'Inhibit switch active'),
('CLOSEDLOOPOPEN', 'Closed loop open'),
('DRIVEFAULT', 'Problem into the drive')):
self._elmostate.create_state(state, human)
def initialize_hardware(self):
# Check that the controller is alive
try:
self._query("VR\r", timeout=50e-3)
except SocketTimeout:
raise RuntimeError("Controller Elmo (%s) is not connected" % \
(self._cnx._host))
def initialize_axis(self, axis):
axis._mode = Cache(axis, "mode", default_value=None)
def initialize_hardware_axis(self, axis):
# Check user-mode
mode = int(self._query("UM"))
asked_mode = axis.config.get('user_mode', int, 5)
if (mode != asked_mode):
self._query("UM=%d" % asked_mode)
# Check closed loop on
if self._query("MO") != '1':
self.set_on(axis)
mode = self._query("UM")
axis._mode.value = int(mode)
def set_on(self, axis):
self._set_power(axis, True)
def set_off(self, axis):
self._set_power(axis, False)
def _set_power(self, axis, activate):
# Activate slave if needed
if axis.config.get('control_slave', bool, False):
self._query("OB[1]=%d" % activate)
self._query("MO=%d" % activate)
def _query(self, msg, in_error_code=False, **keys):
send_message = msg + '\r'
raw_reply = self._cnx.write_readline(send_message, **keys)
if not raw_reply.startswith(send_message): # something weird happened
self._cnx.close()
raise RuntimeError("received reply: %s\n"
"expected message starts with %s" % msg)
reply = raw_reply[len(send_message):]
if not in_error_code and reply.endswith('?'):
error_code = self._query("EC", in_error_code=True)
try:
error_code = int(error_code)
except ValueError: # Weird, don't know what to do
pass
else:
human_error = self.ErrorCode.get(error_code, 'Unknown')
raise RuntimeError("Error %d (%s), Query (%s)" %
(error_code, human_error, msg))
return reply
def start_jog(self, axis, velocity, direction):
self._query("JV=%d" % velocity * direction)
self._query("BG")
def stop_jog(self, axis):
self._query("JV=0")
self._query("BG")
#check if sync_hard needed
def read_position(self, axis):
if axis._mode == 2:
return float(self._query("PX"))
else:
return float(self._query("DV[3]"))
def set_position(self, axis, new_pos):
pos = round(new_pos)
self._set_power(axis, False)
encodeur_name = "PY" if axis._mode == 4 else "PX"
self._query("%s=%d" % (encodeur_name, pos))
self._set_power(axis, True)
self._query("PA=%d" % pos)
return self.read_position(axis)
def read_acceleration(self, axis):
return int(self._query("AC"))
def set_acceleration(self, axis, new_acc):
self._query("AC=%d" % new_acc)
self._query("DC=%d" % new_acc)
return self.read_acceleration(axis)
def read_velocity(self, axis):
return float(self._query("SP"))
def set_velocity(self, axis, new_vel):
self._query("SP=%d" % new_vel)
return self.read_velocity(axis)
def home_search(self, axis, switch, set_pos=None):
#can't set the position when searching home
#we should change emotion to add an option in
#home search i.e: set_pos = None
if set_pos is not None:
commands = [
"HM[3]=3",
"HM[2]=%d" % round(set_pos), "HM[4]=0", "HM[5]=0", "HM[1]=1"
]
else:
commands = ["HM[3]=3", "HM[4]=0", "HM[5]=2", "HM[1]=1"]
for cmd in commands:
self._query(cmd)
#Start search
step_sign = 1 if axis.config.get('steps_per_unit', float) > 0 else -1
self._query("JV=%d", switch * step_sign * self.read_velocity())
self._query("BG")
def state(self, axis):
state = self._elmostate.new()
# check first that the controller is ready to move
# bit0 of Status Register (page 3.135)
ans = int(self._query("SR"))
if (ans & (1 << 7)):
state.set("MOVING")
if (ans & 0x1): # problem into the drive
state.set("DRIVEFAULT")
if not (ans & (1 << 4)): # closed loop open
state.set("CLOSEDLOOPOPEN")
# Check limits
ans = int(self._query("IP"))
if axis.config.get('control_slave', bool, False):
if ans & (1 << 17):
state.set("SLAVESWITCH")
if ans & (1 << 6):
state.set("LIMPOS")
if ans & (1 << 7):
state.set("LIMNEG")
if ans & (1 << 8):
#should be checked in spec ends in MOT_EMERGENCY ?
state.set("INHIBITSWITCH")
# Check motion state
# Wrong if homing
#
ans = int(self._query("MS"))
if ans == 0:
state.set("READY")
elif ans == 1 or ans == 2:
state.set("MOVING")
elif ans == 3:
state.set("FAULT")
return state
def start_one(self, motion):
# check first that the controller is ready to move
# bit0 of Status Register (page 3.135)
ans = int(self._query("SR"))
if (ans & 0x1):
raise RuntimeError("problem into the drive")
if not (ans & (1 << 4)):
raise RuntimeError("closed loop open")
self._query("PA=%d" % round(motion.target_pos))
self._query("BG")
def stop(self, axis):
self._query("ST")
#todo spec macros check if motor is in homing phase...
@object_method(types_info=("None", "int"))
def get_user_mode(self, axis):
return int(self._query("UM"))
@object_method(types_info=("int", "int"))
def set_user_mode(self, axis, mode):
commands = ["MO=0", "UM=%d" % mode]
if mode == 2:
commands.append("PM=1")
commands.append("MO=1")
for cmd in commands:
self._query(cmd)
if mode == 5 or mode == 4:
self.sync_hard()
mode = int(self._query("UM"))
axis._mode.value = mode
return mode
@object_method(types_info=("None", ("float", "float")))
def jog_range(self, axis):
#this method should be in emotion
#todo move it has a generic
return float(self._query("VL[2]")), float(self._query("VH[2]"))
@object_method(types_info=("None", "bool"))
def get_enable_slave(self, axis):
return bool(self._query("OB[1]"))
@object_method(types_info=("bool", "bool"))
def set_enable_slave(self, axis, active):
self._query("OB[1]=%d" % active)
return bool(self._query("OB[1]"))
#encoders
def initialize_encoder(self, encoder):
pass
def read_encoder(self, encoder):
return float(self._query("PX"))
def set_encoder(self, encoder, steps):
self._query("PX=%d" % steps)
class Icepap(Controller):
"""
IcePAP stepper controller without Deep Technology of Communication.
But if you prefer to have it (DTC) move to IcePAP controller class.
Use this class controller at your own risk, because you won't
have any support...
"""
STATUS_DISCODE = {
0 : ('POWERENA', 'power enabled'),
1 : ('NOTACTIVE', 'axis configured as not active'),
2 : ('ALARM', 'alarm condition'),
3 : ('REMRACKDIS', 'remote rack disable input signal'),
4 : ('LOCRACKDIS', 'local rack disable switch'),
5 : ('REMAXISDIS', 'remote axis disable input signal'),
6 : ('LOCAXISDIS', 'local axis disable switch'),
7 : ('SOFTDIS', 'software disable'),
}
STATUS_MODCODE = {
0 : ('OPER', 'operation mode'),
1 : ('PROG', 'programmation mode'),
2 : ('TEST', 'test mode'),
3 : ('FAIL', 'fail mode'),
}
STATUS_STOPCODE = {
0 : ('SCEOM', 'end of movement'),
1 : ('SCSTOP', 'last motion was stopped'),
2 : ('SCABORT', 'last motion was aborted'),
3 : ('SCLIMPOS', 'positive limitswitch reached'),
4 : ('SCLINNEG', 'negative limitswitch reached'),
5 : ('SCSETTLINGTO', 'settling timeout'),
6 : ('SCAXISDIS', 'axis disabled (no alarm)'),
7 : ('SCBIT7', 'n/a'),
8 : ('SCINTFAIL', 'internal failure'),
9 : ('SCMOTFAIL', 'motor failure'),
10 : ('SCPOWEROVL', 'power overload'),
11 : ('SCHEATOVL', 'driver overheating'),
12 : ('SCCLERROR', 'closed loop error'),
13 : ('SCCENCERROR', 'control encoder error'),
14 : ('SCBIT14', 'n/a'),
15 : ('SCEXTALARM', 'external alarm'),
}
def __init__(self,*args,**kwargs):
Controller.__init__(self,*args,**kwargs)
self._cnx = None
self._last_axis_power_time = dict()
def initialize(self):
hostname = self.config.get("host")
self._cnx = Command(hostname,5000,eol='\n')
self._icestate = AxisState()
self._icestate.create_state("POWEROFF", "motor power is off")
for codes in (self.STATUS_DISCODE,self.STATUS_MODCODE,self.STATUS_STOPCODE):
for state,desc in codes.values():
self._icestate.create_state(state,desc)
def finalize(self):
if self._cnx is not None:
self._cnx.close()
def initialize_axis(self,axis):
axis.address = axis.config.get("address",lambda x: x)
if hasattr(axis,'_init_software'):
axis._init_software()
def initialize_hardware_axis(self,axis):
if axis.config.get('autopower', converter=bool, default=True):
try:
self.set_on(axis)
except:
sys.excepthook(*sys.exc_info())
if hasattr(axis,'_init_hardware'):
axis._init_hardware()
#Axis power management
def set_on(self,axis):
"""
Put the axis power on
"""
self._power(axis,True)
def set_off(self,axis):
"""
Put the axis power off
"""
self._power(axis,False)
def _power(self,axis,power):
_ackcommand(self._cnx,"POWER %s %s" %
("ON" if power else "OFF",axis.address))
self._last_axis_power_time[axis] = time.time()
def read_position(self,axis,cache=True):
pos_cmd = "FPOS" if cache else "POS"
return int(_command(self._cnx,"?%s %s" % (pos_cmd,axis.address)))
def set_position(self,axis,new_pos):
if isinstance(axis,SlaveAxis):
pre_cmd = "%d:DISPROT LINKED;" % axis.address
else:
pre_cmd = None
_ackcommand(self._cnx,"POS %s %d" % (axis.address,int(round(new_pos))),
pre_cmd = pre_cmd)
return self.read_position(axis,cache=False)
def read_velocity(self,axis):
return float(_command(self._cnx,"?VELOCITY %s" % axis.address))
def set_velocity(self,axis,new_velocity):
_ackcommand(self._cnx,"VELOCITY %s %f" %
(axis.address,new_velocity))
return self.read_velocity(axis)
def read_acceleration(self,axis):
acctime = float(_command(self._cnx,"?ACCTIME %s" % axis.address))
velocity = self.read_velocity(axis)
return velocity/float(acctime)
def set_acceleration(self,axis,new_acc):
velocity = self.read_velocity(axis)
new_acctime = velocity/new_acc
_ackcommand(self._cnx,"ACCTIME %s %f" % (axis.address,new_acctime))
return self.read_acceleration(axis)
def state(self,axis):
last_power_time = self._last_axis_power_time.get(axis,0)
if time.time() - last_power_time < 1.:
status_cmd = "?STATUS"
else:
self._last_axis_power_time.pop(axis,None)
status_cmd = "?FSTATUS"
status = int(_command(self._cnx,"%s %s" %
(status_cmd,axis.address)),16)
status ^= 1<<23 #neg POWERON FLAG
state = self._icestate.new()
for mask,value in (((1<<9),"READY"),
((1<<10|1<<11),"MOVING"),
((1<<18),"LIMPOS"),
((1<<19),"LIMNEG"),
((1<<20),"HOME"),
((1<<23),"POWEROFF")):
if status & mask:
state.set(value)
state_mode = (status >> 2) & 0x3
if state_mode:
state.set(self.STATUS_MODCODE.get(state_mode)[0])
stop_code = (status >> 14) & 0xf
if stop_code:
state.set(self.STATUS_STOPCODE.get(stop_code)[0])
disable_condition = (status >> 4) & 0x7
if disable_condition:
state.set(self.STATUS_DISCODE.get(disable_condition)[0])
if state.READY:
#if motor is ready then no need to investigate deeper
return state
if not state.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 status & (1<<13):
try:
warning = _command(self._cnx,"%d:?WARNING" % axis.address)
except TypeError:
pass
else:
warn_str = "warning condition: \n" + warning
status.create_state("WARNING",warn_str)
status.set("WARNING")
try:
alarm = _command(self._cnx,"%d:?ALARM" % axis.address)
except (RuntimeError,TypeError):
pass
else:
if alarm != "NO":
alarm_dsc = "alarm condition: " + str(alarm)
state.create_state("ALARMDESC",alarm_dsc)
state.set("ALARMDESC")
return state
def get_info(self,axis):
pre_cmd = '%s:' % axis.address
r = "MOTOR : %s\n" % axis.name
r += "SYSTEM : %s (ID: %s) (VER: %s)\n" % (self._cnx._host,
_command(self._cnx,"0:?ID"),
_command(self._cnx,"?VER"))
r += "DRIVER : %s\n" % axis.address
r += "POWER : %s\n" % _command(self._cnx,pre_cmd + "?POWER")
r += "CLOOP : %s\n" % _command(self._cnx,pre_cmd + "?PCLOOP")
r += "WARNING : %s\n" % _command(self._cnx,pre_cmd + "?WARNING")
r += "ALARM : %s\n" % _command(self._cnx,pre_cmd + "?ALARM")
return r
def raw_write(self,message,data = None):
return _command(self._cnx,message,data)
def raw_write_read(self,message,data = None):
return _ackcommand(self._cnx,message,data)
def prepare_move(self,motion):
pass
def start_one(self,motion):
if isinstance(motion.axis,SlaveAxis):
pre_cmd = "%d:DISPROT LINKED;" % motion.axis.address
else:
pre_cmd = None
_ackcommand(self._cnx,"MOVE %s %d" % (motion.axis.address,
motion.target_pos),
pre_cmd = pre_cmd)
def start_all(self,*motions):
if motions > 1:
cmd = "MOVE GROUP "
cmd += ' '.join(["%s %d" % (m.axis.address,m.target_pos) for m in motions])
_ackcommand(self._cnx,cmd)
elif motions:
self.start_one(motions[0])
def stop(self,axis):
_command(self._cnx,"STOP %s" % axis.address)
def stop_all(self,*motions):
for motion in motions:
self.stop(motion.axis)
def home_search(self,axis,switch):
cmd = "HOME " + ("+1" if switch > 0 else "-1")
_ackcommand(self._cnx,"%s:%s" % (axis.address,cmd))
# IcePAP status is not immediately MOVING after home search command is sent
gevent.sleep(0.2)
def home_state(self,axis):
s = self.state(axis)
if s != 'READY' and s != 'POWEROFF':
s.set('MOVING')
return s
def limit_search(self,axis,limit):
cmd = "SRCH LIM" + ("+" if limit>0 else "-")
_ackcommand(self._cnx,"%s:%s" % (axis.address,cmd))
# TODO: MG18Nov14: remove this sleep (state is not immediately MOVING)
gevent.sleep(0.1)
def initialize_encoder(self,encoder):
# 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
enctype = encoder.config.get("type",str,"ENCIN").upper()
# Minium check on encoder input
if enctype not in ['ENCIN', 'ABSENC', 'INPOS', 'MOTOR', 'AXIS', 'SYNC']:
raise ValueError('Invalid encoder type')
encoder.enctype = enctype
def read_encoder(self,encoder):
value = _command(self._cnx,"?ENC %s %d" % (encoder.enctype,encoder.address))
return int(value)
def set_encoder(self,encoder,steps):
_ackcommand(self._cnx,"ENC %s %d %d" %
(encoder.enctype,encoder.address,steps))
def set_event_positions(self,axis_or_encoder,positions):
int_position = numpy.array(positions,dtype=numpy.int32)
#position has to be ordered
int_position.sort()
address = axis_or_encoder.address
if not len(int_position):
_ackcommand(self._cnx,"%s:ECAMDAT CLEAR" % address)
return
if isinstance(axis_or_encoder,Axis):
source = 'AXIS'
else: # encoder
source = 'MEASURE'
#load trigger positions
_ackcommand(self._cnx,"%s:*ECAMDAT %s DWORD" % (address,source),
int_position)
# send the trigger on the multiplexer
_ackcommand(self._cnx,"%s:SYNCAUX eCAM" % address)
def get_event_positions(self,axis_or_encoder):
"""
For this controller this method should be use
for debugging purposed only...
"""
address = axis_or_encoder.address
#Get the number of positions
reply = _command(self._cnx,"%d:?ECAMDAT" % address)
reply_exp = re.compile("(\w+) +([+-]?\d+) +([+-]?\d+) +(\d+)")
m = reply_exp.match(reply)
if m is None:
raise RuntimeError("Reply Didn't expected: %s" % reply)
source = m.group(1)
nb = int(m.group(4))
if isinstance(axis_or_encoder,Axis):
nb = nb if source == 'AXIS' else 0
else: # encoder
nb = nb if source == "MEASURE" else 0
positions = numpy.zeros((nb,),dtype = numpy.int32)
if nb > 0:
reply_exp = re.compile(".+: +([+-]?\d+)")
reply = _command(self._cnx,"%d:?ECAMDAT %d" % (address,nb))
for i,line in enumerate(reply.split('\n')):
m = reply_exp.match(line)
if m:
pos = int(m.group(1))
positions[i] = pos
return positions
def get_linked_axis(self):
reply = _command(self._cnx,"?LINKED")
linked = dict()
for line in reply.strip().split('\n'):
values = line.split()
linked[values[0]] = [int(x) for x in values[1:]]
return linked
@object_method(types_info=("bool","bool"))
def activate_closed_loop(self,axis,active):
_command(self._cnx,"#%s:PCLOOP %s" % (axis.address,"ON" if active else "OFF"))
return active
@object_method(types_info=("None","bool"))
def is_closed_loop_activate(self,axis):
return True if _command(self._cnx,"%s:?PCLOOP" % axis.address) == 'ON' else False
@object_method(types_info=("None","None"))
def reset_closed_loop(self,axis):
measure_position = int(_command(self._cnx,"%s:?POS MEASURE" % axis.address))
self.set_position(axis,measure_position)
if axis.config.get('autopower', converter=bool, default=True):
self.set_on(axis)
axis.sync_hard()
@object_method(types_info=("None","int"))
def temperature(self,axis):
return int(_command(self._cnx,"%s:?MEAS T" % axis.address))
@object_method(types_info=(("float","bool"),"None"))
def set_tracking_positions(self,axis,positions,cyclic = False):
"""
Send position to the controller which will be tracked.
positions -- are expressed in user unit
cyclic -- cyclic position or not default False
@see activate_track method
"""
address = axis.address
if not len(positions):
_ackcommand(self._cnx,"%s:LISTDAT CLEAR" % address)
return
dial_positions = axis.user2dial(numpy.array(positions, dtype=numpy.float))
step_positions = numpy.array(dial_positions * axis.steps_per_unit,
dtype=numpy.int32)
_ackcommand(self._cnx,"%d:*LISTDAT %s DWORD" %
(address, "CYCLIC" if cyclic else "NOCYCLIC"),
step_positions)
@object_method(types_info=("None",("float","bool")))
def get_tracking_positions(self,axis):
"""
Get the tacking positions.
This method should only be use for debugging
return a tuple with (positions,cyclic flag)
"""
address = axis.address
#Get the number of positions
reply = _command(self._cnx,"%d:?LISTDAT" % address)
reply_exp = re.compile("(\d+) *(\w+)?")
m = reply_exp.match(reply)
if m is None:
raise RuntimeError("Reply didn't expected: %s" % reply)
nb = int(m.group(1))
positions = numpy.zeros((nb,),dtype = numpy.int32)
cyclic = True if m.group(2) == "CYCLIC" else False
if nb > 0:
reply_exp = re.compile(".+: +([+-]?\d+)")
reply = _command(self._cnx,"%d:?LISTDAT %d" % (address,nb))
for i,line in enumerate(reply.split('\n')):
m = reply_exp.match(line)
if m:
pos = int(m.group(1))
positions[i] = pos
dial_positions = positions / axis.steps_per_unit
positions = axis.dial2user(dial_positions)
return positions,cyclic
@object_method(types_info=(("bool","str"),"None"))
def activate_tracking(self,axis,activate,mode = None):
"""
Activate/Deactivate the tracking position depending on
activate flag
mode -- default "INPOS" if None.
mode can be :
- SYNC -> Internal SYNC signal
- ENCIN -> ENCIN signal
- INPOS -> INPOS signal
- ABSENC -> ABSENC signal
"""
address = axis.address
if not activate:
_ackcommand(self._cnx,"STOP %d" % address)
axis.sync_hard()
else:
if mode is None: mode = "INPOS"
possibles_modes = ["SYNC","ENCIN","INPOS","ABSENC"]
if mode not in possibles_modes:
raise ValueError("mode %s is not managed, can only choose %s" %
(mode,possibles_modes))
if mode == "INPOS":
_ackcommand(self._cnx, "%d:POS INPOS 0" % address)
_ackcommand(self._cnx,"%d:LTRACK %s" % (address,mode))
@object_method(types_info=("float", "None"))
def blink(self, axis, second=3.):
"""
Blink axis driver
"""
_command(self._cnx,"%d:BLINK %f" % (axis.address, second))
def reset(self):
_command(self._cnx,"RESET")
def mdspreset(self):
"""
Reset the MASTER DSP
"""
_command(self._cnx,"_dsprst")
def reboot(self):
_command(self._cnx,"REBOOT")
self._cnx.close()
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 Elmo(Controller):
"""
Elmo motor controller
configuration example:
- class: elmo
udp:
url: nscopeelmo
axes:
- name: rot
steps_per_unit: 26222.2
velocity: 377600
acceleration: 755200
control_slave: True
"""
ErrorCode = {
1 : 'Do not Update',
2 : 'Bad Command',
3 : 'Bad Index',
5 : 'BEYOND_ALPHA_BET (Has No Interpreter Meaning',
6 : 'Program is not running',
7 : 'Mode cannot be started - bad initialization data',
8 : 'Motion terminated, probably data underflow',
9 : 'CAN message was lost',
10 : 'Cannot be used by PDO',
11 : 'Cannot write to flash memory',
12 : 'Command not available in this unit mode',
13 : 'Cannot reset communication - UART is busy',
14 : 'Cannot perform CAN NMT service',
15 : 'CAN Life time exceeded - motor shut down',
16 : "The command attribute is array '[]' is expected",
17 : 'Format of UL command is not valid - check the command definition',
18 : 'Empty Assign',
19 : 'Command syntax error',
21 : 'Operand Out of Range',
22 : 'Zero Division',
23 : 'Command cannot be assigned',
24 : 'Bad Operation',
25 : 'Command Not Valid While Moving',
26 : 'Profiler mode not supported in this unit mode (UM)',
28 : 'Out Of Limit',
29 : 'CAN set object return an abort when called from interpreter',
30 : 'No program to continue',
31 : 'CAN get object return an abort when called from interpreter',
32 : 'Communication overrun, parity, noise, or framing error',
33 : 'Bad sensor setting',
34 : 'There is a conflict with another command',
36 : 'Commutation method (CA[17]) or commutation table does not fit to sensor',
37 : 'Two Or More Hall sensors are defined to the same place',
38 : 'Motion start specified for the past',
41 : 'Command is not supported by this product',
42 : 'No Such Label',
43 : 'CAN state machine in fault(object 0x6041 in DS-402)',
45 : 'Return Error From Subroutine',
46 : 'May Not Use Multi- capture Homing Mode With Stop Event',
47 : 'Program does not exist or not Compiled',
48 : 'Motor cold not start - fault reason in CD',
50 : 'Stack Overflow',
51 : 'Inhibit OR Abort inputs are active, Cannot start motor',
52 : 'PVT Queue Full',
53 : 'Only For Current',
54 : 'Bad Data Base',
55 : 'Bad Context',
56 : 'The product grade does not support this command',
57 : 'Motor Must be Off',
58 : 'Motor Must be On',
60 : 'Bad Unit Mode',
61 : 'Data Base Reset',
64 : 'Cannot set the index of an active table',
65 : 'Disabled By SW',
66 : 'Amplifier Not Ready',
67 : 'Recorder Is Busy',
68 : 'Required profiler mode is not supported',
69 : 'Recorder Usage Error',
70 : 'Recorder data Invalid',
71 : 'Homing is busy',
72 : 'Modulo range must be even',
73 : 'Please Set Position',
74 : 'Bad profile database, see 0x2081 for object number (EE[2])',
77 : 'Buffer Too Large',
78 : 'Out of Program Range',
80 : 'ECAM data inconsistent',
81 : 'Download failed see specific error in EE[3]',
82 : 'Program Is Running',
83 : 'Command is not permitted in a program.',
84 : 'The System Is Not In Point To Point Mode',
86 : 'PVT table is soon going to underflow',
88 : 'ECAM last interval is larger than allowed',
90 : 'CAN state machine not ready (object 0x6041 in DS-402)',
91 : 'Bad PVT Head Pointer',
92 : 'PDO not configured',
93 : 'There is a wrong initiation value for this command',
95 : 'ER[3] Too large for modulo setting applied',
96 : 'User program time out',
97 : 'RS232 receive buffer overflow',
98 : 'Cannot measure current offsets',
99 : 'Bad auxiliary sensor configuration',
100 : 'The requested PWM value is not supported',
101 : 'Absolute encoder setting problem',
105 : 'Speed loop KP out of range',
106 : 'Position loop KP out of range',
110 : 'Too long number',
111 : 'KV vector is invalid',
112 : 'KV defines scheduled block but scheduling is off',
113 : 'Exp task queue is full',
114 : 'Exp task queue is empty',
115 : 'Exp output queue is full',
116 : 'Exp output queue is empty',
117 : 'Bad KV setting for sensor filter',
118 : 'Bad KV setting',
119 : 'Analog Sensor filter out of range',
120 : 'Analog Sensor filter may contain 0 or 2 blocks',
121 : 'Please wait until Analog Sensor initialized',
122 : 'Motion mode is not supported or with initialization conflict',
123 : 'Profiler queue is full',
125 : 'Personality not loaded',
126 : 'User Program failed - variable out of program size',
127 : 'Modulo range must be positive',
128 : 'Bad variable index in database',
129 : 'Variable is not an array',
130 : 'Variable name does not exist',
131 : 'Cannot record local variable',
132 : 'Variable is an array',
133 : 'Number of function input arguments is not as expected',
134 : 'Cannot run local label/function with the XQ command',
135 : 'Frequency identification failed',
136 : 'Not a number',
137 : 'Program already compiled',
139 : 'The number of break points exceeds maximal number',
140 : 'An attempt to set/clear break point at the not relevant line',
141 : 'Boot Identity parameters section is not clear',
142 : 'Checksum of data is not correct',
143 : 'Missing boot identity parameters',
144 : 'Numeric Stack underflow',
145 : 'Numeric stack overflow',
146 : 'Expression stack overflow',
147 : 'Executable command within math expression',
148 : 'Nothing in the expression',
149 : 'Unexpected sentence termination',
150 : 'Sentence terminator not found',
151 : 'Parentheses mismatch',
152 : 'Bad operand type',
153 : 'Overflow in a numeric operator',
154 : 'Address is out of data memory segment',
155 : 'Beyond stack range',
156 : 'Bad op-code',
157 : 'No Available program stack',
158 : 'Out of flash memory range',
159 : 'Flash memory verification error',
160 : 'Program aborted by another thread',
161 : 'Program is not halted.',
162 : 'Badly formatted number.',
163 : 'There is not enough space in the program data segment. Try to reduce variable usage in the user program.',
164 : 'EC command (not an error)',
165 : 'An attempt was made to access serial flash memory while busy.',
166 : 'Out of modulo range.',
167 : 'Infinite loop in for loop - zero step',
168 : 'Speed too large to start motor.',
169 : 'Time out using peripheral.(overflo w or busy)',
170 : 'Cannot erase sector in flash memory',
171 : 'Cannot read from flash memory',
172 : 'Cannot write to flash memory',
173 : 'Executable area of program is too large',
174 : 'Program has not been loaded',
175 : 'Cannot write program checksum - clear program (CP)',
176 : 'User code, variables and functions are too large',
181 : 'Writing to Flash program area, failed',
182 : 'PAL Burn Is In Process or no PAL is burned',
183 : 'PAL Burn (PB Command) Is Disabled',
184 : 'Capture option already used by other operation',
185 : 'This element may be modified only when interpolation is not active',
186 : 'Interpolation queue is full',
187 : 'Incorrect Interpolation sub-mode',
188 : 'Gantry slave is disabled',
}
def __init__(self,*args,**kwargs):
Controller.__init__(self,*args,**kwargs)
self._cnx = None
def initialize(self):
config = self.config.config_dict
if get_comm_type(config) == UDP:
opt = {'port':5001,'eol':';'}
else: # SERIAL
opt = {'baudrate':115200,'eol':';'}
self._cnx = get_comm(config,**opt)
self._elmostate = AxisState()
for state,human in (('SLAVESWITCH','Slave switch activate'),
('INHIBITSWITCH','Inhibit switch active'),
('CLOSEDLOOPOPEN','Closed loop open'),
('DRIVEFAULT','Problem into the drive')):
self._elmostate.create_state(state,human)
def initialize_hardware(self):
# Check that the controller is alive
try:
self._query("VR\r",timeout=50e-3)
except SocketTimeout:
raise RuntimeError("Controller Elmo (%s) is not connected" % \
(self._cnx._host))
def initialize_axis(self,axis):
axis._mode = Cache(axis,"mode",default_value = None)
def initialize_hardware_axis(self, axis):
# Check user-mode
mode= int(self._query("UM"))
asked_mode = axis.config.get('user_mode',int,5)
if (mode != asked_mode):
self._query("UM=%d" % asked_mode)
# Check closed loop on
if self._query("MO") != '1':
self.set_on(axis)
mode= self._query("UM")
axis._mode.value = int(mode)
def set_on(self, axis):
self._set_power(axis,True)
def set_off(self, axis):
self._set_power(axis,False)
def _set_power(self, axis, activate):
# Activate slave if needed
if axis.config.get('control_slave',bool,False):
self._query("OB[1]=%d" % activate)
self._query("MO=%d" % activate)
def _query(self,msg,in_error_code = False,**keys):
send_message = msg + '\r'
raw_reply = self._cnx.write_readline(send_message,**keys)
if not raw_reply.startswith(send_message): # something weird happened
self._cnx.close()
raise RuntimeError("received reply: %s\n"
"expected message starts with %s" % msg)
reply = raw_reply[len(send_message):]
if not in_error_code and reply.endswith('?'):
error_code = self._query("EC",in_error_code = True)
try:
error_code = int(error_code)
except ValueError: # Weird, don't know what to do
pass
else:
human_error = self.ErrorCode.get(error_code,'Unknown')
raise RuntimeError("Error %d (%s), Query (%s)" %
(error_code,human_error,msg))
return reply
def start_jog(self,axis,velocity,direction):
self._query("JV=%d" % velocity * direction)
self._query("BG")
def stop_jog(self,axis):
self._query("JV=0")
self._query("BG")
#check if sync_hard needed
def read_position(self,axis):
if axis._mode == 2:
return float(self._query("PX"))
else:
return float(self._query("DV[3]"))
def set_position(self,axis,new_pos):
pos = round(new_pos)
self._set_power(axis,False)
encodeur_name = "PY" if axis._mode == 4 else "PX"
self._query("%s=%d" % (encodeur_name,pos))
self._set_power(axis,True)
self._query("PA=%d" % pos)
return self.read_position(axis)
def read_acceleration(self,axis):
return int(self._query("AC"))
def set_acceleration(self,axis,new_acc):
self._query("AC=%d" % new_acc)
self._query("DC=%d" % new_acc)
return self.read_acceleration(axis)
def read_velocity(self,axis):
return float(self._query("SP"))
def set_velocity(self,axis,new_vel):
self._query("SP=%d" % new_vel)
return self.read_velocity(axis)
def home_search(self,axis,switch,set_pos = None):
#can't set the position when searching home
#we should change emotion to add an option in
#home search i.e: set_pos = None
if set_pos is not None:
commands = ["HM[3]=3","HM[2]=%d" % round(set_pos),
"HM[4]=0","HM[5]=0","HM[1]=1"]
else:
commands = ["HM[3]=3","HM[4]=0","HM[5]=2","HM[1]=1"]
for cmd in commands:
self._query(cmd)
#Start search
step_sign = 1 if axis.config.get('steps_per_unit',float) > 0 else -1
self._query("JV=%d", switch * step_sign * self.read_velocity())
self._query("BG")
def state(self,axis):
state = self._elmostate.new()
# check first that the controller is ready to move
# bit0 of Status Register (page 3.135)
ans = int(self._query("SR"))
if(ans & (1<<7)):
state.set("MOVING")
if(ans & 0x1): # problem into the drive
state.set("DRIVEFAULT")
if not (ans & (1<<4)): # closed loop open
state.set("CLOSEDLOOPOPEN")
# Check limits
ans = int(self._query("IP"))
if axis.config.get('control_slave',bool,False):
if ans & (1<<17):
state.set("SLAVESWITCH")
if ans & (1<<6):
state.set("LIMPOS")
if ans & (1<<7):
state.set("LIMNEG")
if ans & (1<<8):
#should be checked in spec ends in MOT_EMERGENCY ?
state.set("INHIBITSWITCH")
# Check motion state
# Wrong if homing
#
ans = int(self._query("MS"))
if ans == 0:
state.set("READY")
elif ans == 1 or ans == 2:
state.set("MOVING")
elif ans == 3:
state.set("FAULT")
return state
def start_one(self,motion):
# check first that the controller is ready to move
# bit0 of Status Register (page 3.135)
ans = int(self._query("SR"))
if(ans & 0x1):
raise RuntimeError("problem into the drive")
if not (ans & (1<<4)):
raise RuntimeError("closed loop open")
self._query("PA=%d" % round(motion.target_pos))
self._query("BG")
def stop(self,axis):
self._query("ST")
#todo spec macros check if motor is in homing phase...
@object_method(types_info=("None","int"))
def get_user_mode(self,axis):
return int(self._query("UM"))
@object_method(types_info=("int","int"))
def set_user_mode(self,axis,mode):
commands = ["MO=0","UM=%d" % mode]
if mode == 2:
commands.append("PM=1")
commands.append("MO=1")
for cmd in commands:
self._query(cmd)
if mode == 5 or mode == 4:
self.sync_hard()
mode = int(self._query("UM"))
axis._mode.value = mode
return mode
@object_method(types_info=("None",("float","float")))
def jog_range(self,axis):
#this method should be in emotion
#todo move it has a generic
return float(self._query("VL[2]")),float(self._query("VH[2]"))
@object_method(types_info=("None","bool"))
def get_enable_slave(self,axis):
return bool(self._query("OB[1]"))
@object_method(types_info=("bool","bool"))
def set_enable_slave(self,axis,active):
self._query("OB[1]=%d" % active)
return bool(self._query("OB[1]"))
#encoders
def initialize_encoder(self,encoder):
pass
def read_encoder(self,encoder):
return float(self._query("PX"))
def set_encoder(self,encoder,steps):
self._query("PX=%d" % steps)
class Icepap(Controller):
"""
IcePAP stepper controller without Deep Technology of Communication.
But if you prefer to have it (DTC) move to IcePAP controller class.
Use this class controller at your own risk, because you won't
have any support...
"""
STATUS_DISCODE = {
0: ('POWERENA', 'power enabled'),
1: ('NOTACTIVE', 'axis configured as not active'),
2: ('ALARM', 'alarm condition'),
3: ('REMRACKDIS', 'remote rack disable input signal'),
4: ('LOCRACKDIS', 'local rack disable switch'),
5: ('REMAXISDIS', 'remote axis disable input signal'),
6: ('LOCAXISDIS', 'local axis disable switch'),
7: ('SOFTDIS', 'software disable'),
}
STATUS_MODCODE = {
0: ('OPER', 'operation mode'),
1: ('PROG', 'programmation mode'),
2: ('TEST', 'test mode'),
3: ('FAIL', 'fail mode'),
}
STATUS_STOPCODE = {
0: ('SCEOM', 'end of movement'),
1: ('SCSTOP', 'last motion was stopped'),
2: ('SCABORT', 'last motion was aborted'),
3: ('SCLIMPOS', 'positive limitswitch reached'),
4: ('SCLINNEG', 'negative limitswitch reached'),
5: ('SCSETTLINGTO', 'settling timeout'),
6: ('SCAXISDIS', 'axis disabled (no alarm)'),
7: ('SCBIT7', 'n/a'),
8: ('SCINTFAIL', 'internal failure'),
9: ('SCMOTFAIL', 'motor failure'),
10: ('SCPOWEROVL', 'power overload'),
11: ('SCHEATOVL', 'driver overheating'),
12: ('SCCLERROR', 'closed loop error'),
13: ('SCCENCERROR', 'control encoder error'),
14: ('SCBIT14', 'n/a'),
15: ('SCEXTALARM', 'external alarm'),
}
def __init__(self, *args, **kwargs):
Controller.__init__(self, *args, **kwargs)
self._cnx = None
self._last_axis_power_time = dict()
def initialize(self):
hostname = self.config.get("host")
self._cnx = Command(hostname, 5000, eol='\n')
self._icestate = AxisState()
self._icestate.create_state("POWEROFF", "motor power is off")
for codes in (self.STATUS_DISCODE, self.STATUS_MODCODE,
self.STATUS_STOPCODE):
for state, desc in codes.values():
self._icestate.create_state(state, desc)
def finalize(self):
if self._cnx is not None:
self._cnx.close()
def initialize_axis(self, axis):
axis.address = axis.config.get("address", lambda x: x)
if hasattr(axis, '_init_software'):
axis._init_software()
def initialize_hardware_axis(self, axis):
if axis.config.get('autopower', converter=bool, default=True):
try:
self.set_on(axis)
except:
sys.excepthook(*sys.exc_info())
if hasattr(axis, '_init_hardware'):
axis._init_hardware()
#Axis power management
def set_on(self, axis):
"""
Put the axis power on
"""
self._power(axis, True)
def set_off(self, axis):
"""
Put the axis power off
"""
self._power(axis, False)
def _power(self, axis, power):
_ackcommand(self._cnx,
"POWER %s %s" % ("ON" if power else "OFF", axis.address))
self._last_axis_power_time[axis] = time.time()
def read_position(self, axis, cache=True):
pos_cmd = "FPOS" if cache else "POS"
return int(_command(self._cnx, "?%s %s" % (pos_cmd, axis.address)))
def set_position(self, axis, new_pos):
if isinstance(axis, SlaveAxis):
pre_cmd = "%d:DISPROT LINKED;" % axis.address
else:
pre_cmd = None
_ackcommand(self._cnx,
"POS %s %d" % (axis.address, int(round(new_pos))),
pre_cmd=pre_cmd)
return self.read_position(axis, cache=False)
def read_velocity(self, axis):
return float(_command(self._cnx, "?VELOCITY %s" % axis.address))
def set_velocity(self, axis, new_velocity):
_ackcommand(self._cnx, "VELOCITY %s %f" % (axis.address, new_velocity))
return self.read_velocity(axis)
def read_acceleration(self, axis):
acctime = float(_command(self._cnx, "?ACCTIME %s" % axis.address))
velocity = self.read_velocity(axis)
return velocity / float(acctime)
def set_acceleration(self, axis, new_acc):
velocity = self.read_velocity(axis)
new_acctime = velocity / new_acc
_ackcommand(self._cnx, "ACCTIME %s %f" % (axis.address, new_acctime))
return self.read_acceleration(axis)
def state(self, axis):
last_power_time = self._last_axis_power_time.get(axis, 0)
if time.time() - last_power_time < 1.:
status_cmd = "?STATUS"
else:
self._last_axis_power_time.pop(axis, None)
status_cmd = "?FSTATUS"
status = int(_command(self._cnx, "%s %s" % (status_cmd, axis.address)),
16)
status ^= 1 << 23 #neg POWERON FLAG
state = self._icestate.new()
for mask, value in (((1 << 9), "READY"), ((1 << 10 | 1 << 11),
"MOVING"),
((1 << 18), "LIMPOS"), ((1 << 19), "LIMNEG"),
((1 << 20), "HOME"), ((1 << 23), "POWEROFF")):
if status & mask:
state.set(value)
state_mode = (status >> 2) & 0x3
if state_mode:
state.set(self.STATUS_MODCODE.get(state_mode)[0])
stop_code = (status >> 14) & 0xf
if stop_code:
state.set(self.STATUS_STOPCODE.get(stop_code)[0])
disable_condition = (status >> 4) & 0x7
if disable_condition:
state.set(self.STATUS_DISCODE.get(disable_condition)[0])
if state.READY:
#if motor is ready then no need to investigate deeper
return state
if not state.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 status & (1 << 13):
try:
warning = _command(self._cnx, "%d:?WARNING" % axis.address)
except TypeError:
pass
else:
warn_str = "warning condition: \n" + warning
status.create_state("WARNING", warn_str)
status.set("WARNING")
try:
alarm = _command(self._cnx, "%d:?ALARM" % axis.address)
except (RuntimeError, TypeError):
pass
else:
if alarm != "NO":
alarm_dsc = "alarm condition: " + str(alarm)
state.create_state("ALARMDESC", alarm_dsc)
state.set("ALARMDESC")
return state
def get_info(self, axis):
pre_cmd = '%s:' % axis.address
r = "MOTOR : %s\n" % axis.name
r += "SYSTEM : %s (ID: %s) (VER: %s)\n" % (
self._cnx._host, _command(self._cnx,
"0:?ID"), _command(self._cnx, "?VER"))
r += "DRIVER : %s\n" % axis.address
r += "POWER : %s\n" % _command(self._cnx, pre_cmd + "?POWER")
r += "CLOOP : %s\n" % _command(self._cnx, pre_cmd + "?PCLOOP")
r += "WARNING : %s\n" % _command(self._cnx, pre_cmd + "?WARNING")
r += "ALARM : %s\n" % _command(self._cnx, pre_cmd + "?ALARM")
return r
def raw_write(self, message, data=None):
return _command(self._cnx, message, data)
def raw_write_read(self, message, data=None):
return _ackcommand(self._cnx, message, data)
def prepare_move(self, motion):
pass
def start_one(self, motion):
if isinstance(motion.axis, SlaveAxis):
pre_cmd = "%d:DISPROT LINKED;" % motion.axis.address
else:
pre_cmd = None
_ackcommand(self._cnx,
"MOVE %s %d" % (motion.axis.address, motion.target_pos),
pre_cmd=pre_cmd)
def start_all(self, *motions):
if motions > 1:
cmd = "MOVE GROUP "
cmd += ' '.join(
["%s %d" % (m.axis.address, m.target_pos) for m in motions])
_ackcommand(self._cnx, cmd)
elif motions:
self.start_one(motions[0])
def stop(self, axis):
_command(self._cnx, "STOP %s" % axis.address)
def stop_all(self, *motions):
for motion in motions:
self.stop(motion.axis)
def home_search(self, axis, switch):
cmd = "HOME " + ("+1" if switch > 0 else "-1")
_ackcommand(self._cnx, "%s:%s" % (axis.address, cmd))
# IcePAP status is not immediately MOVING after home search command is sent
gevent.sleep(0.2)
def home_state(self, axis):
s = self.state(axis)
if s != 'READY' and s != 'POWEROFF':
s.set('MOVING')
return s
def limit_search(self, axis, limit):
cmd = "SRCH LIM" + ("+" if limit > 0 else "-")
_ackcommand(self._cnx, "%s:%s" % (axis.address, cmd))
# TODO: MG18Nov14: remove this sleep (state is not immediately MOVING)
gevent.sleep(0.1)
def initialize_encoder(self, encoder):
# 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
enctype = encoder.config.get("type", str, "ENCIN").upper()
# Minium check on encoder input
if enctype not in [
'ENCIN', 'ABSENC', 'INPOS', 'MOTOR', 'AXIS', 'SYNC'
]:
raise ValueError('Invalid encoder type')
encoder.enctype = enctype
def read_encoder(self, encoder):
value = _command(self._cnx,
"?ENC %s %d" % (encoder.enctype, encoder.address))
return int(value)
def set_encoder(self, encoder, steps):
_ackcommand(self._cnx,
"ENC %s %d %d" % (encoder.enctype, encoder.address, steps))
def set_event_positions(self, axis_or_encoder, positions):
int_position = numpy.array(positions, dtype=numpy.int32)
#position has to be ordered
int_position.sort()
address = axis_or_encoder.address
if not len(int_position):
_ackcommand(self._cnx, "%s:ECAMDAT CLEAR" % address)
return
if isinstance(axis_or_encoder, Axis):
source = 'AXIS'
else: # encoder
source = 'MEASURE'
#load trigger positions
_ackcommand(self._cnx, "%s:*ECAMDAT %s DWORD" % (address, source),
int_position)
# send the trigger on the multiplexer
_ackcommand(self._cnx, "%s:SYNCAUX eCAM" % address)
def get_event_positions(self, axis_or_encoder):
"""
For this controller this method should be use
for debugging purposed only...
"""
address = axis_or_encoder.address
#Get the number of positions
reply = _command(self._cnx, "%d:?ECAMDAT" % address)
reply_exp = re.compile("(\w+) +([+-]?\d+) +([+-]?\d+) +(\d+)")
m = reply_exp.match(reply)
if m is None:
raise RuntimeError("Reply Didn't expected: %s" % reply)
source = m.group(1)
nb = int(m.group(4))
if isinstance(axis_or_encoder, Axis):
nb = nb if source == 'AXIS' else 0
else: # encoder
nb = nb if source == "MEASURE" else 0
positions = numpy.zeros((nb, ), dtype=numpy.int32)
if nb > 0:
reply_exp = re.compile(".+: +([+-]?\d+)")
reply = _command(self._cnx, "%d:?ECAMDAT %d" % (address, nb))
for i, line in enumerate(reply.split('\n')):
m = reply_exp.match(line)
if m:
pos = int(m.group(1))
positions[i] = pos
return positions
def get_linked_axis(self):
reply = _command(self._cnx, "?LINKED")
linked = dict()
for line in reply.strip().split('\n'):
values = line.split()
linked[values[0]] = [int(x) for x in values[1:]]
return linked
@object_method(types_info=("bool", "bool"))
def activate_closed_loop(self, axis, active):
_command(self._cnx,
"#%s:PCLOOP %s" % (axis.address, "ON" if active else "OFF"))
return active
@object_method(types_info=("None", "bool"))
def is_closed_loop_activate(self, axis):
return True if _command(self._cnx, "%s:?PCLOOP" %
axis.address) == 'ON' else False
@object_method(types_info=("None", "None"))
def reset_closed_loop(self, axis):
measure_position = int(
_command(self._cnx, "%s:?POS MEASURE" % axis.address))
self.set_position(axis, measure_position)
if axis.config.get('autopower', converter=bool, default=True):
self.set_on(axis)
axis.sync_hard()
@object_method(types_info=("None", "int"))
def temperature(self, axis):
return int(_command(self._cnx, "%s:?MEAS T" % axis.address))
@object_method(types_info=(("float", "bool"), "None"))
def set_tracking_positions(self, axis, positions, cyclic=False):
"""
Send position to the controller which will be tracked.
positions -- are expressed in user unit
cyclic -- cyclic position or not default False
@see activate_track method
"""
address = axis.address
if not len(positions):
_ackcommand(self._cnx, "%s:LISTDAT CLEAR" % address)
return
dial_positions = axis.user2dial(
numpy.array(positions, dtype=numpy.float))
step_positions = numpy.array(dial_positions * axis.steps_per_unit,
dtype=numpy.int32)
_ackcommand(
self._cnx, "%d:*LISTDAT %s DWORD" %
(address, "CYCLIC" if cyclic else "NOCYCLIC"), step_positions)
@object_method(types_info=("None", ("float", "bool")))
def get_tracking_positions(self, axis):
"""
Get the tacking positions.
This method should only be use for debugging
return a tuple with (positions,cyclic flag)
"""
address = axis.address
#Get the number of positions
reply = _command(self._cnx, "%d:?LISTDAT" % address)
reply_exp = re.compile("(\d+) *(\w+)?")
m = reply_exp.match(reply)
if m is None:
raise RuntimeError("Reply didn't expected: %s" % reply)
nb = int(m.group(1))
positions = numpy.zeros((nb, ), dtype=numpy.int32)
cyclic = True if m.group(2) == "CYCLIC" else False
if nb > 0:
reply_exp = re.compile(".+: +([+-]?\d+)")
reply = _command(self._cnx, "%d:?LISTDAT %d" % (address, nb))
for i, line in enumerate(reply.split('\n')):
m = reply_exp.match(line)
if m:
pos = int(m.group(1))
positions[i] = pos
dial_positions = positions / axis.steps_per_unit
positions = axis.dial2user(dial_positions)
return positions, cyclic
@object_method(types_info=(("bool", "str"), "None"))
def activate_tracking(self, axis, activate, mode=None):
"""
Activate/Deactivate the tracking position depending on
activate flag
mode -- default "INPOS" if None.
mode can be :
- SYNC -> Internal SYNC signal
- ENCIN -> ENCIN signal
- INPOS -> INPOS signal
- ABSENC -> ABSENC signal
"""
address = axis.address
if not activate:
_ackcommand(self._cnx, "STOP %d" % address)
axis.sync_hard()
else:
if mode is None: mode = "INPOS"
possibles_modes = ["SYNC", "ENCIN", "INPOS", "ABSENC"]
if mode not in possibles_modes:
raise ValueError("mode %s is not managed, can only choose %s" %
(mode, possibles_modes))
if mode == "INPOS":
_ackcommand(self._cnx, "%d:POS INPOS 0" % address)
_ackcommand(self._cnx, "%d:LTRACK %s" % (address, mode))
@object_method(types_info=("float", "None"))
def blink(self, axis, second=3.):
"""
Blink axis driver
"""
_command(self._cnx, "%d:BLINK %f" % (axis.address, second))
def reset(self):
_command(self._cnx, "RESET")
def mdspreset(self):
"""
Reset the MASTER DSP
"""
_command(self._cnx, "_dsprst")
def reboot(self):
_command(self._cnx, "REBOOT")
self._cnx.close()
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
def test_bad_name():
s = AxisState()
with pytest.raises(ValueError):
s.create_state("A bad state")
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