mockup.py 28.1 KB
Newer Older
1
2
3
4
# -*- coding: utf-8 -*-
#
# This file is part of the bliss project
#
5
# Copyright (c) 2015-2022 Beamline Control Unit, ESRF
6
# Distributed under the GNU LGPLv3. See LICENSE for more info.
7
8
9
10
11
12
13
14
15
16
17

"""
mockup.py : a mockup controller for bliss.

config :
 'velocity' in unit/s
 'acceleration' in unit/s^2
 'steps_per_unit' in unit^-1  (default 1)
 'backlash' in unit
"""

18
import math
19
20
import time
import random
21
import collections
22
import gevent
23
import numpy as np
24

25
from bliss.physics.trajectory import LinearTrajectory
26
from bliss.controllers.motor import Controller, CalcController
27
from bliss.common.axis import Axis, AxisState
28
from bliss.common.closed_loop import ClosedLoopState
29
from bliss.common import event
30
from bliss.config.settings import SimpleSetting
31
from bliss.common.hook import MotionHook
32
33
from bliss.common.utils import object_method
from bliss.common.utils import object_attribute_get, object_attribute_set
34
from bliss.common.logtools import log_debug
35

36

37
38
39
40
41
42
43
44
45
46
def rand_noise(min_deviation, max_deviation):
    """
    Pick a value in:
    ]-max_deviation, -min_deviation] U [min_deviation, max_deviation[
    """
    assert min_deviation <= max_deviation
    abs_val = random.uniform(min_deviation, max_deviation)
    return random.choice((-abs_val, abs_val))


