mockup.py 26.5 KB
Newer Older
1
2
3
4
# -*- coding: utf-8 -*-
#
# This file is part of the bliss project
#
Benoit Formet's avatar
Benoit Formet committed
5
# Copyright (c) 2015-2020 Beamline Control Unit, ESRF
6
# Distributed under the GNU LGPLv3. See LICENSE for more info.
7
import math
8
9
import time
import random
10
import gevent
11
import collections
12
import numpy as np
13

14
from bliss.physics.trajectory import LinearTrajectory
15
from bliss.controllers.motor import Controller, CalcController
16
from bliss.common.axis import Axis, AxisState
17
from bliss.common.switch import Switch as BaseSwitch
18
from bliss.common import event
19
from bliss.config.static import get_config
20
from bliss.config.settings import SimpleSetting
21
from bliss.common.hook import MotionHook
22
23
from bliss.common.utils import object_method
from bliss.common.utils import object_attribute_get, object_attribute_set
24
from bliss.common.logtools import log_debug
25

26

Cyril Guilloud's avatar
Cyril Guilloud committed
27
28
29
"""
mockup.py : a mockup controller for bliss.

blissadm on ID26's avatar
blissadm on ID26 committed
30
31
config :
 'velocity' in unit/s
cyril@lid269's avatar
cyril@lid269 committed
32
 'acceleration' in unit/s^2
blissadm on ID26's avatar
blissadm on ID26 committed
33
34
35
36
 'steps_per_unit' in unit^-1  (default 1)
 'backlash' in unit
"""

37

38
class Motion:
39
40
41
42
43
44
45
46
47
48
    """Describe a single motion"""

    def __init__(self, pi, pf, velocity, acceleration, hard_limits, ti=None):
        # TODO: take hard limits into account (complicated).
        # For now just shorten the movement
        self.hard_limits = low_limit, high_limit = hard_limits
        if pf > high_limit:
            pf = high_limit
        if pf < low_limit:
            pf = low_limit
49
        self.trajectory = LinearTrajectory(pi, pf, velocity, acceleration, ti)
50
51


52
53
54
55
56
class Switch(BaseSwitch):
    def __init__(self, name, controller, config):
        super().__init__(name, config)


57
class MockupAxis(Axis):
58
    def __init__(self, *args, **kwargs):
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
        Axis.__init__(self, *args, **kwargs)

    def get_motion(self, *args, **kwargs):
        motion = Axis.get_motion(self, *args, **kwargs)
        if motion is None:
            self.backlash_move = 0
            self.target_pos = None
        else:
            self.target_pos = motion.target_pos
            self.backlash_move = (
                motion.target_pos / self.steps_per_unit if motion.backlash else 0
            )
        return motion


class Mockup(Controller):
75
76
    def __init__(self, *args, **kwargs):  # config
        super().__init__(*args, **kwargs)
Matias Guijarro's avatar
Matias Guijarro committed
77
78

        self._axis_moves = {}
Matias Guijarro's avatar
Matias Guijarro committed
79
        self.__encoders = {}
80
        self.__switches = {}
81
        self._axes_data = collections.defaultdict(dict)
Cyril Guilloud's avatar
Cyril Guilloud committed
82
83

        # Custom attributes.
84
        self.__voltages = {}
Cyril Guilloud's avatar
Cyril Guilloud committed
85
        self.__cust_attr_float = {}
Matias Guijarro's avatar
Matias Guijarro committed
86

87
        self._hw_state = AxisState("READY")
88
        self.__hw_limit = float("-inf"), float("+inf")
89

90
        self._hw_state.create_state("PARKED", "mot au parking")
Matias Guijarro's avatar
Matias Guijarro committed
91

92
93
94
    def _create_subitem_from_config(
        self, name, cfg, parent_key, item_class, item_obj=None
    ):
95
96
97
98
99
        if parent_key == "switches":
            switch = item_class(name, self, cfg)
            self._switches[name] = switch
            return switch
        else:
Perceval Guillou's avatar
add doc    
Perceval Guillou committed
100
            return super()._create_subitem_from_config(
101
                name, cfg, parent_key, item_class, item_obj
Perceval Guillou's avatar
add doc    
Perceval Guillou committed
102
            )
