implemented motorcontrol, untested

micropizza/main.py

@@ -38,6 +38,47 @@ CMD_RW = 'R'        # rewind
 LED = PWM(Pin(25))
 LED.freq(_PWM_FREQ)
 
+class Scroll:
+    def __init__(self, cnt_pin:Pin, end_pin:Pin, en_pin:PWM, a_pin:Pin, b_pin:Pin) -> None:
+        self.c = 0
+        self._dir = 0
+        self._speed = 0
+        self.en_pin = en_pin
+        self.a_pin = a_pin
+        self.b_pin = b_pin
+        self.c_pin = cnt_pin
+        self.c_pin.irq(self._count, Pin.IRQ_RISING | Pin.IRQ_FALLING)
+        self.e_pin = end_pin
+        self.e_pin.irq(self._stop, Pin.IRQ_RISING)        
+    
+    def _count(self, pin):
+        self.c += self._dir
+
+    def _stop(self, pin):
+        self.a_pin.value(0)
+        self.b_pin.value(0)
+
+    @property
+    def dir(self):
+        return self._dir
+    
+    @dir.setter
+    def dir(self, d):
+        self.a_pin.value(1 if d > 0 else 0)
+        self.b_pin.value(1 if d < 0 else 0)
+        self._dir = d
+
+    @property
+    def speed(self):
+        return self._speed * self._dir
+
+    @speed.setter
+    def speed(self, speed):
+        self._speed = speed
+        self.en_pin.duty_u16(speed * (_PWM_MAX // 100))
+        self.dir = (1 if speed > 0 else
+                    (-1 if speed < 0 else 0))
+
 pwm_bl = PWM(Pin(_PIN_EN_BACKLIGHT))
 pwm_bl.freq(_PWM_FREQ)
 pwm_fl = PWM(Pin(_PIN_EN_FRONTLIGHT))
@@ -65,14 +106,29 @@ end_h = Pin(_PIN_SENS_END_HOR, Pin.IN)
 
 uart0 = UART(0, baudrate=9600, tx=Pin(0), rx=Pin(1))
 
+scroll_h = Scroll(cnt_h, end_h, pwm_mh, pin_mh_a, pin_mh_b)
+scroll_v = Scroll(cnt_v, end_v, pwm_mv, pin_mv_a, pin_mv_b)
+
 def motor_vert(*opts):
-    print(f'motor_vert({opts})')
+    _motorctrl(scroll_v, *opts)
     return 'OK'
 
 def motor_hor(*opts):
-    print(f'motor_hor({opts})')
+    _motorctrl(scroll_h, *opts)
     return 'OK'
 
+def _motorctrl(scroll: Scroll, steps: int, *opts):
+    end = scroll.c + steps
+    while True:
+        err = end - scroll.c
+        if abs(err) > 4:
+            speed = (err / (2 * steps)) + 0.5
+            scroll.speed = int(100 * speed)
+        else:
+            scroll.speed = 0
+            break
+        time.sleep_ms(5)   
+
 def backlight(*opts):
     _fade_led(pwm_bl, *opts)
     return 'OK'