1 from __future__
import division, absolute_import
10 def checkPos(pos, minPos, maxPos, nearPos, wrapPref, doWrap):
11 """Apply wrap preference and verify that position is in bounds
13 @param[in] pos position, wrap unknown
14 @param[in] minPos minimum allowed position
15 @param[in] maxPos maximum allowed position
16 @param[in] nearPos nearby angle (the reference for near wrap or doWrap)
18 if true (slewing): use wrapPref and unwrap as desired, avoiding limits
19 if false (tracking): ignore wrapPref and only wrap near, even if out of bounds
20 @param[in] wrapPref wrap preference; ignored if doWrap false
22 - outPos: wrapped position, or nan if errCode not AxisErr_OK
24 - AxisErr_OK if no error
25 - AxisErr_MinPos if pos < minPos
26 - AxisErr_MaxPos if pos > maxPos
27 - AxisErr_CannotCompute if wrapPref = None and nearPos is not finite
29 @throw RuntimeError if wrapPref unknown (if doWrap true)
31 @warning: If the limits of motion span a range greater than 720 degrees,
32 this routine will not use the excess, unless doWrap false or wrapPref is "near".
33 See "details" for more information about the ranges used.
36 The limits minPos and maxPos are inclusive, meaning
37 outPos may equal either limit and still be in bounds.
39 The following ranges are used:
41 doWrap false [nearPos - 180, nearPos + 180)
42 WrapType_None no change; outPos = pos
44 Ideal Range (may be adjusted to avoid limits)
45 WrapType_Nearest [nearPos - 180, nearPos + 180)
46 WrapType_Negative [meanPos - 360, meanPos)
47 WrapType_Middle [meanPos - 180, meanPos + 180)
48 WrapType_Positive [meanPos, meanPos + 360)
51 - meanPos = (minPos + maxPos) / 2
52 - [,) means the lower limit is inclusive and the upper exclusive.
53 - Ideal Range means the range if limits are not an issue.
54 If the upper end is out of bounds, both ends are decreased.
55 Then if the lower end is out of bounds, both ends are increased.
56 Then the position is wrapped and range checked.
59 Given an angle X and a range [A, A + 360),
60 the angle WrapX = X wrapped into the given range is given by:
61 WrapX = Y, if Y >= A, Y + 360 otherwise,
63 Y = ((X - A) mod 360) + A
64 and "mod" is the mathematical modulo function,
65 which is similar to the FORTRAN MOD function except that
66 S mod T is always in the range [0, T) (T > 0)
70 errCode = tcc.base.AxisErr_OK
72 if not numpy.isfinite(pos):
73 return (pos, tcc.base.AxisErr_CannotCompute)
76 if (
not doWrap)
or (wrapPref == tcc.base.WrapType_None):
83 outPos = coordConv.wrapNear(pos, nearPos)
86 errCode = tcc.base.AxisErr_MinPos
88 errCode = tcc.base.AxisErr_MaxPos
93 meanPos = (minPos + maxPos) / 2.0
94 if wrapPref == tcc.base.WrapType_Nearest:
95 wrapMin = nearPos - 180.0
96 elif wrapPref == tcc.base.WrapType_Negative:
97 wrapMin = meanPos - 360.0
98 elif wrapPref == tcc.base.WrapType_Middle:
99 wrapMin = meanPos - 180.0
100 elif wrapPref == tcc.base.WrapType_Positive:
103 raise RuntimeError(
"Unknown wrap preference %s" % (wrapPref,))
105 if not numpy.isfinite(wrapMin):
106 return (coordConv.DoubleNaN, tcc.base.AxisErr_CannotCompute)
114 wrapMin = min(wrapMin, maxPos - 360.0)
115 wrapMin = max(wrapMin, minPos)
118 diffMod360 = (pos - wrapMin) % 360.0
120 diffMod360 = diffMod360 + 360.0
122 outPos = diffMod360 + wrapMin
130 errCode = tcc.base.AxisErr_MinPos
132 errCode = tcc.base.AxisErr_MaxPos
134 if errCode != tcc.base.AxisErr_OK:
135 outPos = float(
"nan")
137 return (outPos, errCode)