103

104
105
106
107
    def steps_position_precision(self, axis):
        """Mockup is really a stepper motor controller"""
        return 1

108
    def read_hw_position(self, axis):
109
        return self._axes_data[axis]["hw_position"].get()
110
111

    def set_hw_position(self, axis, position):
112
        self._axes_data[axis]["hw_position"].set(position)
113

Cyril Guilloud's avatar
Cyril Guilloud committed
114
    """
Matias Guijarro's avatar
Matias Guijarro committed
115
    Axes initialization actions.
Cyril Guilloud's avatar
Cyril Guilloud committed
116
    """
117

118
    def _add_axis(self, axis):
119
120
121
122
123
124
        # this is a counter to check if an axis is added multiple times,
        # it is incremented in `initalize_axis()`
        self._axes_data[axis]["init_count"] = 0
        # those 3 are to simulate a real controller (one with internal settings, that
        # keep those for multiple clients)
        self._axes_data[axis]["hw_position"] = SimpleSetting(
125
            f"motor_mockup:{axis.name}:hw_position", default_value=0
126
127
        )
        self._axes_data[axis]["curr_acc"] = SimpleSetting(
128
            f"motor_mockup:{axis.name}:curr_acc", default_value=0
129
130
        )
        self._axes_data[axis]["curr_velocity"] = SimpleSetting(
131
            f"motor_mockup:{axis.name}:curr_velocity", default_value=0
132
133
        )

134
        self._axis_moves[axis] = {"motion": None}
135
        if self.read_hw_position(axis) is None:
136
            self.set_hw_position(axis, 0)
Matias Guijarro's avatar
Matias Guijarro committed
137

138
    def initialize_hardware_axis(self, axis):
139
        pass
140

141
    def initialize_axis(self, axis):
Piergiorgio Pancino's avatar
Piergiorgio Pancino committed
142
        log_debug(self, "initializing axis %s", axis.name)
143

144
145
146
147
        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
        )
148

Matias Guijarro's avatar
Matias Guijarro committed
149
        # this is to test axis are initialized only once
150
        self._axes_data[axis]["init_count"] += 1
151
        axis.stop_jog_called = False
Matias Guijarro's avatar
Matias Guijarro committed
152

153
154
155
156
157
        # the next lines are there to test issue #1601
        old_low_limit = axis.low_limit
        axis.low_limit = -999
        axis.low_limit = old_low_limit

Matias Guijarro's avatar
Matias Guijarro committed
158
    def initialize_encoder(self, encoder):
159
160
161
        enc_config = self.__encoders.setdefault(encoder, {})
        enc_config.setdefault("measured_noise", None)
        enc_config.setdefault("steps", None)
Matias Guijarro's avatar
Matias Guijarro committed
162

Cyril Guilloud's avatar
Cyril Guilloud committed
163
    """
Matias Guijarro's avatar
Matias Guijarro committed
164
    Actions to perform at controller closing.
Cyril Guilloud's avatar
Cyril Guilloud committed
165
    """
166

Matias Guijarro's avatar
Matias Guijarro committed
167
168
169
    def finalize(self):
        pass

170
171
172
173
    def _get_axis_motion(self, axis, t=None):
        """Get an updated motion object.
           Also updates the motor hardware position setting if a motion is
           occuring"""
174
        motion = self._axis_moves[axis]["motion"]
175

176
177
178
179
        if motion:
            if t is None:
                t = time.time()
            pos = motion.trajectory.position(t)
180
            self.set_hw_position(axis, pos)
181
            if t > motion.trajectory.tf:
182
                self._axis_moves[axis]["motion"] = motion = None
183
184
        return motion

185
    def set_hw_limits(self, axis, low_limit, high_limit):
Piergiorgio Pancino's avatar
Piergiorgio Pancino committed
186
        log_debug(self, "set axis limit low=%s, high=%s", low_limit, high_limit)
187
        if low_limit is None:
188
            low_limit = float("-inf")
189
        if high_limit is None:
190
            high_limit = float("+inf")
191
        if high_limit < low_limit:
192
193
194
            raise ValueError("Cannot set hard low limit > high limit")
        ll = axis.user2dial(low_limit) * axis.steps_per_unit
        hl = axis.user2dial(high_limit) * axis.steps_per_unit
