added utils from different project

2024-12-15 22:50:41 +01:00
parent 6db9bd85a5
commit eb0eb436ed
15 changed files with 2636 additions and 0 deletions
@@ -0,0 +1,54 @@
+import math
+import matplotlib.pyplot as plt
+
+from utils.defines import *
+from utils.generator import *
+from matplotlib.ticker import (MultipleLocator, AutoMinorLocator)
+
+def main():
+    st = Stepper(1.8, 32)
+    print(f"Step = {st.Step}; Steps = {st.Steps}")
+
+    wv = QuadWave(64)
+    # plt.plot(wv.Sine, label="sin")
+    # plt.plot(wv.Cosine, label="cos")
+    # plt.legend()
+    # plt.show()
+
+    gen = Generator(st, 0.001, 1, 1)
+
+    curves = []
+    curves.append(gen.createSmoothStep(math.pi)) # 180 deg 
+    curves.append(gen.createSmoothStep(math.pi*1/6)) # 60 deg 
+    curves.append(gen.createBuildUpStep(math.pi)) # 180 deg 
+    curves.append(gen.createRapidStartSwingInEnd(math.pi)) # 180 deg 
+    curves.append(gen.createJumpInTarget(math.pi)) # 180 deg 
+
+    fig, axes = plt.subplots(len(curves), 1, layout='constrained')
+
+    for (i,ax) in enumerate(axes):
+        crv = curves[i]
+        ax.plot(crv["p"], label="ϕ")
+        ax.plot(crv["w"], label="ω", linestyle="--")
+
+        dax = ax.twinx()  # instantiate a second Axes that shares the same x-axis
+        dax.step(range(len(crv["o"])), crv["o"], label="o", linewidth=.5)
+        print(f"EndPos = {crv["pE"]}; (Delta = {math.pi - crv["pE"]}) in {crv["T"] * 1000} steps")
+
+    tMax = max(map(lambda x : x["T"], curves))
+    tEnd = math.ceil(tMax)*1000
+
+    for ax in axes:
+        ax.yaxis.set_ticks([0, math.pi/3, math.pi*2/3, math.pi],["Picto0", "Picto1", "Picto2", "Picto3"])
+        ax.xaxis.set_ticks(range(0,tEnd+1000,1000), range(0,math.ceil(tMax)+1,1))
+        ax.xaxis.set_ticks(range(0,tEnd+1000,100), minor = True)
+        ax.yaxis.grid(linestyle='dotted')
+        ax.set_xlabel('time (s)')
+        ax. set_ylabel('angle')
+        # ax.legend()
+
+    plt.show()
+
+    
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,33 @@
+if __name__ == "__main__": # If the Helper.py file is run directly
+    print("This is a modul and should be imported")
+
+import math
+
+class Stepper:
+    # step as degrees
+    Step : float
+    # step as radiant
+    StepRad : float
+    # steps for full resolution
+    Steps : int
+    # microsteps per step
+    Resolution : int
+
+    def __init__(self, step, res):
+        self.Step = step
+        self.Steps = 360 / step
+        self.StepRad = math.pi * step/180
+        self.Resolution = res
+
+class QuadWave:
+    Sine : []
+    Cosine : []
+    def __init__(self, resolution):
+        sine = []
+        cosine = []
+        s = 2 * math.pi / resolution
+        for i in range(resolution):
+            sine.append(math.sin(s * i))   
+            cosine.append(math.cos(s * i))   
+        self.Sine = sine
+        self.Cosine = cosine    
@@ -0,0 +1,180 @@
+if __name__ == "__main__": # If the Helper.py file is run directly
+    print("This is a modul and should be imported")
+
+import math
+
+class Generator:
+    dT = 0 
+    aMax = 0
+    wMax = 0 # v = a * t
+    # s = a * t^2 / 2
+    # s = v * t
+
+    def __init__(self, stpr, dt, a, v):
+        self.stepper = stpr
+        self.dT = dt
+        self.aMax = a
+        self.wMax = v
+
+    def __initVars(self):
+        self.__pArr = [0]
+        self.__wArr = [0]
+        self.__tArr = [0]
+        self.__t = 0
+
+    def __appendPoint(self, p, w, t):
+        self.__wArr.append(w)
+        self.__pArr.append(p)
+        self.__tArr.append(t)
+
+    def __accelUntil(self, a, wInit, check):
+        t = 0
+        pX = self.__pArr[-1]
+        while True:
+            w = wInit + a * t
+            p = pX + wInit * t + a * t**2 / 2
+            if check(w, p):
+                break
+            self.__appendPoint(p, w, self.__t + t)
+            t = t + self.dT
+        self.__t = self.__t + t
+
+    def __getStopDistance(self, a, w):
+        tDec = w / a
+        pDec = a * tDec**2 / 2
+        return (pDec, tDec)
+
+    def __moveUntil(self, wM, pEnd): 
+        t = 0
+        pX = self.__pArr[-1]
+        while True:
+            p = pX + wM * t
+            if p > pEnd:
+                break
+            self.__appendPoint(p, wM, self.__t + t)
+            t = t + self.dT
+        self.__t = self.__t + t
+
+    def __buildUpSwing(self, a, vM, swings):
+        t = 0
+        pX = self.__pArr[-1]
+
+        TwMax = vM / a
+        tau = 2*math.pi
+        sTau = swings*tau
+
+        while True:
+            t = t + self.dT
+            env = a * t
+            p = math.sin(sTau*t/TwMax) * env * TwMax/sTau
+            w = (p-self.__pArr[-1])/self.dT
+            if w > self.wMax:
+                break 
+            self.__appendPoint(p, w, self.__t + t)
+
+        self.__t = self.__t + t
+
+    def __transformToStepper(self) :
+        stp = self.stepper.StepRad
+        res = stp / self.stepper.Resolution
+        oArr = map(lambda x : int((x % stp) / res), self.__pArr)
+        oArr = list(oArr)
+        return {"p": self.__pArr, "w": self.__wArr, "t": self.__tArr, "pE": self.__pArr[-1], "T": self.__t, "o": oArr}
+
+    def __smoothSubStep(self, delta, aM, wM):
+        (pDec,_) = self.__getStopDistance(aM, wM)
+
+        if (pDec < (delta - self.__pArr[-1])/2):
+            # t0..t1: acceleration to max velocity
+            self.__accelUntil(aM, 0, lambda w,p : w > wM)
+
+            # t1..t2: movement to until almost destination
+            self.__moveUntil(wM, delta - pDec)
+
+            # t2..r3: deceleration until destination
+            self.__accelUntil(-aM, wM, lambda w,p : w < 0)
+        else:
+            # t0..t1: acceleration until center is reached
+            pCen = ((delta - self.__pArr[-1]) /2 + self.__pArr[-1]) 
+            self.__accelUntil(aM, 0, lambda w,p : p > pCen)
+
+            # t2..r3: deceleration until destination
+            self.__accelUntil(-aM, self.__wArr[-1], lambda w,p : w < 0)
+
+    def createSmoothStep(self, delta):
+        self.__initVars()
+
+        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)
+
+        if (pDec < delta/2):
+            # t0..t1: acceleration to max velocity
+            self.__accelUntil(self.aMax, 0, lambda w,p : w > self.wMax)
+
+            # t1..t2: movement to until almost destination
+            self.__moveUntil(self.wMax, delta - pDec)
+
+            # t2..r3: deceleration until destination
+            self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)
+        else:
+            # t0..t1: acceleration until center is reached
+            pCen = (self.__pArr[-1]) + delta /2
+            self.__accelUntil(self.aMax, 0, lambda w,p : p > pCen)
+
+            # t2..r3: deceleration until destination
+            self.__accelUntil(-self.aMax, self.__wArr[-1] , lambda w,p : w < 0)
+
+        return self.__transformToStepper()    
+    
+    def createBuildUpStep(self, delta): 
+        self.__initVars()
+
+        # t0..t1: increasing swing
+        self.__buildUpSwing(self.aMax/4, self.wMax, 3)
+        
+        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)
+
+        # t1..t2: movement to until almost destination
+        self.__moveUntil(self.wMax, delta - pDec)
+        
+        # t2..r3: deceleration until destination
+        self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)
+        
+        return self.__transformToStepper()
+
+    def createRapidStartSwingInEnd(self, delta):
+        self.__initVars()
+
+        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)
+
+        # t0..t1: acceleration to max velocity
+        self.__accelUntil(self.aMax*2, 0, lambda w,p : w > self.wMax*2)
+        
+        # t1..r2: deceleration until moving speed
+        self.__accelUntil(-self.aMax, self.wMax*2, lambda w,p : w < self.wMax )
+
+        # t2..t3: movement to until destination (override)
+        self.__moveUntil(self.wMax, delta)
+
+        # t3..t4: deceleration
+        self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)
+
+        pCen = delta + (self.__pArr[-1] - delta) /2
+        self.__accelUntil(-self.aMax, 0, lambda w,p : p < pCen )
+        self.__accelUntil(self.aMax, self.__wArr[-1], lambda w,p : w > 0)
+
+        return self.__transformToStepper()
+
+    def createJumpInTarget(self, delta):
+        self.__initVars()
+
+        jumpInRange = math.pi/3
+        deltaPre = delta - jumpInRange
+        
+        self.__smoothSubStep(deltaPre, self.aMax, self.wMax)
+        
+        jumpInSteps = [jumpInRange/2, jumpInRange/3, jumpInRange/6]
+        nextPoint = deltaPre
+        for step in jumpInSteps:
+            nextPoint = nextPoint + step
+            self.__smoothSubStep(nextPoint, self.aMax*10, self.wMax*2)
+        return self.__transformToStepper()