47
class Motion:
48
49
50
51
52
53
54
55
56
57
    """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
58
        self.trajectory = LinearTrajectory(pi, pf, velocity, acceleration, ti)
59
60


61
class MockupAxis(Axis):
62
    def __init__(self, *args, **kwargs):
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
        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):
79
80
81
82
    """
    Simulated motor controller for tests and demo.
    """

83
84
    def __init__(self, *args, **kwargs):  # config
        super().__init__(*args, **kwargs)
Matias Guijarro's avatar
Matias Guijarro committed
85
86

        self._axis_moves = {}
Matias Guijarro's avatar
Matias Guijarro committed
87
        self.__encoders = {}
88
        self.__switches = {}
89
90
91
92

        self._cloop_state = {}
        self._cloop_params = {}

93
        self._axes_data = collections.defaultdict(dict)
Cyril Guilloud's avatar
Cyril Guilloud committed
94
95

        # Custom attributes.
96
        self.__voltages = {}
Cyril Guilloud's avatar
Cyril Guilloud committed
97
        self.__cust_attr_float = {}
Matias Guijarro's avatar
Matias Guijarro committed
98

99
        self._hw_state = AxisState("READY")
100
        self.__hw_limit = float("-inf"), float("+inf")
101

102
        self._hw_state.create_state("PARKED", "mot au parking")
Matias Guijarro's avatar
Matias Guijarro committed
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
139
        self._cloop_state[axis] = ClosedLoopState.UNKNOWN

140
    def initialize_hardware_axis(self, axis):
141
142
143
144
        if axis.closed_loop:
            self._cloop_params["kp"] = axis.closed_loop.kp
            self._cloop_params["ki"] = axis.closed_loop.ki
            self._cloop_params["kd"] = axis.closed_loop.kd
145

146
    def initialize_axis(self, axis):
Piergiorgio Pancino's avatar
Piergiorgio Pancino committed
147
        log_debug(self, "initializing axis %s", axis.name)
148

149
150
151
152
        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
        )
153

Matias Guijarro's avatar
Matias Guijarro committed
154
        # this is to test axis are initialized only once
155
        self._axes_data[axis]["init_count"] += 1
156
        axis.stop_jog_called = False
Matias Guijarro's avatar
Matias Guijarro committed
157

158
159
160
161
162
        # 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
163
    def initialize_encoder(self, encoder):
164
165
166
167
        """
        If linked to an axis, encoder initialization is called at axis
        initialization.
        """
168
        enc_config = self.__encoders.setdefault(encoder, {})
169
170
171
        enc_config.setdefault(
            "measured_noise", {"min_deviation": 0, "max_deviation": 0}
        )
172
173
174
175
        # mock_offset simulates the coupling between the controller and
        # encoder position. For encoders with no axis, mock_offset simply
        # represents the encoder's absolute position.
        enc_config.setdefault("mock_offset", 0)
Matias Guijarro's avatar
Matias Guijarro committed
176

Matias Guijarro's avatar
Matias Guijarro committed
177
178
179
    def finalize(self):
        pass

180
    def _get_axis_motion(self, axis, t=None):
181
182
183
184
185
        """
        Get an updated motion object.
        Also updates the motor hardware position setting if a motion is
        occuring
        """
186
        motion = self._axis_moves[axis]["motion"]
187

188
189
190
191
        if motion:
            if t is None:
                t = time.time()
            pos = motion.trajectory.position(t)
192
            self.set_hw_position(axis, pos)
193
            if t > motion.trajectory.tf:
194
                self._axis_moves[axis]["motion"] = motion = None
195
196
        return motion

197
    def set_hw_limits(self, axis, low_limit, high_limit):
Piergiorgio Pancino's avatar
Piergiorgio Pancino committed
198
        log_debug(self, "set axis limit low=%s, high=%s", low_limit, high_limit)
199
        if low_limit is None:
200
            low_limit = float("-inf")
201
        if high_limit is None:
202
            high_limit = float("+inf")
203
        if high_limit < low_limit:
204
205
206
            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
207
208
        # low limit and high limits may now be exchanged,
        # because of the signs or steps per unit or user<->dial conversion
209
        if hl < ll:
210
            ll, hl = hl, ll
211
        self.__hw_limit = (ll, hl)
212

213
214
215
    def start_all(self, *motion_list):
        t0 = time.time()
        for motion in motion_list:
216
            self.start_one(motion, t0=t0)
Matias Guijarro's avatar
Matias Guijarro committed
217

218
    def start_one(self, motion, t0=None):
Matias Guijarro's avatar
Matias Guijarro committed
219
        assert isinstance(motion.target_pos, float)
Matias Guijarro's avatar
Matias Guijarro committed
220
        axis = motion.axis
Piergiorgio Pancino's avatar
Piergiorgio Pancino committed
221
        log_debug(self, "moving %s to %s", axis.name, motion.target_pos)
222
        if self._get_axis_motion(axis):
223
            raise RuntimeError("Cannot start motion. Motion already in place")
224
        pos = self.read_position(axis)
225
226
        vel = self.read_velocity(axis)
        accel = self.read_acceleration(axis)
227
        end_pos = motion.target_pos
228
229
        if t0 is None:
            t0 = time.time()
230
        axis_motion = Motion(pos, end_pos, vel, accel, self.__hw_limit, ti=t0)
231
        self._axis_moves[axis]["motion"] = axis_motion
Matias Guijarro's avatar
Matias Guijarro committed
232

233
    def start_jog(self, axis, velocity, direction):
234
        axis.stop_jog_called = False
235
        t0 = time.time()
236
237
        pos = self.read_position(axis)
        self.set_velocity(axis, velocity)
238
        accel = self.read_acceleration(axis)
239
        target = float("+inf") if direction > 0 else float("-inf")
240
        motion = Motion(pos, target, velocity, accel, self.__hw_limit, ti=t0)
241
        self._axis_moves[axis]["motion"] = motion
242

243
    def read_position(self, axis, t=None):
Cyril Guilloud's avatar
Cyril Guilloud committed
244
        """
245
        Return the position (measured or desired) taken from controller
Cyril Guilloud's avatar
Cyril Guilloud committed
246
        in controller unit (steps).
Cyril Guilloud's avatar
Cyril Guilloud committed
247
        """
248
        gevent.sleep(0.005)  # simulate I/O
249

250
        t = t or time.time()
251
252
        motion = self._get_axis_motion(axis, t)
        if motion is None:
253
            pos = self.read_hw_position(axis)
254
255
        else:
            pos = motion.trajectory.position(t)
Piergiorgio Pancino's avatar
Piergiorgio Pancino committed
256
        log_debug(self, "%s position is %s", axis.name, pos)
257
258
259
        if math.isnan(pos):
            # issue 1551: support nan as a position
            return pos
260
261
        return int(round(pos))

262
    def read_encoder(self, encoder):
263
        """