195
196
        # low limit and high limits may now be exchanged,
        # because of the signs or steps per unit or user<->dial conversion
197
        if hl < ll:
198
            ll, hl = hl, ll
199
        self.__hw_limit = (ll, hl)
200

201
202
203
    def start_all(self, *motion_list):
        t0 = time.time()
        for motion in motion_list:
204
            self.start_one(motion, t0=t0)
Matias Guijarro's avatar
Matias Guijarro committed
205

206
    def start_one(self, motion, t0=None):
Matias Guijarro's avatar
Matias Guijarro committed
207
        assert isinstance(motion.target_pos, float)
Matias Guijarro's avatar
Matias Guijarro committed
208
        axis = motion.axis
Piergiorgio Pancino's avatar
Piergiorgio Pancino committed
209
        log_debug(self, "moving %s to %s", axis.name, motion.target_pos)
210
        if self._get_axis_motion(axis):
211
            raise RuntimeError("Cannot start motion. Motion already in place")
212
        pos = self.read_position(axis)
213
214
        vel = self.read_velocity(axis)
        accel = self.read_acceleration(axis)
215
        end_pos = motion.target_pos
216
217
        if t0 is None:
            t0 = time.time()
218
        axis_motion = Motion(pos, end_pos, vel, accel, self.__hw_limit, ti=t0)
219
        self._axis_moves[axis]["motion"] = axis_motion
Matias Guijarro's avatar
Matias Guijarro committed
220

221
    def start_jog(self, axis, velocity, direction):
222
        axis.stop_jog_called = False
223
        t0 = time.time()
224
225
        pos = self.read_position(axis)
        self.set_velocity(axis, velocity)
226
        accel = self.read_acceleration(axis)
227
        target = float("+inf") if direction > 0 else float("-inf")
228
        motion = Motion(pos, target, velocity, accel, self.__hw_limit, ti=t0)
229
        self._axis_moves[axis]["motion"] = motion
230

231
    def read_position(self, axis, t=None):
Cyril Guilloud's avatar
Cyril Guilloud committed
232
        """
233
        Return the position (measured or desired) taken from controller
Cyril Guilloud's avatar
Cyril Guilloud committed
234
        in controller unit (steps).
Cyril Guilloud's avatar
Cyril Guilloud committed
235
        """
236
        gevent.sleep(0.005)  # simulate I/O
237

238
        t = t or time.time()
239
240
        motion = self._get_axis_motion(axis, t)
        if motion is None:
241
            pos = self.read_hw_position(axis)
242
243
        else:
            pos = motion.trajectory.position(t)
Piergiorgio Pancino's avatar
Piergiorgio Pancino committed
244
        log_debug(self, "%s position is %s", axis.name, pos)
245
246
247
        if math.isnan(pos):
            # issue 1551: support nan as a position
            return pos
248
249
        return int(round(pos))

250
    def read_encoder(self, encoder):
251
        """
252
        Return encoder position.
253
254
        unit : 'encoder steps'
        """
255
256
257
258
        amplitude = self.__encoders[encoder]["measured_noise"]
        if amplitude is not None and amplitude > 0:
            # Simulates noisy encoder.
            noise_mm = random.uniform(-amplitude, amplitude)
259
        else:
260
261
262
263
264
265
266
267
            noise_mm = 0
        enc_steps = self.__encoders[encoder]["steps"]
        axis = encoder.axis
        if axis:
            if enc_steps is None:
                _pos = self.read_position(axis) / float(axis.steps_per_unit)
                if noise_mm:
                    _pos += noise_mm
268
269
                enc_steps = _pos * encoder.steps_per_unit
        return enc_steps
Matias Guijarro's avatar
Matias Guijarro committed
270

271
272
273
    def read_encoder_multiple(self, *encoder_list):
        return [self.read_encoder(enc) for enc in encoder_list]

Matias Guijarro's avatar
Matias Guijarro committed
274
    def set_encoder(self, encoder, encoder_steps):
275
        self.__encoders[encoder]["steps"] = encoder_steps
Matias Guijarro's avatar
Matias Guijarro committed
276

