From 059720b0260d68c5c8a6cec5fc6c1e90cc61be43 Mon Sep 17 00:00:00 2001 From: grytole Date: Sat, 27 Sep 2025 14:55:28 +0300 Subject: [PATCH] Add implementation for micropython --- README.md | 23 +++ micropython/motion_generator.py | 206 +++++++++++++++++++++++++++ micropython/test_motion_generator.py | 37 +++++ 3 files changed, 266 insertions(+) create mode 100644 micropython/motion_generator.py create mode 100644 micropython/test_motion_generator.py diff --git a/README.md b/README.md index ce5a558..347f1c5 100644 --- a/README.md +++ b/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) diff --git a/micropython/motion_generator.py b/micropython/motion_generator.py new file mode 100644 index 0000000..de8ac4e --- /dev/null +++ b/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 + diff --git a/micropython/test_motion_generator.py b/micropython/test_motion_generator.py new file mode 100644 index 0000000..87be88f --- /dev/null +++ b/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)))