Skip to content

Commit 2c08d7e

Browse files
IhorNehrutsaIhorNehrutsa
committed
Add Stepper Motor PWM-Counter driver.
1 parent 01ab7ba commit 2c08d7e

File tree

5 files changed

+469
-0
lines changed

5 files changed

+469
-0
lines changed

‎.gitignore

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,3 +4,5 @@ __pycache__
44
*/dist/
55
*.org
66
*.1
7+
*.mp4
8+
*.png

‎micropython/drivers/motion/stepper_motor/stepper_motor_pwm_counter/README.md

Lines changed: 229 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,229 @@
1+
## Stepper motor PWM-Counter driver
2+
3+
This MicroPython software driver is designed to control stepper motor using STEP/DIR hardware driver.
4+
5+
![stepper_motor_driver](https://github.com/IhorNehrutsa/micropython-lib/assets/70886343/27933a08-7225-4931-a1ee-8e0042d0b822)
6+
7+
* The driver signal "STEP" (aka "PULSE") is intended for clock pulses. In one pulse, the motor rotor turns one step. The higher the frequency of pulses, the higher the speed of rotation of the rotor.
8+
9+
* The driver signal "DIR" is intended to select the direction of rotation of the engine ("1" - in one direction, "0" - in the other direction).
10+
11+
![forward_reverse](https://github.com/IhorNehrutsa/micropython-lib/assets/70886343/f1986469-6fca-4d10-a6f2-262020a19946)
12+
13+
### Hardware
14+
15+
As an example of a STEP / DIR hardware driver:
16+
17+
* [TMC2209](https://wiki.fysetc.com/Silent2209) module, TB6612,
18+
19+
* [TB6560-V2](https://mypractic.com/stepper-motor-driver-tb6560-v2-description-characteristics-recommendations-for-use) module,
20+
21+
* [TB6600](https://mytectutor.com/tb6600-stepper-motor-driver-with-arduino) based driver,
22+
23+
* DM860H, DM556 etc.
24+
25+
### Software
26+
27+
The main feature of this driver is that the generation and counting of pulses are performed by hardware, which frees up time in the main loop. PWM will start pulses and Counter will stop pulses in irq handler.
28+
29+
The PWM unit creates STEP pulses and sets the motor speed.
30+
31+
The GPIO unit controls the DIR pin, the direction of rotation of the motor.
32+
33+
The Counter unit counts pulses, that is, the actual position of the stepper motor.
34+
35+
![stepper_motor_pwm_counter](https://github.com/IhorNehrutsa/micropython-lib/assets/70886343/4e6cf4b9-b198-4fa6-8bcc-51d873bf74ce)
36+
37+
In general case MicroPython ports need 4 pins: PWM STEP output, GPIO DIR output, Counter STEP input, Counter DIR input (red wires in the image).
38+
39+
The ESP32 port allows to connect Counter inputs to the same outputs inside the MCU(green wires in the picture), so 2 pins are needed.
40+
41+
This driver requires PR's:
42+
43+
[esp32/PWM: Reduce inconsitencies between ports. #10854](https://github.com/micropython/micropython/pull/10854)
44+
45+
[ESP32: Add Quadrature Encoder and Pulse Counter classes. #8766](https://github.com/micropython/micropython/pull/8766)
46+
47+
Constructor
48+
-----------
49+
50+
class:: StepperMotorPwmCounter(pin_step, pin_dir, freq, reverse)
51+
52+
Construct and return a new StepperMotorPwmCounter object using the following parameters:
53+
54+
- *pin_step* is the entity on which the PWM is output, which is usually a
55+
:ref:`machine.Pin <machine.Pin>` object, but a port may allow other values, like integers.
56+
- *freq* should be an integer which sets the frequency in Hz for the
57+
PWM cycle i.e. motor speed.
58+
- *reverse* reverse the motor direction if the value is True
59+
60+
Properties
61+
----------
62+
63+
property:: StepperMotorPwmCounter.freq
64+
65+
Get/set the current frequency of the STEP/PWM output.
66+
67+
property:: StepperMotorPwmCounter.steps_counter
68+
69+
Get current steps position form the Counter.
70+
71+
property:: StepperMotorPwmCounter.steps_target
72+
73+
Get/set the steps target.
74+
75+
Methods
76+
-------
77+
78+
method:: StepperMotorPwmCounter.deinit()
79+
80+
Disable the PWM output.
81+
82+
method:: StepperMotorPwmCounter.go()
83+
84+
Call it in the main loop to move the motor to the steps_target position.
85+
86+
method:: StepperMotorPwmCounter.is_ready()
87+
88+
Return True if steps_target is achieved.
89+
90+
Tested on ESP32.
91+
92+
**Simple example is:**
93+
```
94+
# stepper_motor_pwm_counter_test1.py
95+
96+
from time import sleep
97+
98+
from stepper_motor_pwm_counter import StepperMotorPwmCounter
99+
100+
try:
101+
motor = StepperMotorPwmCounter(26, 23, freq=10_000)
102+
print(motor)
103+
104+
motor.steps_target = 8192
105+
while True:
106+
if not motor.is_ready():
107+
motor.go()
108+
print(f'motor.steps_target={motor.steps_target}, motor.steps_counter={motor.steps_counter}, motor.is_ready()={motor.is_ready()}')
109+
else:
110+
print()
111+
print(f'motor.steps_target={motor.steps_target}, motor.steps_counter={motor.steps_counter}, motor.is_ready()={motor.is_ready()}')
112+
print('SET steps_target', -motor.steps_target)
113+
print('sleep(1)')
114+
print()
115+
sleep(1)
116+
motor.steps_target = -motor.steps_target
117+
motor.go()
118+
119+
sleep(0.1)
120+
121+
except Exception as e:
122+
print(e)
123+
raise e
124+
finally:
125+
try:
126+
motor.deinit()
127+
except:
128+
pass
129+
```
130+
131+
**Output is:**
132+
```
133+
StepMotorPWMCounter(pin_step=Pin(26), pin_dir=Pin(23), freq=10000, reverse=0,
134+
pwm=PWM(Pin(26), freq=10000, duty_u16=0),
135+
counter=Counter(0, src=Pin(26), direction=Pin(23), edge=Counter.RISING, filter_ns=0))
136+
motor.steps_target=8192, motor.steps_counter=2, motor.is_ready()=False
137+
motor.steps_target=8192, motor.steps_counter=1025, motor.is_ready()=False
138+
motor.steps_target=8192, motor.steps_counter=2048, motor.is_ready()=False
139+
motor.steps_target=8192, motor.steps_counter=3071, motor.is_ready()=False
140+
motor.steps_target=8192, motor.steps_counter=4094, motor.is_ready()=False
141+
motor.steps_target=8192, motor.steps_counter=5117, motor.is_ready()=False
142+
motor.steps_target=8192, motor.steps_counter=6139, motor.is_ready()=False
143+
motor.steps_target=8192, motor.steps_counter=7162, motor.is_ready()=False
144+
motor.steps_target=8192, motor.steps_counter=8185, motor.is_ready()=False
145+
irq_handler: steps_over_run=6, counter.get_value()=8204
146+
147+
motor.steps_target=8192, motor.steps_counter=8204, motor.is_ready()=True
148+
SET steps_target -8192
149+
sleep(1)
150+
151+
motor.steps_target=-8192, motor.steps_counter=7200, motor.is_ready()=False
152+
motor.steps_target=-8192, motor.steps_counter=6176, motor.is_ready()=False
153+
motor.steps_target=-8192, motor.steps_counter=5153, motor.is_ready()=False
154+
motor.steps_target=-8192, motor.steps_counter=4130, motor.is_ready()=False
155+
motor.steps_target=-8192, motor.steps_counter=3107, motor.is_ready()=False
156+
motor.steps_target=-8192, motor.steps_counter=2084, motor.is_ready()=False
157+
motor.steps_target=-8192, motor.steps_counter=1061, motor.is_ready()=False
158+
motor.steps_target=-8192, motor.steps_counter=37, motor.is_ready()=False
159+
motor.steps_target=-8192, motor.steps_counter=-986, motor.is_ready()=False
160+
motor.steps_target=-8192, motor.steps_counter=-2009, motor.is_ready()=False
161+
motor.steps_target=-8192, motor.steps_counter=-3032, motor.is_ready()=False
162+
motor.steps_target=-8192, motor.steps_counter=-4054, motor.is_ready()=False
163+
motor.steps_target=-8192, motor.steps_counter=-5077, motor.is_ready()=False
164+
motor.steps_target=-8192, motor.steps_counter=-6100, motor.is_ready()=False
165+
motor.steps_target=-8192, motor.steps_counter=-7123, motor.is_ready()=False
166+
motor.steps_target=-8192, motor.steps_counter=-8146, motor.is_ready()=False
167+
irq_handler: steps_over_run=4, counter.get_value()=-8188
168+
motor.steps_target=-8192, motor.steps_counter=-8189, motor.is_ready()=False
169+
170+
motor.steps_target=-8192, motor.steps_counter=-9209, motor.is_ready()=True
171+
SET steps_target 8192
172+
sleep(1)
173+
174+
motor.steps_target=8192, motor.steps_counter=-8205, motor.is_ready()=False
175+
Traceback (most recent call last):
176+
File "<stdin>", line 25, in <module>
177+
KeyboardInterrupt:
178+
```
179+
180+
**Example with motor speed acceleration/deceleration:**
181+
```
182+
# stepper_motor_pwm_counter_test2.py
183+
184+
from time import sleep
185+
186+
from stepper_motor_pwm_counter import StepperMotorPwmCounter
187+
188+
189+
try:
190+
motor = StepperMotorPwmCounter(26, 23)
191+
print(motor)
192+
193+
f_min = 3_000
194+
f_max = 50_000
195+
df = 1_000
196+
motor.freq = f_min
197+
motor_steps_start = motor.steps_counter
198+
motor.steps_target = 8192 * 10
199+
while True:
200+
if not motor.is_ready():
201+
motor.go()
202+
else:
203+
print()
204+
print(f'motor.steps_target={motor.steps_target}, motor.steps_counter={motor.steps_counter}, motor.is_ready()={motor.is_ready()}')
205+
print('SET steps_target', -motor.steps_target)
206+
print('sleep(1)')
207+
print()
208+
sleep(1)
209+
motor_steps_start = motor.steps_target
210+
motor.steps_target = -motor.steps_target
211+
motor.go()
212+
213+
214+
m = min(abs(motor.steps_counter - motor_steps_start), abs(motor.steps_target - motor.steps_counter))
215+
motor.freq = min(f_min + df * m // 1000, f_max)
216+
217+
sleep(0.1)
218+
219+
except Exception as e:
220+
print(e)
221+
raise e
222+
finally:
223+
try:
224+
motor.deinit()
225+
except:
226+
pass
227+
228+
```
229+
[Motor speed acceleration/deceleration video](https://drive.google.com/file/d/1HOkmqnaepOOmt4XUEJzPtQJNCVQrRUXs/view?usp=drive_link)

‎micropython/drivers/motion/stepper_motor/stepper_motor_pwm_counter/stepper_motor_pwm_counter.py

Lines changed: 157 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,157 @@
1+
from utime import ticks_diff, ticks_us, ticks_ms
2+
from machine import Pin, PWM, Counter
3+
4+
5+
class StepperMotorPwmCounter:
6+
def __init__(self, pin_step, pin_dir, freq=5_000, reverse=0, counter=None, pwm=None):
7+
if isinstance(pin_step, Pin):
8+
self.pin_step = pin_step
9+
else:
10+
self.pin_step = Pin(pin_step, Pin.OUT)
11+
12+
13+
if isinstance(pin_dir, Pin):
14+
self.pin_dir = pin_dir
15+
else:
16+
self.pin_dir = Pin(pin_dir, Pin.OUT, value=0)
17+
18+
self.freq = freq
19+
20+
self.reverse = reverse # reverse the direction of movement of the motor
21+
22+
if isinstance(counter, Counter):
23+
self._counter = counter
24+
else:
25+
self._counter = Counter(-1, src=pin_step, direction=pin_dir)
26+
27+
self._steps_target = 0
28+
self._match = 0
29+
self._steps_over_run = 0
30+
31+
self._direction = 0 # the current direction of movement of the motor (-1 - movement in the negative direction, 0 - motionless, 1 - movement in the positive direction)
32+
33+
# must be after Counter() initialization!
34+
if isinstance(pwm, PWM):
35+
self._pwm = pwm
36+
else:
37+
self._pwm = PWM(pin_step, freq=self._freq, duty_u16=0) # 0%
38+
39+
def __repr__(self):
40+
return f"StepMotorPWMCounter(pin_step={self.pin_step}, pin_dir={self.pin_dir}, freq={self._freq}, reverse={self._reverse}, pwm={self._pwm}, counter={self._counter})"
41+
42+
def deinit(self):
43+
try:
44+
self._pwm.deinit()
45+
except:
46+
pass
47+
try:
48+
self._counter.irq(handler=None)
49+
except:
50+
pass
51+
try:
52+
self._counter.deinit()
53+
except:
54+
pass
55+
56+
# -----------------------------------------------------------------------
57+
@property
58+
def reverse(self):
59+
return self._reverse
60+
61+
@reverse.setter
62+
def reverse(self, reverse:int):
63+
self._reverse = 1 if bool(reverse) else 0
64+
65+
# -----------------------------------------------------------------------
66+
@property
67+
def freq(self):
68+
return self._freq
69+
70+
@freq.setter
71+
def freq(self, freq):
72+
self._freq = freq if freq > 0 else 1 # pulse frequency in Hz
73+
74+
# -----------------------------------------------------------------------
75+
@property
76+
def direction(self) -> int:
77+
return self._direction
78+
79+
@direction.setter
80+
def direction(self, delta:int):
81+
if delta > 0:
82+
self._direction = 1
83+
self.pin_dir(1 ^ self._reverse)
84+
elif delta < 0:
85+
self._direction = -1
86+
self.pin_dir(0 ^ self._reverse)
87+
else:
88+
self._direction = 0
89+
#print(f'Set direction:{delta} to {self._direction}')
90+
91+
# -----------------------------------------------------------------------
92+
@property
93+
def steps_counter(self) -> int:
94+
return self._counter.get_value()
95+
96+
# -----------------------------------------------------------------------
97+
@property
98+
def steps_target(self) -> int:
99+
return self._steps_target
100+
101+
@steps_target.setter
102+
def steps_target(self, steps_target):
103+
# Set the target position that will be achieved in the main loop
104+
if self._steps_target != steps_target:
105+
self._steps_target = steps_target
106+
107+
delta = self._steps_target - self._counter.get_value()
108+
if delta > 0:
109+
self._match = self._steps_target - self._steps_over_run# * 2
110+
self._counter.irq(handler=self.irq_handler, trigger=Counter.IRQ_MATCH1, value=self._match)
111+
elif delta < 0:
112+
self._match = self._steps_target + self._steps_over_run# * 2
113+
self._counter.irq(handler=self.irq_handler, trigger=Counter.IRQ_MATCH1, value=self._match)
114+
115+
# -----------------------------------------------------------------------
116+
def irq_handler(self, obj):
117+
self.stop_pulses()
118+
self._steps_over_run = (self._steps_over_run + abs(self._counter.get_value() - self._match)) // 2
119+
print(f" irq_handler: steps_over_run={self._steps_over_run}, counter.get_value()={self._counter.get_value()}")
120+
121+
def start_pulses(self):
122+
self._pwm.freq(self._freq)
123+
self._pwm.duty_u16(32768)
124+
125+
def stop_pulses(self):
126+
self._pwm.duty_u16(0)
127+
128+
def stop(self):
129+
self.stop_pulses()
130+
self._steps_target = self._counter.get_value()
131+
132+
def go(self):
133+
delta = self._steps_target - self._counter.get_value()
134+
if delta > 0:
135+
self.direction = 1
136+
self.start_pulses()
137+
elif delta < 0:
138+
self.direction = -1
139+
self.start_pulses()
140+
else:
141+
self.stop_pulses()
142+
# print(f" go: delta={delta}, steps_target={self._steps_target}, match={self._match}, counter.get_value()={self._counter.get_value()}, direction={self._direction}, steps_over_run={self._steps_over_run}, freq={self._freq}")
143+
144+
def is_ready(self) -> bool:
145+
delta = self._steps_target - self._counter.get_value()
146+
# print(f" is_ready: delta={delta}, counter.get_value()={self._counter.get_value()}, steps_target={self._steps_target}, direction={self._direction}")
147+
if self._direction > 0:
148+
if delta <= 0:
149+
self.stop_pulses()
150+
return True
151+
elif self._direction < 0:
152+
if delta >= 0:
153+
self.stop_pulses()
154+
return True
155+
else:
156+
return delta == 0
157+
return False

0 commit comments

Comments
 (0)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy