add new hal for double H bridge stepper motor driver

cnc/config.py

@@ -2,19 +2,32 @@
 # Hardware config.
 
 # Maximum velocity for each axis in millimeter per minute.
-MAX_VELOCITY_MM_PER_MIN_X = 240
+MAX_VELOCITY_MM_PER_MIN_X = 4001
-MAX_VELOCITY_MM_PER_MIN_Y = 240
+MAX_VELOCITY_MM_PER_MIN_Y = 4001
-MAX_VELOCITY_MM_PER_MIN_Z = 0
+MAX_VELOCITY_MM_PER_MIN_Z = 1
-MAX_VELOCITY_MM_PER_MIN_E = 0
+MAX_VELOCITY_MM_PER_MIN_E = 1
-MIN_VELOCITY_MM_PER_MIN = 1
+MIN_VELOCITY_MM_PER_MIN = 50
 # Average velocity for endstop calibration procedure
 CALIBRATION_VELOCITY_MM_PER_MIN = 30
 
 # Stepper motors steps per millimeter for each axis.
-STEPPER_PULSES_PER_MM_X = 1.65
+ENABLE_L293D = True
-STEPPER_PULSES_PER_MM_Y = 1.65
+if ENABLE_L293D:
-STEPPER_PULSES_PER_MM_Z = 0
+    ENABLE_FULL_STEP = False
-STEPPER_PULSES_PER_MM_E = 0
+    ENABLE_HALF_STEP = True
+    if ENABLE_FULL_STEP:
+        STEPPER_PULSES_PER_MM_X = 6.66
+        STEPPER_PULSES_PER_MM_Y = 6.66
+    elif ENABLE_HALF_STEP:
+        STEPPER_PULSES_PER_MM_X = 13.33
+        STEPPER_PULSES_PER_MM_Y = 13.33
+
+STEPPER_PULSES_PER_MM_Z = 1
+STEPPER_PULSES_PER_MM_E = 1
+
+# Stepper motors steps per rotation for each axis.
+STEPPER_STEPS_PER_REV_X = 20
+STEPPER_STEPS_PER_REV_Y = 20
 
 # Invert axises direction, by default(False) high level means increase of
 # position. For inverted(True) axis, high level means decrease of position.
