Make use of a single ServoTrigger for multiple Servo PIOs
rodzic
ff011f90e4
commit
54002ee6ff
|
@ -6,7 +6,8 @@ from time import sleep
|
|||
|
||||
@asm_pio()
|
||||
def servo_trigger():
|
||||
irq(clear, rel(1)) # Clear next relative ISR, allows servo code to run again
|
||||
irq(clear, 4) # Clear next relative ISR, allows servo code to run again
|
||||
irq(4)
|
||||
mov(y, x) # Counter is stored in x, copy to y for use
|
||||
label("base")
|
||||
jmp(y_dec, "base") # wait for programmed time
|
||||
|
@ -15,7 +16,7 @@ def servo_trigger():
|
|||
def servo_prog():
|
||||
wrap_target()
|
||||
|
||||
irq(block, rel(0)) .side(0) # Wait here for IRQ to be released by trigger SM
|
||||
wait(0, "irq", 4) .side(0) # Wait here for IRQ to be released by trigger SM
|
||||
|
||||
pull(noblock) # pull new pulse length into fifo (pull fifo into OSR, if empty fifo copies X->OSR)
|
||||
mov(x, osr) # Keep most recent pull data stashed in X, for recycling by noblock (later)
|
||||
|
@ -41,7 +42,7 @@ class ServoTrigger:
|
|||
|
||||
trig_frq = 10_000 #Hz
|
||||
sm_trig = StateMachine(sm_idx, servo_trigger, freq=trig_frq)
|
||||
trig_ctr = (trig_frq // 1000 * trig_target) - 3 # 3 instructions to have perfect 20ms on IRQ
|
||||
trig_ctr = (trig_frq // 1000 * trig_target) - 4 # 3 instructions to have perfect 20ms on IRQ
|
||||
|
||||
sm_trig.put(trig_ctr)
|
||||
sm_trig.exec("pull()")
|
||||
|
@ -57,11 +58,11 @@ class Servo:
|
|||
Preload the ISR with the base duration (fixed pulse length for position 0°)
|
||||
Send position data via FIFO into the OSR (variable pulse length for 0°..max)
|
||||
'''
|
||||
def __init__(self, sm_idx, pin):
|
||||
def __init__(self, sm_idx, pin, min_pulse, max_pulse):
|
||||
self.baseFrq = 1_000_000 # 1MHz = 1us clock base
|
||||
|
||||
self.base_pulse = 1000 # us, base width of pulse
|
||||
self.free_pulse = 1000 # us, max. additional length set by percent
|
||||
self.base_pulse = min_pulse # us, base width of pulse
|
||||
self.free_pulse = max_pulse - min_pulse # us, max. additional length set by percent
|
||||
|
||||
self.sm = StateMachine(sm_idx, servo_prog, freq=self.baseFrq, sideset_base=Pin(pin))
|
||||
|
||||
|
@ -76,13 +77,15 @@ class Servo:
|
|||
self.sm.put(int(self.free_pulse*n))
|
||||
|
||||
# Trigger needs to be the sm before the servo, so the IRQs set by rel(n) match
|
||||
trig = Servo_Trigger(0)
|
||||
s = Servo(1, 16) # phys IO on pin 16
|
||||
|
||||
trig2 = Servo_Trigger(2)
|
||||
s2 = Servo(3, 25) # Builtin LED
|
||||
trig = ServoTrigger(0)
|
||||
s = Servo(1, 16, 1000, 2000) # phys IO on pin 16, with 1ms..2ms pulse width
|
||||
s2 = Servo(3, 17, 1000, 2000) # phys IO on pin 16, with 1ms..2ms pulse width
|
||||
|
||||
while True:
|
||||
for p in range(10+1):
|
||||
s.pos(p/10)
|
||||
sleep(0.5)
|
||||
p = p/10 # Scale 0..10 to 0..1
|
||||
|
||||
s.pos(p)
|
||||
s2.pos(1-p)
|
||||
|
||||
sleep(0.5)
|
||||
|
|
Ładowanie…
Reference in New Issue