264
        Return encoder position.
265
266
        unit : 'encoder steps'
        """
267
268
        axis = encoder.axis
        if axis:
269
270
271
272
            motor_pos = self.read_position(axis) / float(axis.steps_per_unit)
            encoder_pos = motor_pos + self.__encoders[encoder]["mock_offset"]
        else:
            encoder_pos = self.__encoders[encoder]["mock_offset"]
273
274
        noise = rand_noise(**self.__encoders[encoder]["measured_noise"])
        return (encoder_pos + noise) * encoder.steps_per_unit
Matias Guijarro's avatar
Matias Guijarro committed
275

276
277
278
    def read_encoder_multiple(self, *encoder_list):
        return [self.read_encoder(enc) for enc in encoder_list]

Matias Guijarro's avatar
Matias Guijarro committed
279
    def set_encoder(self, encoder, encoder_steps):
280
281
282
283
284
285
286
        target_pos = encoder_steps / encoder.steps_per_unit
        axis = encoder.axis
        if axis:
            motor_pos = self.read_position(axis) / float(axis.steps_per_unit)
            self.__encoders[encoder]["mock_offset"] = target_pos - motor_pos
        else:
            self.__encoders[encoder]["mock_offset"] = target_pos
Matias Guijarro's avatar
Matias Guijarro committed
287

cyril@lid269's avatar
cyril@lid269 committed
288
289
290
    """
    VELOCITY
    """
291

292
    def read_velocity(self, axis):
Cyril Guilloud's avatar
Cyril Guilloud committed
293
        """
294
        Return the current velocity taken from controller
295
        in motor units.
Cyril Guilloud's avatar
Cyril Guilloud committed
296
        """
297
        return self._axes_data[axis]["curr_velocity"].get() * abs(axis.steps_per_unit)
Matias Guijarro's avatar
Matias Guijarro committed
298

299
    def set_velocity(self, axis, new_velocity):
Cyril Guilloud's avatar
Cyril Guilloud committed
300
        """
301
        <new_velocity> is in motor units
Cyril Guilloud's avatar
Cyril Guilloud committed
302
        """
303
        vel = new_velocity / abs(axis.steps_per_unit)
304
305
        if vel >= 1e9:
            raise RuntimeError("Invalid velocity")
306
        self._axes_data[axis]["curr_velocity"].set(vel)
307
        return vel
308

cyril@lid269's avatar
cyril@lid269 committed
309
310
311
    """
    ACCELERATION
    """
312

cyril@lid269's avatar
cyril@lid269 committed
313
    def read_acceleration(self, axis):
blissadm's avatar
blissadm committed
314
315
316
        """
        must return acceleration in controller units / s2
        """
317
        return self._axes_data[axis]["curr_acc"].get() * abs(axis.steps_per_unit)
cyril@lid269's avatar
cyril@lid269 committed
318
319

    def set_acceleration(self, axis, new_acceleration):
blissadm's avatar
blissadm committed
320
321
322
        """
        <new_acceleration> is in controller units / s2
        """
323
        acc = new_acceleration / abs(axis.steps_per_unit)
324
325
        if acc >= 1e9:
            raise RuntimeError("Invalid acceleration")
326
        self._axes_data[axis]["curr_acc"].set(acc)
327
        return acc
cyril@lid269's avatar
cyril@lid269 committed
328

cyril@lid269's avatar
cyril@lid269 committed
329
330
331
    """
    ON / OFF
    """
332

333
    def set_on(self, axis):
334
335
        self._hw_state.clear()
        self._hw_state.set("READY")
336
337

    def set_off(self, axis):
338
        self._hw_state.set("OFF")
339

lolo@lid269's avatar
lolo@lid269 committed
340
341
342
    """
    Hard limits
    """
343

344
345
346
    def _check_hw_limits(self, axis):
        ll, hl = self.__hw_limit
        pos = self.read_position(axis)
347
        if pos <= ll:
348
            return AxisState("READY", "LIMNEG")
349
        elif pos >= hl:
350
            return AxisState("READY", "LIMPOS")
351
        if self._hw_state.OFF:
352
353
354
355
356
            return AxisState("OFF")
        else:
            s = AxisState(self._hw_state)
            s.set("READY")
            return s
357

Cyril Guilloud's avatar
Cyril Guilloud committed
358
    """
359
    STATE
Cyril Guilloud's avatar
Cyril Guilloud committed
360
    """
361

Matias Guijarro's avatar
Matias Guijarro committed
362
    def state(self, axis):
363
        gevent.sleep(0.005)  # simulate I/O
364
365
        motion = self._get_axis_motion(axis)
        if motion is None:
366
            return self._check_hw_limits(axis)
367
        else:
368
            return AxisState("MOVING")
369

370
371
372
373
    def stop_jog(self, axis):
        axis.stop_jog_called = True
        return Controller.stop_jog(self, axis)

Cyril Guilloud's avatar
Cyril Guilloud committed
374
    """
Matias Guijarro's avatar
Matias Guijarro committed
375
    Must send a command to the controller to abort the motion of given axis.
Cyril Guilloud's avatar
Cyril Guilloud committed
376
    """
377

378
    def stop(self, axis, t=None):
379
380
        if t is None:
            t = time.time()
381
382
        motion = self._get_axis_motion(axis, t)
        if motion:
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
415
416
417
418
419
420
421
422
423
424
425
            # 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
426

427
    def stop_all(self, *motion_list):
428
        t = time.time()
429
        for motion in motion_list:
430
            self.stop(motion.axis, t=t)
431

lolo@lid269's avatar
lolo@lid269 committed
432
433
434
    """
    HOME and limits search
    """
435

436
    def home_search(self, axis, switch):
437
        self._axis_moves[axis]["delta"] = switch
438
        self._axis_moves[axis]["t0"] = time.time()
Cyril Guilloud's avatar
Cyril Guilloud committed
439
440
        self._axis_moves[axis]["home_search_start_time"] = time.time()

441
    def home_state(self, axis):
442
        if (time.time() - self._axis_moves[axis]["home_search_start_time"]) > 1:
443
            self.set_hw_position(axis, 0)
cyril@lid269's avatar
cyril@lid269 committed
444
445
446
            return AxisState("READY")
        else:
            return AxisState("MOVING")
Cyril Guilloud's avatar
Cyril Guilloud committed
447

448
    def limit_search(self, axis, limit):
449
        target = float("+inf") if limit > 0 else float("-inf")
450
451
452
453
        pos = self.read_position(axis)
        vel = self.read_velocity(axis)
        accel = self.read_acceleration(axis)
        motion = Motion(pos, target, vel, accel, self.__hw_limit)
454
        self._axis_moves[axis]["motion"] = motion
455

Cyril Guilloud's avatar
Cyril Guilloud committed
456
457
458
459
460
461
462
    def __info__(self):
        """Return information about Controller"""
        info_str = f"Controller name: {self.name}\n"

        return info_str

    def get_axis_info(self, axis):
Valentin Valls's avatar
Valentin Valls committed
463
        """Return 'mockup'-specific info about <axis>"""
Cyril Guilloud's avatar
Cyril Guilloud committed
464
465
466
467
        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
468

469
470
471
    def get_id(self, axis):
        return "MOCKUP AXIS %s" % (axis.name)

472
    def set_position(self, axis, new_position):
Valentin Valls's avatar
Valentin Valls committed
473
        """Set the position of <axis> in controller to <new_position>.
474
475
        This method is the way to define an offset for <axis>.
        """
476
477
        motion = self._get_axis_motion(axis)
        if motion:
478
            raise RuntimeError("Cannot set position while moving !")
479

480
481
        self.set_hw_position(axis, new_position)
        self._axis_moves[axis]["target"] = new_position
482

483
        return new_position
Matias Guijarro's avatar
Matias Guijarro committed
484

485
    def put_discrepancy(self, axis, disc):
486
487
488
        """Create a discrepancy (for testing purposes) between axis and
        controller.
        """
489
        self.set_position(axis, self.read_position(axis) + disc)
490

491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
    """
    CLOSED LOOP
    """

    def get_closed_loop_requirements(self):
        return ["kp", "ki", "kd"]

    def get_closed_loop_state(self, axis):
        return self._cloop_state[axis]

    def activate_closed_loop(self, axis, onoff=True):
        if onoff:
            self._cloop_state[axis] = ClosedLoopState.ON
        else:
            self._cloop_state[axis] = ClosedLoopState.OFF
        return self._cloop_state[axis]

    def set_closed_loop_param(self, axis, param, value):
        """
        Hardware specific method to set closed loop parameters.
        """
        if param not in self.get_closed_loop_requirements():
            raise KeyError(f"Unknown closed loop parameter: {param}")
        self._cloop_params[param] = value

    def get_closed_loop_param(self, axis, param):
        """
        Hardware specific method to read closed loop parameters.
        """
        if param not in self.get_closed_loop_requirements():
            raise KeyError(f"Unknown closed loop parameter: {param}")
        return self._cloop_params[param]

Cyril Guilloud's avatar
Cyril Guilloud committed
524
    """