cyril@lid269's avatar
cyril@lid269 committed
277
278
279
    """
    VELOCITY
    """
280

281
    def read_velocity(self, axis):
Cyril Guilloud's avatar
Cyril Guilloud committed
282
        """
283
        Return the current velocity taken from controller
284
        in motor units.
Cyril Guilloud's avatar
Cyril Guilloud committed
285
        """
286
        return self._axes_data[axis]["curr_velocity"].get() * abs(axis.steps_per_unit)
Matias Guijarro's avatar
Matias Guijarro committed
287

288
    def set_velocity(self, axis, new_velocity):
Cyril Guilloud's avatar
Cyril Guilloud committed
289
        """
290
        <new_velocity> is in motor units
Cyril Guilloud's avatar
Cyril Guilloud committed
291
        """
292
        vel = new_velocity / abs(axis.steps_per_unit)
293
294
        if vel >= 1e9:
            raise RuntimeError("Invalid velocity")
295
        self._axes_data[axis]["curr_velocity"].set(vel)
296
        return vel
297

cyril@lid269's avatar
cyril@lid269 committed
298
299
300
    """
    ACCELERATION
    """
301

cyril@lid269's avatar
cyril@lid269 committed
302
    def read_acceleration(self, axis):
blissadm's avatar
blissadm committed
303
304
305
        """
        must return acceleration in controller units / s2
        """
306
        return self._axes_data[axis]["curr_acc"].get() * abs(axis.steps_per_unit)
cyril@lid269's avatar
cyril@lid269 committed
307
308

    def set_acceleration(self, axis, new_acceleration):
blissadm's avatar
blissadm committed
309
310
311
        """
        <new_acceleration> is in controller units / s2
        """
312
        acc = new_acceleration / abs(axis.steps_per_unit)
313
314
        if acc >= 1e9:
            raise RuntimeError("Invalid acceleration")
315
        self._axes_data[axis]["curr_acc"].set(acc)
316
        return acc
cyril@lid269's avatar
cyril@lid269 committed
317

cyril@lid269's avatar
cyril@lid269 committed
318
319
320
    """
    ON / OFF
    """
321

322
    def set_on(self, axis):
323
324
        self._hw_state.clear()
        self._hw_state.set("READY")
325
326

    def set_off(self, axis):
327
        self._hw_state.set("OFF")
328

lolo@lid269's avatar
lolo@lid269 committed
329
330
331
    """
    Hard limits
    """
332

333
334
335
    def _check_hw_limits(self, axis):
        ll, hl = self.__hw_limit
        pos = self.read_position(axis)
336
        if pos <= ll:
337
            return AxisState("READY", "LIMNEG")
338
        elif pos >= hl:
339
            return AxisState("READY", "LIMPOS")
340
        if self._hw_state.OFF:
341
342
343
344
345
            return AxisState("OFF")
        else:
            s = AxisState(self._hw_state)
            s.set("READY")
            return s
346

Cyril Guilloud's avatar
Cyril Guilloud committed
347
    """
348
    STATE
Cyril Guilloud's avatar
Cyril Guilloud committed
349
    """
350

Matias Guijarro's avatar
Matias Guijarro committed
351
    def state(self, axis):
352
        gevent.sleep(0.005)  # simulate I/O
353
354
        motion = self._get_axis_motion(axis)
        if motion is None:
355
            return self._check_hw_limits(axis)
356
        else:
357
            return AxisState("MOVING")
358

359
360
361
362
    def stop_jog(self, axis):
        axis.stop_jog_called = True
        return Controller.stop_jog(self, axis)

Cyril Guilloud's avatar
Cyril Guilloud committed
363
    """
Matias Guijarro's avatar
Matias Guijarro committed
364
    Must send a command to the controller to abort the motion of given axis.
Cyril Guilloud's avatar
Cyril Guilloud committed
365
    """
366

367
    def stop(self, axis, t=None):
368
369
        if t is None:
            t = time.time()