@@ -53,31 +66,30 @@ BED_PID = {"P": 0.226740848076,
 # Pins configuration.
 
 # Enable pin for all steppers, low level is enabled.
-ENABLE_STEPPER_ENABLE_PIN = False
+ENABLE_STEPPER_ENABLE_PIN = True
-STEPPERS_ENABLE_PIN = 26
+STEPPERS_ENABLE_PIN = 0
-STEPPER_STEP_PIN_X = 21
-STEPPER_STEP_PIN_Y = 16
-STEPPER_STEP_PIN_Z = 12
-STEPPER_STEP_PIN_E = 8
-ENABLE_L293D = True
-if ENABLE_L293D:
-    STEPPER_STEP_PINS_X = [4,17,27,22]
-    ENABLE_FULL_STEP = False
-    ENABLE_HALF_STEP = True
 
-STEPPER_DIR_PIN_X = 20
+STEPPER_STEP_PIN_X = 0
-STEPPER_DIR_PIN_Y = 19
+STEPPER_STEP_PIN_Y = 0
-STEPPER_DIR_PIN_Z = 13
+STEPPER_STEP_PIN_Z = 0
-STEPPER_DIR_PIN_E = 7
+STEPPER_STEP_PIN_E = 0
+if ENABLE_L293D:
+    STEPPER_STEP_PINS_X = [22,27,17,4]
+    STEPPER_STEP_PINS_Y = [12,16,20,21] 
+
+STEPPER_DIR_PIN_X = 0
+STEPPER_DIR_PIN_Y = 0 
+STEPPER_DIR_PIN_Z = 0
+STEPPER_DIR_PIN_E = 0
 
 ENABLE_SPINDLE = False
-SPINDLE_PWM_PIN = 4
+SPINDLE_PWM_PIN = 0
 ENABLE_FAN = False
-FAN_PIN = 27
+FAN_PIN = 0
 ENABLE_EXTRUDER_HEATER = False
-EXTRUDER_HEATER_PIN = 18
+EXTRUDER_HEATER_PIN = 0
 ENABLE_BED_HEATER = False
-BED_HEATER_PIN = 22
+BED_HEATER_PIN = 0
 if ENABLE_EXTRUDER_HEATER:
     EXTRUDER_TEMPERATURE_SENSOR_CHANNEL = 2
 if ENABLE_BED_HEATER:

cnc/hal.py

@@ -98,6 +98,10 @@
 
 # check which module to import
 try:
+    from cnc.config import *
+    if ENABLE_L293D:
+        from cnc.hal_raspberry.hal2 import *
+    else:
         from cnc.hal_raspberry.hal import *
 except ImportError:
     print("----- Hardware not detected, using virtual environment -----")

cnc/hal_raspberry/hal2.py

@@ -0,0 +1,316 @@
+import time
+
+from cnc.hal_raspberry import rpgpio
+from cnc.pulses import *
+from cnc.config import *
+
+US_IN_SECONDS = 1000000
+
+gpio = rpgpio.GPIO()
+dma = rpgpio.DMAGPIO()
+pwm = rpgpio.DMAPWM()
+watchdog = rpgpio.DMAWatchdog()
+
+class stepper:
+    ''' double H bridge stepper motor driver '''
+
+    STEPPER_ENABLED = 0
+    STEPPER_DISABLED = 1
+
+    STATE_DIR_DIRECT = 0
+    STATE_DIR_INV = 1
+    
+    AXE_X = 0
+    AXE_Y = 1
+    AXE_Z = 2
+
+    MODE_FULL = 0
+    MODE_HALF = 1
+
+    HALF_STEP_SEQ = [[1,0,0,0], # Phase A
+                     [1,0,1,0], # Phase AB
+                     [0,0,1,0], # Phase B
+                     [0,1,1,0], # Phase A'B
+                     [0,1,0,0], # Phase A'
+                     [0,1,0,1], # Phase A'B'
+                     [0,0,0,1], # Phase B'
+                     [1,0,0,1]] # Phase AB'
+
+    FULL_STEP_SEQ = [[1,0,0,0], # Phase A
+                     [0,0,1,0], # Phase B
+                     [0,1,0,0], # Phase A'
+                     [0,0,0,1]] # Phase B'
+
+    def __init__(self, axe=AXE_X, pins=[]):
+        ''' contructor '''
+        self.state = self.STEPPER_DISABLED 
+        self.dir = self.STATE_DIR_DIRECT
+        self.axe = axe
+        self.seq = self.FULL_STEP_SEQ
+        self.pins = pins
+        self.step_number = 0
+        self.current_seq_num = 0 
+        self.number_of_steps = 0
+
+    def init(self):
+        ''' init gpios '''
+        self.state = self.STEPPER_ENABLED 
+        for pin in self.pins:
+            logging.debug("set pin {} out".format(pin))
+            gpio.init(pin, rpgpio.GPIO.MODE_OUTPUT)
+
+    def set_steps_number(self, number=0):
+        ''' set the number of steps of the motor '''
+        self.number_of_steps = number
+
+    def set_mode(self, mode):
+        ''' set operationnal mode '''
+        if mode > self.MODE_HALF:
+            raise ValueError
+        elif mode == self.MODE_FULL:
+            self.seq = self.FULL_STEP_SEQ
+            if self.dir == self.STATE_DIR_INV:
+                self.seq.reverse()
+        elif mode == self.MODE_HALF:
+            self.seq = self.HALF_STEP_SEQ
+            if self.dir == self.STATE_DIR_INV:
+                self.seq.reverse()
+
+    def set_dir(self, direction):
+        ''' set direction '''
+        if direction > self.STATE_DIR_INV:
+            raise ValueError
+        elif direction == self.STATE_DIR_INV and self.dir == self.STATE_DIR_DIRECT:
+            self.dir = direction
+            self.seq.reverse()
+        elif direction == self.STATE_DIR_DIRECT and self.dir == self.STATE_DIR_INV:
+            self.dir = direction
+            self.seq.reverse()
+        else:
+            logging.warning("direction already set")
+
+    def inc_seq_num(self):
+        ''' increment current sequence number '''
+        self.current_seq_num += 1
+
+    def dec_seq_num(self):
+        ''' decrement current sequence number '''
+        self.current_seq_num -= 1
+
+    def get_current_seq(self):
+        ''' get current sequence '''
+        i = self.current_seq_num % (len(self.seq))
+        return self.seq[i]
+
+    def disable(self):
+        ''' disable stepper '''
+        self.state = self.STEPPER_DISABLED
+        for pin in self.pins:
+            gpio.clear(pin)
+
+    def enable(self):
+        ''' enable stepper '''
+        self.state = self.STEPPER_ENABLED
+
+    def set_speed(self, speed):
+        ''' sets the speed in rev per minute '''
+        logging.debug("speed = {}".format(60*1000*1000 / self.number_of_steps / speed))
+
+stepper_x = stepper(axe=stepper.AXE_X, pins=STEPPER_STEP_PINS_X)
+stepper_y = stepper(axe=stepper.AXE_Y, pins=STEPPER_STEP_PINS_Y)
+
+def init():
+    """ Initialize GPIO pins and machine itself.
+    """
+    logging.info("initialisation of gpios ...")
+    # Init X stepper
+    stepper_x.init()
+    stepper_x.set_mode(mode = stepper.MODE_HALF)
+    stepper_x.set_steps_number(STEPPER_STEPS_PER_REV_X)
+    stepper_x.enable()
+    # Init Y stepper
+#    stepper_y.init()
+#    stepper_y.set_mode(mode = stepper.MODE_HALF)
+#    stepper_x.set_steps_number(STEPPER_STEPS_PER_REV_Y)
+#    stepper_y.enable()
+    # Watchdog start
+    watchdog.start()
+
+
+def spindle_control(percent):
+    """ Spindle control implementation.
+    :param percent: spindle speed in percent 0..100. If 0, stop the spindle.
+    """
+    logging.debug("spindle control not implemented")
+
+def fan_control(on_off):
+    """
+    Cooling fan control.
+    :param on_off: boolean value if fan is enabled.
+    """
+    logging.debug("fan control not implemented")
+
+def extruder_heater_control(percent):
+    """ Extruder heater control.
+    :param percent: heater power in percent 0..100. 0 turns heater off.
+    """
+    logging.debug("extruder heater control not implemented")
+
+def bed_heater_control(percent):
+    """ Hot bed heater control.
+    :param percent: heater power in percent 0..100. 0 turns heater off.
+    """
+    logging.debug("bed heater control not implemented")
+
+def get_extruder_temperature():
+    """ Measure extruder temperature.
+    :return: temperature in Celsius.
+    """
+    logging.debug("extruder temperature not implemented")
+
+def get_bed_temperature():
+    """ Measure bed temperature.
+    :return: temperature in Celsius.
+    """
+    logging.debug("bed temperature not implemented")
+
+def disable_steppers():
+    """ Disable all steppers until any movement occurs.
+    """
+    stepper_x.disable()
+    stepper_y.disable()
+
+def calibrate(x, y, z):
+    """ Move head to home position till end stop switch will be triggered.
+    Do not return till all procedures are completed.
+    :param x: boolean, True to calibrate X axis.
+    :param y: boolean, True to calibrate Y axis.
+    :param z: boolean, True to calibrate Z axis.
+    :return: boolean, True if all specified end stops were triggered.
+    """
+    logging.debug("calibrate not implemented")
+
+def move(generator):
+    """ Move head to specified position
+    :param generator: PulseGenerator object.
+    """
+    # Fill buffer right before currently running(previous sequence) dma
+    # this mode implements kind of round buffer, but protects if CPU is not
+    # powerful enough to calculate buffer in advance, faster then machine
+    # moving. In this case machine would safely paused between commands until
+    # calculation is done.
+
+    # G1 X50 F100 : permet de faire un mouvement de 50mm selon l'axe X lent (100mm/min soit 1.66mm/s).
+
+    # 4 control blocks per 32 bytes
+    bytes_per_iter = 4 * dma.control_block_size()
+    # prepare and run dma
+    dma.clear() # should just clear current address, but not stop current DMA
+    prevx = 0
+    prevy = 0
+    is_ran = False
+    instant = INSTANT_RUN
+    st = time.time()
+    current_cb = 0
+    k = 0
+    k0 = 0
+    idx = 0
+    for direction, tx, ty, tz, te in generator:
+        if current_cb is not None:
+            logging.debug("{} + {} => result = {}".format(dma.current_address(), bytes_per_iter, dma.current_address() + bytes_per_iter))
+            while dma.current_address() + bytes_per_iter >= current_cb:
+                time.sleep(0.001)
+                current_cb = dma.current_control_block()
+                logging.debug("current control block : {}".format(current_cb))
+                if current_cb is None:
+                    k0 = k
+                    st = time.time()
+                    break  # previous dma sequence has stopped
+#        logging.debug("[{}] direction: {} - tx: {} - ty: {} - tz: {} - te: {}".format(idx, direction, tx, ty, tz, te))
+        if direction:  # set up directions
+            if tx > 0:
+                stepper_x.set_dir(stepper.STATE_DIR_DIRECT)
+            elif tx < 0:
+                stepper_x.set_dir(stepper.STATE_DIR_INV)
+            if ty > 0:
+                stepper_y.set_dir(stepper.STATE_DIR_DIRECT)
+            elif ty < 0:
+                stepper_y.set_dir(stepper.STATE_DIR_INV)
+            dma.add_delay(STEPPER_PULSE_LENGTH_US)
+            continue
+        
+        mask_x = 0
+        mask_y = 0
+        if tx is not None:
+            # transform sec in microsec
+            kx = int(round(tx * US_IN_SECONDS))
+            # retreive current sequence to mask pins
+            cur_seq_x = stepper_x.get_current_seq()
+            #logging.debug("[TX] prev = {} - K = {} - diff : {} - cur_seq_x : {}".format(prevx, kx, kx - prevx, cur_seq_x)) 
+            # mask pins
+            for i, enable in enumerate(cur_seq_x):
+                if enable:
+                    mask_x += 1 << STEPPER_STEP_PINS_X[i]
+            # set pulse with diff between current time and previous time
+#            logging.debug("[TX] MASK: {} - TIME(us): {}".format(bin(mask_x), kx - prevx))
+            dma.add_pulse(mask_x, kx - prevx)
+            stepper_x.inc_seq_num()
+            prevx = kx + STEPPER_PULSE_LENGTH_US
+        if ty is not None:
+            logging.debug("not implemented ...")
+#            ky = int(round(ty * US_IN_SECONDS))
+#            cur_seq_y = stepper_y.get_current_seq()
+#            #logging.debug("[TY] prev = {} - K = {} - diff : {} - cur_seq_y : {}".format(prevy, ky, ky - prevy, cur_seq_y)) 
+#            for j, enable in enumerate(cur_seq_y):
+#                if enable:
+#                    mask_y += 1 << STEPPER_STEP_PINS_Y[j]
+#            logging.debug("[TY] MASK: {} - TIME(us): {}".format(bin(mask_y), ky - prevy))
+#            dma.add_pulse(mask_y, ky - prevy)
+#            stepper_y.inc_seq_num()
+#            prevy = ky + STEPPER_PULSE_LENGTH_US
+        # run stream ...
+#        if not is_ran and instant and current_cb is None:
+#            # TODO: wait 100 ms
+#            time.sleep(0.1)
+#            dma.run_stream()
+#            is_ran = True
+#            logging.debug("starting ... Addr: {}".format(dma.current_address()))
+        idx += 1
+
+    pt = time.time()
+    if not is_ran:
+        # after long command, we can fill short buffer, that why we may need to
+        #  wait until long command finishes
+        while dma.is_active():
+            logging.debug("wait ...")
+            time.sleep(0.01)
+        dma.run(False)
+    else:
+        logging.debug("finalize_stream ...")
+        # stream mode can be activated only if previous command was finished.
+        dma.finalize_stream()
+
+    logging.info("prepared in " + str(round(pt - st, 2)) + "s, estimated in "
+                 + str(round(generator.total_time_s(), 2)) + "s")
+
+def join():
+    """ Wait till motors work.
+    """
+    logging.info("hal join()")
+    # wait till dma works
+    while dma.is_active():
+        time.sleep(0.01)
+
+def deinit():
+    """ De-initialize hardware.
+    """
+    logging.info("deinit")
+    join()
+    disable_steppers()
+    pwm.remove_all()
+    watchdog.stop()
+
+def watchdog_feed():
+    """ Feed hardware watchdog.
+    """
+    watchdog.feed()

cnc/hal_raspberry/rpgpio.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 
-#from .rpgpio_private import *
+from .rpgpio_private import *
-from rpgpio_private import *
+#from rpgpio_private import *
 
 import time
 import logging
@@ -463,7 +463,9 @@ def test():
 
 # for testing purpose
 def main():
-    pins = [4,17,27,22]
+    #pins_x = [4,17,27,22]
+    pins_x = [22,27,17,4]
+    pins_y = [12,16,20,21]
     seqs = [[1,0,0,0], # Phase A
             [1,0,1,0], # Phase AB
             [0,0,1,0], # Phase B
@@ -472,39 +474,50 @@ def main():
             [0,1,0,1], # Phase A'B'
             [0,0,0,1], # Phase B'
             [1,0,0,1]] # Phase AB'
+    seqs.reverse()
     g = GPIO()
-    for pin in pins:
+    for pin in pins_x:
         print("Start output PIN: {}".format(pin))
         g.init(pin, GPIO.MODE_OUTPUT)
 
+    #for pin in pins_y:
+    #    print("Start output PIN: {}".format(pin))
+    #    g.init(pin, GPIO.MODE_OUTPUT)
     dg = DMAGPIO()
-#    mask = 0
-#    for pin in pins:
-#        mask += 1 << pin
-#    print("MASK : {}".format(bin(mask)))
-#
-#    dg.add_pulse(mask, 500000)
-#    dg.add_delay(500000)
-    for seq in seqs:
-        mask = 0
-        for idx, enable in enumerate(seq):
-            if enable:
-                mask += 1 << pins[idx]
-        dg.add_pulse(mask, 10000)
-    dg.add_delay(250000)
 
-    dg.run(True)
+#    counter_max = 100
-    print("dmagpio is started")
+#    counter_current = 0
+#    while counter_current < counter_max:
+#        for i in range(0, 63):
+#            for seq in seqs:
+#                mask = 0
+#                for idx, enable in enumerate(seq):
+#                    if enable:
+#                        mask += 1 << pins_x[idx]
+#                        #mask += 1 << pins_y[idx]
+#                dg.add_pulse(mask, 1000)
+#        dg.finalize_stream()
+#
+#        dg.run_stream()
+#        while dg.is_active():
+#            time.sleep(0.01)
+#        dg.clear()
+#        seqs.reverse()
+#        counter_current += 1
     try:
         print("press enter to stop...")
         sys.stdin.readline()
     except KeyboardInterrupt:
         pass
+    dg.clear()
     dg.stop()
 
-    for pin in pins:
+    for pin in pins_x:
         print("Stop output PIN: {}".format(pin))
         g.clear(pin)
+#    for pin in pins_y:
+#        print("Stop output PIN: {}".format(pin))
+#        g.clear(pin)
 
 if __name__ == "__main__":
     main()

cnc/logging_config.py

@@ -1,6 +1,7 @@
 import logging
 
-logging.basicConfig(level=logging.CRITICAL,
+#logging.basicConfig(level=logging.CRITICAL,
+logging.basicConfig(level=logging.DEBUG,
                     format='[%(levelname)s] %(message)s')
 
 

cnc/main.py

@@ -42,7 +42,8 @@ def do_line(line):
 
 
 def main():
-    logging_config.debug_disable()
+    #logging_config.debug_disable()
+    logging_config.debug_enable()
     try:
         if len(sys.argv) > 1:
             # Read file with gcode

cnc/sensors/ads111x.py

@@ -13,6 +13,8 @@ class __I2CDev(object):
     def __init__(self):
         self._os_close = os.close
         # Initialize i2c interface and register it for closing on exit.
+        if not os.path.isfile("/dev/i2c-1"):
+            raise ImportError("i2c not configured")
         self._dev = os.open("/dev/i2c-1", os.O_SYNC | os.O_RDWR)
         if self._dev < 0:
             raise ImportError("i2c device not found")