Add implementation for micropython

README.md

@@ -42,6 +42,29 @@ Note: The reason why I wrote my own library is because the existing ones are NOT
  // Reset internal state
  trapezoidalProfile->reset();
  ``` 
+```python
+from motion_generator import MotionGenerator
+
+# Define the MotionGenerator object
+trapezoidal_profile = MotionGenerator(200, 500, 0)
+
+# Retrieve calculated position
+position_ref = 100
+position = trapezoidal_profile.update(position_ref)
+
+# Retrieve current velocity
+velocity = trapezoidal_profile.velocity()
+
+# Retrieve current acceleration
+acceleration = trapezoidal_profile.acceleration()
+
+# Check if profile is finished
+if trapezoidal_profile.finished():
+    pass
+
+# Reset internal state
+trapezoidal_profile.reset()
+```
 ## Example plot
 ### Trapezoidal motion generator
 ![Simulation of a trapezoidal motion profile](https://github.com/EFeru/MotionGenerator/blob/main/matlab/simPhoto.png)

micropython/motion_generator.py

@@ -0,0 +1,206 @@
+import time
+from math import sqrt
+
+class MotionGenerator:
+    def __init__(self, maxVel, maxAcc, initPos):
+        self.maxVel = maxVel
+        self.maxAcc = maxAcc
+        self.initPos = initPos
+        self.oldTime = time.ticks_us()
+        self.lastTime = self.oldTime
+        self.deltaTime = 0
+        self.reset()
+        self.signOfMotion = 1
+        self.is_trapezoid = True
+        self.is_finished = False
+
+    def update(self, posRef):
+        if self.oldPosRef != posRef:
+            self.is_finished = False
+
+            # shift state variables
+            self.oldPosRef = posRef
+            self.oldPos = self.pos
+            self.oldVel = self.vel
+            self.oldTime = self.lastTime
+
+            # calc breaking time and distance
+            self.tBrk = abs(self.oldVel) / self.maxAcc
+            self.dBrk = self.tBrk * abs(self.oldVel) / 2.0
+
+            # calc sign of motion
+            oldVelSign = self._sign(self.oldVel)
+            oldPosTarget = self.oldPos + oldVelSign * self.dBrk
+            self.signOfMotion = self._sign(posRef - oldPosTarget)
+
+            if self.signOfMotion != oldVelSign:
+                # brake is needed
+                self.tAcc = self.maxVel / self.maxAcc
+                self.dAcc = self.tAcc * self.maxVel / 2.0
+            else:
+                self.tBrk = 0.0
+                self.dBrk = 0.0
+                self.tAcc = (self.maxVel - abs(self.oldVel)) / self.maxAcc
+                self.dAcc = self.tAcc * (self.maxVel + abs(self.oldVel)) / 2.0
+
+            # calc total distance to go after braking
+            self.dTot = abs(posRef - self.oldPos + self.signOfMotion * self.dBrk)
+
+            self.tDec = self.maxVel / self.maxAcc
+            self.dDec = self.tDec * self.maxVel / 2.0
+            self.dVel = self.dTot - (self.dAcc + self.dDec)
+            self.tVel = self.dVel / self.maxVel
+
+            if self.tVel > 0:
+                self.is_trapezoid = True
+            else:
+                # not enough time to reach max velocity
+                self.is_trapezoid = False
+
+                # recalc distances and periods
+                if self.signOfMotion != oldVelSign:
+                    # brake is needed
+                    self.velSt = sqrt(self.maxAcc * self.dTot)
+                    self.tAcc = self.velSt / self.maxAcc
+                    self.dAcc = self.tAcc * self.velSt / 2.0
+                else:
+                    self.tBrk = 0.0
+                    self.dBrk = 0.0
+                    self.dTot = abs(posRef - self.oldPos)
+                    self.velSt = sqrt(0.5 * (self.oldVel**2) + self.maxAcc * self.dTot)
+                    self.tAcc = (self.velSt - abs(self.oldVel)) / self.maxAcc
+                    self.dAcc = self.tAcc * (self.velSt + abs(self.oldVel)) / 2.0
+
+                self.tDec = self.velSt / self.maxAcc
+                self.dDec = self.tDec * self.velSt / 2.0
+
+        currTime = time.ticks_us()
+        self.deltaTime = time.ticks_diff(currTime, self.oldTime)
+
+        # calc new setpoint
+        if self.is_trapezoid == True:
+            self._calculate_trapezoidal_profile(posRef)
+        else:
+            self._calculate_triangular_profile(posRef)
+
+        self.lastTime = currTime
+        return self.pos
+
+    def velocity(self):
+        return self.vel
+
+    def acceleration(self):
+        return self.acc
+
+    def maxVelocity(self, maxVel=None):
+        if maxVel == None:
+            return self.maxVel
+        else:
+            self.maxVel = maxVel
+
+    def maxAcceleration(self, maxAcc=None):
+        if maxAcc == None:
+            return self.maxAcc
+        else:
+            self.maxAcc = maxAcc
+
+    def initPosition(self, initPos=None):
+        if initPos == None:
+            return self.initPos
+        else:
+            self.initPos = initPos
+            self.pos = initPos
+            self.oldPos = initPos
+
+    def finished(self):
+        return self.is_finished
+
+    def reset(self):
+        self.pos = self.initPos
+        self.oldPos = self.initPos
+        self.oldPosRef = 0.0
+        self.vel = 0.0
+        self.acc = 0.0
+        self.oldVel = 0.0
+        self.dBrk = 0.0
+        self.dAcc = 0.0
+        self.dVel = 0.0
+        self.dDec = 0.0
+        self.dTot = 0.0
+        self.tBrk = 0.0
+        self.tAcc = 0.0
+        self.tVel = 0.0
+        self.tDec = 0.0
+        self.velSt = 0.0
+
+    def _sign(self, val):
+        if val < 0.0:
+            return -1
+        elif val > 0.0:
+            return 1
+        else:
+            return 0
+
+    def _calculate_trapezoidal_profile(self, posRef):
+        t = self.deltaTime / 1_000_000
+        signM = self.signOfMotion
+        accelPeriodEnd = self.tBrk + self.tAcc
+        coastPeriodEnd = accelPeriodEnd + self.tVel
+        decelPeriodEnd = coastPeriodEnd + self.tDec
+
+        if t <= accelPeriodEnd:
+            # acceleration phase
+            self.acc = signM * self.maxAcc
+            currVel = self.acc * t
+            self.vel = self.oldVel + currVel
+            currPos = 0.5 * currVel * t
+            self.pos = self.oldPos + self.oldVel * t + currPos
+        elif t < coastPeriodEnd:
+            # coasting phase
+            self.acc = 0.0
+            self.vel = signM * self.maxVel
+            dCoastLeft = self.maxVel * (t - self.tBrk - self.tAcc)
+            self.pos = self.oldPos + signM * (-self.dBrk + self.dAcc + dCoastLeft)
+        elif t < decelPeriodEnd:
+            # deceleration phase
+            self.acc = -(signM * self.maxAcc)
+            tDecLeft = t - self.tBrk - self.tAcc - self.tVel
+            currVel = self.maxAcc * tDecLeft
+            self.vel = signM * (self.maxVel - currVel)
+            dDecLeft = self.maxVel * tDecLeft - 0.5 * currVel * tDecLeft
+            self.pos = self.oldPos + signM * (-self.dBrk + self.dAcc + self.dVel + dDecLeft)
+        else:
+            # target reached
+            self.pos = posRef
+            self.vel = 0.0
+            self.acc = 0.0
+            self.is_finished = True
+
+    def _calculate_triangular_profile(self, posRef):
+        t = self.deltaTime / 1_000_000
+        signM = self.signOfMotion
+        accelPeriodEnd = self.tBrk + self.tAcc
+        decelPeriodEnd = accelPeriodEnd + self.tDec
+
+        if t <= accelPeriodEnd:
+            # acceleration phase
+            self.acc = signM * self.maxAcc
+            currVel = self.acc * t
+            self.vel = self.oldVel + currVel
+            currPos = 0.5 * currVel * t
+            self.pos = self.oldPos + self.oldVel * t + currPos
+        elif t < decelPeriodEnd:
+            # deceleration phase
+            self.acc = -(signM * self.maxAcc)
+            tDecLeft = t - self.tBrk - self.tAcc
+            currVel = self.maxAcc * tDecLeft
+            self.vel = signM * (self.velSt - currVel)
+            dDecLeft = self.velSt * tDecLeft - 0.5 * currVel * tDecLeft
+            self.pos = self.oldPos + signM * (-self.dBrk + self.dAcc + dDecLeft)
+        else:
+            # target reached
+            self.pos = posRef
+            self.vel = 0.0
+            self.acc = 0.0
+            self.is_finished = True
+

micropython/test_motion_generator.py

@@ -0,0 +1,37 @@
+import time
+from motion_generator import MotionGenerator
+
+MAX_VEL = 200
+MAX_ACC = 500
+INIT_POS = 0
+
+NUM_TICKS = 100
+NUM_STEPS = 5
+TEST_PERIOD_S = 5
+test_refs = [0, -25, 150, 100, 165]
+
+mg = MotionGenerator(MAX_VEL, MAX_ACC, INIT_POS)
+total_time_us = 0
+ref = 0.0
+
+for t in range(NUM_TICKS):
+    ts = time.ticks_us()
+    if t % (NUM_TICKS // NUM_STEPS) == 0 and len(test_refs) > 0:
+        ref = test_refs.pop(0)
+
+    pos = mg.update(ref)
+    vel = mg.velocity()
+    acc = mg.acceleration()
+
+    total_time_us += time.ticks_diff(time.ticks_us(), ts)
+    print("ref:{} pos:{} vel:{} acc:{}".format(ref, pos, vel, acc))
+    time.sleep(TEST_PERIOD_S / NUM_TICKS)
+
+total_time_s = total_time_us / 1_000_000
+tick_time_s = total_time_s / NUM_TICKS
+update_freq_hz = 1 / tick_time_s
+
+print("Done:")
+print(" {} iterations took {} s".format(NUM_TICKS, total_time_s))
+print(" {} s per iteration".format(tick_time_s))
+print(" {} Hz".format(round(update_freq_hz, 1)))