370
371
        motion = self._get_axis_motion(axis, t)
        if motion:
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
            # simulate deceleration
            ti = motion.trajectory.ti
            pi = motion.trajectory.pi
            pf = motion.trajectory.pf
            pa = motion.trajectory.pa
            pb = motion.trajectory.pb
            pos = self.read_position(axis)
            d = 1 if motion.trajectory.positive else -1
            a = motion.trajectory.acceleration
            v = motion.trajectory.velocity

            if math.isinf(pf):
                # jog
                new_pi = pi
                new_pf = pos + d * motion.trajectory.accel_dp
            else:
                if d > 0:
                    # going from pi to pa, pb, then pf
                    if pos < pa:
                        # didn't reach full velocity yet
                        new_pi = pi
                        new_pf = pos + (pos - pi)
                    elif pos > pb:
                        # already decelerrating
                        new_pi = pi
                        new_pf = pf - (pos - pb)
                    else:
                        new_pi = pi
                        new_pf = pf - (pb - pos)
                else:
                    if pos > pa:
                        new_pi = pi
                        new_pf = pos - (pi - pos)
                    elif pos < pb:
                        new_pi = pi
                        new_pf = pf + (pb - pos)
                    else:
                        new_pi = pi
                        new_pf = pf + (pos - pb)

            self._axis_moves[axis]["motion"] = Motion(
                new_pi, new_pf, v, a, self.__hw_limit, ti=ti
            )
Matias Guijarro's avatar
Matias Guijarro committed
415

416
    def stop_all(self, *motion_list):
417
        t = time.time()
418
        for motion in motion_list:
419
            self.stop(motion.axis, t=t)
420

lolo@lid269's avatar
lolo@lid269 committed
421
422
423
    """
    HOME and limits search
    """
424

425
    def home_search(self, axis, switch):
426
        self._axis_moves[axis]["delta"] = switch
427
        self._axis_moves[axis]["t0"] = time.time()
Cyril Guilloud's avatar
Cyril Guilloud committed
428
429
        self._axis_moves[axis]["home_search_start_time"] = time.time()

430
    def home_state(self, axis):
431
        if (time.time() - self._axis_moves[axis]["home_search_start_time"]) > 1:
432
            self.set_hw_position(axis, 0)
cyril@lid269's avatar
cyril@lid269 committed
433
434
435
            return AxisState("READY")
        else:
            return AxisState("MOVING")
Cyril Guilloud's avatar
Cyril Guilloud committed
436

437
    def limit_search(self, axis, limit):
438
        target = float("+inf") if limit > 0 else float("-inf")
439
440
441
442
        pos = self.read_position(axis)
        vel = self.read_velocity(axis)
        accel = self.read_acceleration(axis)
        motion = Motion(pos, target, vel, accel, self.__hw_limit)
443
        self._axis_moves[axis]["motion"] = motion
444

Cyril Guilloud's avatar
Cyril Guilloud committed
445
446
447
448
449
450
451
452
453
454
455
456
457
    def __info__(self):
        """Return information about Controller"""
        info_str = f"Controller name: {self.name}\n"

        return info_str

    def get_axis_info(self, axis):
        """ Return 'mockup'-specific info about <axis>
        """
        info_str = "MOCKUP AXIS:\n"
        info_str += f"    this axis ({axis.name}) is a simulation axis\n"

        return info_str
Cyril Guilloud's avatar
Cyril Guilloud committed
458

459
460
461
    def get_id(self, axis):
        return "MOCKUP AXIS %s" % (axis.name)

462
463
464
465
    def set_position(self, axis, new_position):
        """ Set the position of <axis> in controller to <new_position>.
        This method is the way to define an offset for <axis>.
        """
466
467
        motion = self._get_axis_motion(axis)
        if motion:
468
            raise RuntimeError("Cannot set position while moving !")
469

470
471
        self.set_hw_position(axis, new_position)
        self._axis_moves[axis]["target"] = new_position
472

473
        return new_position
Matias Guijarro's avatar
Matias Guijarro committed
474

475
    def put_discrepancy(self, axis, disc):
476
477
478
        """Create a discrepancy (for testing purposes) between axis and
        controller.
        """
479
        self.set_position(axis, self.read_position(axis) + disc)
480

Cyril Guilloud's avatar
Cyril Guilloud committed
481
    """
cyril@lid269's avatar
cyril@lid269 committed
482
    Custom axis methods
Cyril Guilloud's avatar
Cyril Guilloud committed
483
    """
484
    # VOID VOID
485
    @object_method