cyril@lid269's avatar
cyril@lid269 committed
525
    Custom axis methods
Cyril Guilloud's avatar
Cyril Guilloud committed
526
    """
527
    # VOID VOID
528
    @object_method
529
    def custom_park(self, axis):
530
        """doc-str of custom_park"""
531
        log_debug(self, "custom_park : parking")
532
        self._hw_state.set("PARKED")
533
534

    # VOID LONG
535
    @object_method(types_info=("None", "int"))
536
537
538
    def custom_get_forty_two(self, axis):
        return 42

539
    # LONG LONG  + renaming.
540
    @object_method(name="CustomGetTwice", types_info=("int", "int"))
541
542
543
544
    def custom_get_twice(self, axis, LongValue):
        return LongValue * 2

    # STRING STRING
545
    @object_method(types_info=("str", "str"))
546
    def custom_get_chapi(self, axis, value):
547
        """doc-str of custom_get_chapi"""
548
549
550
551
552
553
        if value == "chapi":
            return "chapo"
        elif value == "titi":
            return "toto"
        else:
            return "bla"
Matias Guijarro's avatar
Matias Guijarro committed
554

555
    # STRING VOID
556
    @object_method(types_info=("str", "None"))
557
    def custom_send_command(self, axis, value):
558
        log_debug(self, "custom_send_command(axis=%s value=%r):" % (axis.name, value))
blissadm@lid269 ID26's avatar
blissadm@lid269 ID26 committed
559

560
    # Types by default (None, None)
561
    @object_method
blissadm@lid269 ID26's avatar
blissadm@lid269 ID26 committed
562
    def custom_command_no_types(self, axis):
Vincent Michel's avatar
Vincent Michel committed
563
        print("print with no types")
564

565
566
567
568
569
    @object_method
    def generate_error(self, axis):
        # For testing purposes.
        raise RuntimeError("Testing Error")

570
    @object_method(types_info=("float", "None"))
571
    def custom_set_measured_noise(self, axis, max_deviation, min_deviation=0):
572
573
        """
        Custom axis method to add a random noise, given in user units,
574
575
576
        to measured positions. Use min_deviation to avoid "luckily" clean
        samples in tests which aim to outreach tolerance values.
        By the way we add a ref to the corresponding axis.
