offsetpiezo-tracking-simulation/TrackerSim.py at master · csoeller/offsetpiezo-tracking-simulation · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
import matplotlib.pyplot as plt
import numpy as np

import numpy.random as nr
def gausserr(scale):
    if scale <= 0:
        return 0
    else:
        return nr.normal(scale=scale)

class Tracker(object):
    def __init__(self,offsetpiezo,sampleTimeCourse,dzerr=0.0):
        self.piezo = offsetpiezo
        self.stc = sampleTimeCourse
        self.dzerr=dzerr
        self.history = []

    def opInit(self,pos):
        self.piezo.MoveTo(0,pos)
        self.calibpos = pos
        self.lastAdjustment = 0
        self.previoushistory = self.history
        self.history = []

    def compare(self,tick):
        samplez = self.stc.z(tick)
        focus = self.piezo.focus()

        posDelta = self.piezo.GetPos(0) - self.calibpos
        dz = focus - samplez + gausserr(self.dzerr)
        if self.useposDelta:
            if self.addDelta:
                return dz + posDelta
            else:
                return dz - posDelta
        else:
            return dz

    def track(self,tolerance,minDelay=5, useposDelta=False, addDelta=True):
        self.minDelay = minDelay
        self.focusTolerance = tolerance
        self.useposDelta = useposDelta
        self.addDelta = addDelta

        # if we had previous runs the nominal pos can already
        # be contaminated, we should somehow reset the offset
        # and get pos back to where it should be.
        self.opInit(self.stc.z(0))
        for tick in range(self.stc.ntics):
            t = self.stc.t(tick)
            dz = self.compare(tick)
            self.history.append((t, dz, self.piezo.GetOffset(), self.piezo.GetPos(0),
                                 self.stc.z(tick), self.piezo.focus()))
            if abs(dz) > self.focusTolerance and self.lastAdjustment >= self.minDelay:
                zcorr = self.piezo.GetOffset() - dz
                self.piezo.SetOffset(zcorr)
                self.lastAdjustment = 0
            else:
                self.lastAdjustment += 1

    def plothistory(self):
        t, dz, poffset, pos, samplez, focus  = np.array(self.history).T

        plt.subplot(4,1,1)
        plt.plot(t,dz)
        plt.title('dz')

        plt.subplot(4,1,2)
        plt.plot(t,poffset)
        plt.plot(t,pos)
        plt.title('offset & pos')

        plt.subplot(4,1,3)
        plt.plot(t,samplez)
        plt.plot(t,focus)
        plt.title('sample vs tracked focus')

        plt.subplot(4,1,4)
        plt.plot(t,focus-samplez)
        plt.title('tracked focus - sample pos')
        plt.show()

class TrackerCmdTC(Tracker):
    def __init__(self,offsetpiezo,sampleTimeCourse, cmdTimeCourse, dzerr=0.0):
        self.cmdTC = cmdTimeCourse
        super(self.__class__, self).__init__(offsetpiezo,sampleTimeCourse,dzerr)

    def compare(self,tick):
        samplez = self.stc.z(tick)
        focus = self.piezo.focus()

        posDelta = self.cmdTC.z(tick) - self.calibpos
        dz = focus - samplez
        return dz - posDelta + gausserr(self.dzerr)

    def plothistory(self):
        t, dz, poffset, pos, samplez, focus  = np.array(self.history).T

        plt.subplot(4,1,1)
        plt.plot(t,dz)
        plt.title('dz')

        plt.subplot(4,1,2)
        plt.plot(t,poffset)
        plt.plot(t,pos)
        plt.title('offset & pos')

        plt.subplot(4,1,3)
        plt.plot(t,samplez)
        plt.plot(t,focus)
        plt.title('sample vs tracked focus')

        plt.subplot(4,1,4)
        plt.plot(t,focus-samplez)
        plt.plot(self.cmdTC._t,self.cmdTC._z)
        plt.title('tracked focus - sample pos')
        plt.show()