486
    def custom_park(self, axis):
487
        """doc-str of custom_park"""
488
        log_debug(self, "custom_park : parking")
489
        self._hw_state.set("PARKED")
490
491

    # VOID LONG
492
    @object_method(types_info=("None", "int"))
493
494
495
    def custom_get_forty_two(self, axis):
        return 42

496
    # LONG LONG  + renaming.
497
    @object_method(name="CustomGetTwice", types_info=("int", "int"))
498
499
500
501
    def custom_get_twice(self, axis, LongValue):
        return LongValue * 2

    # STRING STRING
502
    @object_method(types_info=("str", "str"))
503
    def custom_get_chapi(self, axis, value):
504
        """doc-str of custom_get_chapi"""
505
506
507
508
509
510
        if value == "chapi":
            return "chapo"
        elif value == "titi":
            return "toto"
        else:
            return "bla"
Matias Guijarro's avatar
Matias Guijarro committed
511

512
    # STRING VOID
513
    @object_method(types_info=("str", "None"))
514
    def custom_send_command(self, axis, value):
515
        log_debug(self, "custom_send_command(axis=%s value=%r):" % (axis.name, value))
blissadm@lid269 ID26's avatar
blissadm@lid269 ID26 committed
516

517
    # BOOL NONE
518
    @object_method(name="Set_Closed_Loop", types_info=("bool", "None"))
519
520
    def _set_closed_loop(self, axis, onoff=True):
        pass  # print "I set the closed loop ", onoff
521

522
    # Types by default (None, None)
523
    @object_method
blissadm@lid269 ID26's avatar
blissadm@lid269 ID26 committed
524
    def custom_command_no_types(self, axis):
Vincent Michel's avatar
Vincent Michel committed
525
        print("print with no types")
526

527
528
529
530
531
    @object_method
    def generate_error(self, axis):
        # For testing purposes.
        raise RuntimeError("Testing Error")

532
533
534
    def custom_get_measured_noise(self, axis):
        noise = 0.0
        if not axis.encoder in self.__encoders:
535
536
537
            raise KeyError(
                "cannot read measured noise: %s " "doesn't have encoder" % axis.name
            )
538
        noise = self.__encoders[axis.encoder].get("measured_noise", None)
539

540
    @object_method(types_info=("float", "None"))
541
542
543
544
545
    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.
546
        By the way we add a ref to the coresponding axis.