577
        """
578
579
580
581
582
        assert min_deviation <= max_deviation
        self.__encoders.setdefault(axis.encoder, {})["measured_noise"] = {
            "min_deviation": min_deviation,
            "max_deviation": max_deviation,
        }
583

Cyril Guilloud's avatar
Cyril Guilloud committed
584
585
586
    """
    Custom attributes methods
    """
587
588

    @object_attribute_get(type_info="int")
589
    def get_voltage(self, axis):
590
        """doc-str of get_voltage"""
591
        return self.__voltages.setdefault(axis, 10000)
592

593
    @object_attribute_set(type_info="int")
594
    def set_voltage(self, axis, voltage):
595
        """doc-str of set_voltage"""
596
        self.__voltages[axis] = voltage
Cyril Guilloud's avatar
Cyril Guilloud committed
597

598
    @object_attribute_get(type_info="float")
Cyril Guilloud's avatar
Cyril Guilloud committed
599
    def get_cust_attr_float(self, axis):
600
        return self.__cust_attr_float.setdefault(axis, 9.999)
Cyril Guilloud's avatar
Cyril Guilloud committed
601

602
    @object_attribute_set(type_info="float")
Cyril Guilloud's avatar
Cyril Guilloud committed
603
    def set_cust_attr_float(self, axis, value):
Cyril Guilloud's avatar
Cyril Guilloud committed
604
        self.__cust_attr_float[axis] = value
Cyril Guilloud's avatar
Cyril Guilloud committed
605

606
607
608
    def has_trajectory(self):
        return True

609
    def prepare_trajectory(self, *trajectories):
610
611
        pass

612
    def move_to_trajectory(self, *trajectories):
613
614
        pass

615
    def start_trajectory(self, *trajectories):
616
        pass
617

618
619
620
621
    def stop_trajectory(self, *trajectories):
        pass


622
623
624
625
626
class MockupHook(MotionHook):
    """Motion hook used for pytest"""

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

628
629
630
631
632
633
634
635
636
637
638
        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 = ()

639
    def pre_move(self, motion_list):
640
        print(self.name, "in pre_move hook")
641
642
        if self.config.get("pre_move_error", False):
            raise self.Error("cannot pre_move")
643
644
645
        self.nb_pre_move += 1
        self.last_pre_move_args = motion_list

646
    def post_move(self, motion_list):
647
        print(self.name, "in post_move hook")
648
649
        if self.config.get("post_move_error", False):
            raise self.Error("cannot post_move")
650
651
        self.nb_post_move += 1
        self.last_post_move_args = motion_list
652

653

654
655
656
657
658
class FaultyMockup(Mockup):
    def __init__(self, *args, **kwargs):
        Mockup.__init__(self, *args, **kwargs)

        self.bad_state = False
659
        self.fault_state = False
660
661
662
        self.bad_start = False
        self.bad_state_after_start = False
        self.bad_stop = False
663
        self.bad_encoder = False
664
        self.bad_position = False
665
        self.bad_position_only_once = False
666
        self.nan_position = False
667
        self.position_reading_delay = 0
668
669
        self.state_recovery_delay = 1
        self.state_msg_index = 0
670
        self.__encoders = {}
671
672
673
674
675

    def state(self, axis):
        if self.bad_state:
            self.state_msg_index += 1
            raise RuntimeError("BAD STATE %d" % self.state_msg_index)
676
677
678
        elif self.fault_state:
            self._axis_moves[axis]["motion"] = None  # stop motion immediately
            return AxisState("FAULT")
679
680
681
        else:
            return Mockup.state(self, axis)

682
683
684
685
686
    def initialize_encoder(self, encoder):
        """
        If linked to an axis, encoder initialization is called at axis
        initialization.
        """
687
688
689
690
        if self.bad_encoder:
            # Simulate a bug during initialization
            1 / 0
        else:
691
            super().initialize_encoder(encoder)
692

693
694
695
696
697
698
699
700
701
702
703
704
705
706
    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

707
708
709
710
711
712
713
714
715
716
    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
717
718
719
                    gevent.spawn_later(
                        self.state_recovery_delay, setattr, self, "bad_state", False
                    )
720
721
722
723
724
725

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

727
    def read_position(self, axis, t=None):
728
729
        if self.position_reading_delay > 0:
            gevent.sleep(self.position_reading_delay)
730
731
        if self.bad_position:
            raise RuntimeError("BAD POSITION")
732
733
734
        elif self.bad_position_only_once:
            self.bad_position_only_once = False
            raise RuntimeError("BAD POSITION")
735
736
        elif self.nan_position:
            return float("nan")
737
738
739
        else:
            return Mockup.read_position(self, axis, t)

740
741
742
743
744
745
746
747
748
749
750
751
752

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
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794


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
795
        axis._unit = "keV"
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817

    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
818
        calc_mot_axis._unit == "keV"
819
820
821
822
823
824
825
826
827
828
829
830
831
        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}
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853


# 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
854
        valid = (truebend > 0) & (absty < 75.0)
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
        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
872
        valid = (truebend > 0) & (absty < 75.0)
873
874
875
876
        # - versus + above:
        rz = np.where(valid, angle - bend_offset, angle)
        calc_dict = {"bend": bend, "ty": ty, "rz": rz}
        return calc_dict
877
878
879
880
881
882
883
884


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
885
886


Linus Pithan's avatar
Linus Pithan committed
887
888
889
890
891
892
893
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}