547
        """
548
        self.__encoders.setdefault(axis.encoder, {})["measured_noise"] = noise
549

Cyril Guilloud's avatar
Cyril Guilloud committed
550
551
552
    """
    Custom attributes methods
    """
553
554

    @object_attribute_get(type_info="int")
555
    def get_voltage(self, axis):
556
        """doc-str of get_voltage"""
557
        return self.__voltages.setdefault(axis, 10000)
558

559
    @object_attribute_set(type_info="int")
560
    def set_voltage(self, axis, voltage):
561
        """doc-str of set_voltage"""
562
        self.__voltages[axis] = voltage
Cyril Guilloud's avatar
Cyril Guilloud committed
563

564
    @object_attribute_get(type_info="float")
Cyril Guilloud's avatar
Cyril Guilloud committed
565
    def get_cust_attr_float(self, axis):
566
        return self.__cust_attr_float.setdefault(axis, 9.999)
Cyril Guilloud's avatar
Cyril Guilloud committed
567

568
    @object_attribute_set(type_info="float")
Cyril Guilloud's avatar
Cyril Guilloud committed
569
    def set_cust_attr_float(self, axis, value):
Cyril Guilloud's avatar
Cyril Guilloud committed
570
        self.__cust_attr_float[axis] = value
Cyril Guilloud's avatar
Cyril Guilloud committed
571

572
573
574
    def has_trajectory(self):
        return True

575
    def prepare_trajectory(self, *trajectories):
576
577
        pass

578
    def move_to_trajectory(self, *trajectories):
579
580
        pass

581
    def start_trajectory(self, *trajectories):
582
        pass
583

584
585
586
587
    def stop_trajectory(self, *trajectories):
        pass


588
589
590
591
592
class MockupHook(MotionHook):
    """Motion hook used for pytest"""

    class Error(Exception):
        """Mockup hook error"""
593

594
595
596
597
598
599
600
601
602
603
604
        pass

    def __init__(self, name, config):
        super(MockupHook, self).__init__()
        self.name = name
        self.config = config
        self.nb_pre_move = 0
        self.nb_post_move = 0
        self.last_pre_move_args = ()
        self.last_post_move_args = ()

605
    def pre_move(self, motion_list):
606
        print(self.name, "in pre_move hook")
607
608
        if self.config.get("pre_move_error", False):
            raise self.Error("cannot pre_move")
609
610
611
        self.nb_pre_move += 1
        self.last_pre_move_args = motion_list

612
    def post_move(self, motion_list):
613
        print(self.name, "in post_move hook")
614
615
        if self.config.get("post_move_error", False):
            raise self.Error("cannot post_move")
616
617
        self.nb_post_move += 1
        self.last_post_move_args = motion_list
618

619

620
621
622
623
624
class FaultyMockup(Mockup):
    def __init__(self, *args, **kwargs):
        Mockup.__init__(self, *args, **kwargs)

        self.bad_state = False
625
        self.fault_state = False
626
627
628
        self.bad_start = False
        self.bad_state_after_start = False
        self.bad_stop = False
629
        self.bad_position = False
630
        self.bad_position_only_once = False
631
        self.nan_position = False
632
        self.position_reading_delay = 0
633
634
635
636
637
638
639
        self.state_recovery_delay = 1
        self.state_msg_index = 0

    def state(self, axis):
        if self.bad_state:
            self.state_msg_index += 1
            raise RuntimeError("BAD STATE %d" % self.state_msg_index)
640
641
642
        elif self.fault_state:
            self._axis_moves[axis]["motion"] = None  # stop motion immediately
            return AxisState("FAULT")
643
644
645
        else:
            return Mockup.state(self, axis)

646
647
648
649
650
651
652
653
654
655
656
657
658
659
    def _check_hw_limits(self, axis):
        ll, hl = self._Mockup__hw_limit
        pos = super().read_position(axis)
        if pos <= ll:
            return AxisState("READY", "LIMNEG")
        elif 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

660
661
662
663
664
665
666
667
668
669
    def start_one(self, motion, **kw):
        self.state_msg_index = 0
        if self.bad_start:
            raise RuntimeError("BAD START")
        else:
            try:
                return Mockup.start_one(self, motion, **kw)
            finally:
                if self.bad_state_after_start:
                    self.bad_state = True
670
671
672
                    gevent.spawn_later(
                        self.state_recovery_delay, setattr, self, "bad_state", False
                    )
673
674
675
676
677
678

    def stop(self, axis, **kw):
        if self.bad_stop:
            raise RuntimeError("BAD STOP")
        else:
            return Mockup.stop(self, axis, **kw)
679

680
    def read_position(self, axis, t=None):
681
682
        if self.position_reading_delay > 0:
            gevent.sleep(self.position_reading_delay)
683
684
        if self.bad_position:
            raise RuntimeError("BAD POSITION")
685
686
687
        elif self.bad_position_only_once:
            self.bad_position_only_once = False
            raise RuntimeError("BAD POSITION")
688
689
        elif self.nan_position:
            return float("nan")
690
691
692
        else:
            return Mockup.read_position(self, axis, t)

693
694
695
696
697
698
699
700
701
702
703
704
705

class CustomMockup(Mockup):
    def __init__(self, *args, **kwargs):
        Mockup.__init__(self, *args, **kwargs)

        self.axis_settings.add("custom_setting1", str)

    @object_method(types_info=(None, str))
    def set_custom_setting1(self, axis, new_value=None):
        pass

    def read_custom_setting1(self, axis):
        pass
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747


class calc_motor_mockup(CalcController):
    """
    Calculation Bliss controller

    real_mot
        real motor axis alias

    calc_mot
        calculated axis alias

    s_param
        specific_parameter to use for the calculation axis (e.g. gain factor)
        As it can change, we want to treat it as settings parameter as well.
        The parameter can have an initial value in the config file.

    Example of the config file:

    .. code-block:: yaml

        controller:
            class: calc_motor_mockup
            axes:
                -
                    name: $real_motor_name
                    tags: real real_mot
                -
                    name: calc_mot
                    tags: calc_mot
                    s_param: 2 #this is optional
    """

    def __init__(self, *args, **kwargs):
        CalcController.__init__(self, *args, **kwargs)
        self._axis = None
        self.axis_settings.add("s_param", float)

    def initialize_axis(self, axis):
        self._axis = axis
        CalcController.initialize_axis(self, axis)
        event.connect(axis, "s_param", self._calc_from_real)
Linus Pithan's avatar
Linus Pithan committed
748
        axis._unit = "keV"
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770

    def close(self):
        if self._axis is not None:
            event.disconnect(self._axis, "s_param", self._calc_from_real)
            self._axis = None
        super(calc_motor_mockup, self).close()

    """
    #Example to use s_param as property instead of settings.
    #s_param is set in the YAML config file.
    @property
    def s_param(self):
        return self.__s_param

    @s_param.setter
    def s_param(self, s_param):
        self.__s_param = s_param
        self._calc_from_real()
    """

    def calc_from_real(self, positions_dict):
        calc_mot_axis = self._tagged["calc_mot"][0]
Linus Pithan's avatar
Linus Pithan committed
771
        calc_mot_axis._unit == "keV"
772
773
774
775
776
777
778
779
780
781
782
783
784
        s_param = calc_mot_axis.settings.get("s_param")
        # this formula is just an example
        calc_pos = s_param * positions_dict["real_mot"]

        return {"calc_mot": calc_pos}

    def calc_to_real(self, positions_dict):
        calc_mot_axis = self._tagged["calc_mot"][0]
        s_param = calc_mot_axis.settings.get("s_param")
        # this formula is just an example
        real_pos = positions_dict["calc_mot"] / s_param

        return {"real_mot": real_pos}
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829


# issue 1909
class llangle_mockup(CalcController):
    def initialize(self):
        CalcController.initialize(self)
        self.bend_zero = self.config.get("bend_zero", float)
        self.bend_y = self.config.get("bend_y", float)
        self.ty_zero = self.config.get("ty_zero", float)

    def calc_from_real(self, positions_dict):
        # Angle due to pusher not being a rotation
        # Effect of bending
        # Effect of translation
        bend = positions_dict["bend"]
        rz = positions_dict["rz"]
        ty = positions_dict["ty"]

        truebend = bend - self.bend_zero  # pass through
        absty = ty - self.ty_zero
        bend_offset = np.degrees(truebend * absty / self.bend_y)
        # only for bent crystal and mono in beam
        valid = (truebend > 0) & (absty < 75.)
        angle = np.where(valid, rz + bend_offset, rz)
        calc_dict = {"angle": angle, "truebend": truebend, "absty": absty}  # computed
        return calc_dict

    def calc_to_real(self, positions_dict):
        #
        angle = positions_dict["angle"]
        # Effect of bending
        truebend = positions_dict["truebend"]
        bend = truebend + self.bend_zero
        # Effect of translation
        absty = positions_dict["absty"]  # llty1 / llty2
        ty = absty + self.ty_zero
        # Assume we go to the destination ty / bend.
        # Compute the effect for the angle only
        bend_offset = np.degrees(truebend * absty / self.bend_y)
        # only for bent crystal and mono in beam
        valid = (truebend > 0) & (absty < 75.)
        # - versus + above:
        rz = np.where(valid, angle - bend_offset, angle)
        calc_dict = {"bend": bend, "ty": ty, "rz": rz}
        return calc_dict
830
831
832
833
834
835
836
837


class FaultyCalc(CalcController):
    def calc_from_real(self, positions_dict):
        return {"calc_mot": None}

    def calc_to_real(self, positions_dict):
        return {self._axis_tag(x): None for x in self.reals}
Perceval Guillou's avatar
Perceval Guillou committed
838
839


Linus Pithan's avatar
Linus Pithan committed
840
841
842
843
844
845
846
class CoupledMotionCalc(CalcController):
    def calc_from_real(self, positions_dict):
        return {"calc_mot": positions_dict["mot1"]}

    def calc_to_real(self, positions_dict):
        real_pos = positions_dict["calc_mot"]
        return {self._axis_tag(x): real_pos for x in self.reals}