modify pin configuration, add calibration fonctionnality to hal raspberrypi and correct velocity

cnc/config.py

@@ -2,13 +2,13 @@
 # Hardware config.
 
 # Maximum velocity for each axis in millimeter per minute.
-MAX_VELOCITY_MM_PER_MIN_X = 4001
+MAX_VELOCITY_MM_PER_MIN_X = 3001
-MAX_VELOCITY_MM_PER_MIN_Y = 4001
+MAX_VELOCITY_MM_PER_MIN_Y = 3001
 MAX_VELOCITY_MM_PER_MIN_Z = 1
 MAX_VELOCITY_MM_PER_MIN_E = 1
-MIN_VELOCITY_MM_PER_MIN = 50
+MIN_VELOCITY_MM_PER_MIN = 10
 # Average velocity for endstop calibration procedure
-CALIBRATION_VELOCITY_MM_PER_MIN = 30
+CALIBRATION_VELOCITY_MM_PER_MIN = 1000
 
 # Stepper motors steps per millimeter for each axis.
 ENABLE_L293D = True
@@ -31,8 +31,8 @@ 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.
-STEPPER_INVERTED_X = False 
+STEPPER_INVERTED_X = True 
-STEPPER_INVERTED_Y = True
+STEPPER_INVERTED_Y = False
 STEPPER_INVERTED_Z = False
 STEPPER_INVERTED_E = True
 
@@ -44,8 +44,8 @@ ENDSTOP_INVERTED_Y = True
 ENDSTOP_INVERTED_Z = False  # Auto leveler
 
 # Workplace physical size.
-TABLE_SIZE_X_MM = 41
+TABLE_SIZE_X_MM = 51
-TABLE_SIZE_Y_MM = 51
+TABLE_SIZE_Y_MM = 41
 TABLE_SIZE_Z_MM = 0
 
 # Mixed settings.
@@ -74,9 +74,9 @@ STEPPER_STEP_PIN_Y = 0
 STEPPER_STEP_PIN_Z = 0
 STEPPER_STEP_PIN_E = 0
 if ENABLE_L293D:
-    STEPPER_STEP_PINS_X = [22,27,17,4]
+    STEPPER_STEP_PINS_Y = [24,25,8,7]
-    STEPPER_STEP_PINS_Y = [12,16,20,21] 
+    STEPPER_STEP_PINS_X = [12,16,20,21] 
-    PEN_PIN = 25
+    PEN_PIN = 18 
 
 STEPPER_DIR_PIN_X = 0
 STEPPER_DIR_PIN_Y = 0 
@@ -96,8 +96,8 @@ if ENABLE_EXTRUDER_HEATER:
 if ENABLE_BED_HEATER:
     BED_TEMPERATURE_SENSOR_CHANNEL = 1
 
-ENDSTOP_PIN_X = 23
+ENDSTOP_PIN_X = 10
-ENDSTOP_PIN_Y = 10
+ENDSTOP_PIN_Y = 9
 ENDSTOP_PIN_Z = 25
 
 # -----------------------------------------------------------------------------

cnc/gmachine.py

@@ -335,11 +335,14 @@ class GMachine(object):
                                       self._convertCoordinates)
             # coord = self._position + delta
         velocity = gcode.get('F', self._velocity)
+        logging.debug("Velocity = {}".format(velocity))
         radius = gcode.radius(Coordinates(0.0, 0.0, 0.0, 0.0),
                               self._convertCoordinates)
         # check parameters
-        if velocity < MIN_VELOCITY_MM_PER_MIN:
+        if velocity < MIN_VELOCITY_MM_PER_MIN and velocity > 1:
             raise GMachineException("feed speed too low")
+        elif velocity == 1:
+            velocity = MIN_VELOCITY_MM_PER_MIN
         # select command and run it
         if c == 'G0':  # rapid move
             vl = max(MAX_VELOCITY_MM_PER_MIN_X,

cnc/hal_raspberry/hal2.py

@@ -75,6 +75,10 @@ class stepper:
             self.seq = self.HALF_STEP_SEQ[:]
             if self.dir == self.STATE_DIR_INV:
                 self.seq.reverse()
+    
+    def get_mode(self):
+        ''' get current operationnal mode '''
+        return self.seq
 
     def set_dir(self, direction):
         ''' set direction '''
@@ -127,6 +131,16 @@ class stepper:
         ''' enable stepper '''
         self.state = self.STEPPER_ENABLED
 
+    def get_cur_mask_and_inc_step(self): 
+        ''' retreive current mask pins and increment sequence '''
+        mask = 0
+        # mask pins
+        for i, enable in enumerate(self.seq[self.current_seq_num]):
+            if enable:
+                mask += 1 << self.pins[i]
+        self.inc_seq_num()
+        return mask
+
     def debug(self):
         ''' debug mode '''
         logging.debug("state = {} - \
@@ -169,13 +183,15 @@ def init():
         stepper_y.set_dir(stepper.STATE_DIR_INV)
     stepper_y.enable()
     stepper_y.debug()
+    # Init EndStop
+    gpio.init(ENDSTOP_PIN_X, rpgpio.GPIO.MODE_INPUT_NOPULL)
+    gpio.init(ENDSTOP_PIN_Y, rpgpio.GPIO.MODE_INPUT_NOPULL)
     # Init pen pin
     gpio.init(PEN_PIN, rpgpio.GPIO.MODE_OUTPUT)
     gpio.clear(PEN_PIN)
     # Watchdog start
     watchdog.start()
 
-
 def spindle_control(percent):
     """ Spindle control implementation.
     :param percent: spindle speed in percent 0..100. If 0, stop the spindle.
@@ -243,7 +259,59 @@ def calibrate(x, y, z):
     :param z: boolean, True to calibrate Z axis.
     :return: boolean, True if all specified end stops were triggered.
     """
-    logging.debug("calibrate not implemented")
+    max_size = 0
+    mask = 0
+    if x:
+        max_size = max(max_size, TABLE_SIZE_X_MM * STEPPER_PULSES_PER_MM_X)
+        stepper_x.set_dir(stepper.STATE_DIR_DIRECT)
+    if y:
+        max_size = max(max_size, TABLE_SIZE_Y_MM * STEPPER_PULSES_PER_MM_Y)
+        stepper_y.set_dir(stepper.STATE_DIR_INV)
+
+    pulses_per_mm_avg = (STEPPER_PULSES_PER_MM_X + STEPPER_PULSES_PER_MM_Y) / 2.0
+    pulses_per_sec = CALIBRATION_VELOCITY_MM_PER_MIN / 60.0 * pulses_per_mm_avg
+    delay = int(US_IN_SECONDS / pulses_per_sec)
+
+    logging.info("[CALIBRATE] num = {} pulses/mm".format(pulses_per_mm_avg))
+    logging.info("[CALIBRATE] num = {} pulses/sec".format(pulses_per_sec))
+    logging.info("[CALIBRATE] delay = {} us".format(delay))
+
+    dma.clear()
+    if not gpio.read(ENDSTOP_PIN_X):
+        # retreive current sequence to X mask pins
+        for _ in stepper_x.get_mode():
+            mask = 0
+            seq = stepper_x.get_current_seq()
+            mask = stepper_x.get_cur_mask_and_inc_step()
+            logging.debug("[X AXIS] MASK = {:#032b} - SEQ = {}".format(mask, seq))
+            dma.add_pulse(mask, delay)
+        dma.finalize_stream()
+        dma.run(True)
+        while dma.is_active():
+            time.sleep(0.1)
+            x_endstop = gpio.read(ENDSTOP_PIN_X) 
+            if x_endstop:
+                dma.stop()
+                dma.clear()
+
+    if not gpio.read(ENDSTOP_PIN_Y):
+        # retreive current sequence to mask Y pins
+        for _ in stepper_y.get_mode():
+            mask = 0
+            seq = stepper_y.get_current_seq()
+            mask = stepper_y.get_cur_mask_and_inc_step()
+            logging.debug("[Y AXIS] MASK = {:#032b} - SEQ = {}".format(mask, seq))
+            dma.add_pulse(mask, delay)
+
+        dma.finalize_stream()
+        dma.run(True)
+        while dma.is_active():
+            time.sleep(0.1)
+            y_endstop = gpio.read(ENDSTOP_PIN_Y) 
+            if y_endstop:
+                dma.stop()
+                dma.clear()
+    return True
 
 def move(generator):
     """ Move head to specified position
@@ -261,6 +329,7 @@ def move(generator):
     bytes_per_iter = 4 * dma.control_block_size()
     # prepare and run dma
     dma.clear() # should just clear current address, but not stop current DMA
+    prev = 0
     prevx = 0
     prevy = 0
     is_ran = False
@@ -300,39 +369,56 @@ def move(generator):
         
         mask_x = 0
         mask_y = 0
-        if tx is not None:
+        mask = 0
+        kx = 0
+        ky = 0
+        if tx is not None and ty is not None:
+            # transform sec in microsec
+            kx = int(round(tx * US_IN_SECONDS))
+            ky = int(round(ty * US_IN_SECONDS))
+            # search min between kx and ky
+            k = min(kx, ky)
+            logging.debug("[TX/TY] prev : {} - K : {} - diff : {}".format(prev, k, k - prev)) 
+            mask = stepper_x.get_cur_mask_and_inc_step()
+            mask += stepper_y.get_cur_mask_and_inc_step()
+
+            # set pulse with diff between current time and previous time
+            dma.add_pulse(mask, k - prev)
+
+            if kx - k > 0: # stay time to complete course for x axis
+                mask_x = stepper_x.get_cur_mask_and_inc_step()
+                # set pulse with diff between current time and previous time
+                dma.add_pulse(mask_x, (kx - k) - prev)
+
+            if ky - k > 0: # stay time to complete course for y axis
+                mask_y = stepper_y.get_cur_mask_and_inc_step()
+                # set pulse with diff between current time and previous time
+                dma.add_pulse(mask_y, (ky - k) - prev)
+
+            prev = k
+
+        elif 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()
+            mask_x = stepper_x.get_cur_mask_and_inc_step()
-            #logging.debug("[TX] prev = {} - K = {} - diff : {} - cur_seq_x : {}".format(prevx, kx, kx - prevx, cur_seq_x)) 
+            logging.debug("[TX {}] K : {}us - delay : {}us - diff : {}us".format(tx, kx, kx - prevx, kx - prev)) 
-            # mask pins
-            for i, enable_x in enumerate(cur_seq_x):
-                if enable_x:
-                    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)
-            dma.add_pulse(mask_x, kx - prevx)
+            prevx = kx
-            stepper_x.inc_seq_num()
+            prev = prevx
-            prevx = kx + STEPPER_PULSE_LENGTH_US
+
-        if ty is not None:
+        elif ty is not None:
+            # transform sec in microsec
             ky = int(round(ty * US_IN_SECONDS))
-            cur_seq_y = stepper_y.get_current_seq()
+            # retreive current sequence to mask pins
-            #logging.debug("[TY] prev = {} - K = {} - diff : {} - cur_seq_y : {}".format(prevy, ky, ky - prevy, cur_seq_y)) 
+            mask_y = stepper_y.get_cur_mask_and_inc_step()
-            for j, enable_y in enumerate(cur_seq_y):
+            logging.debug("[TY {}] K : {}us - delay : {}us - diff : {}us".format(ty, ky, ky - prevy, ky - prev)) 
-                if enable_y:
+            # set pulse with diff between current time and previous time
-                    mask_y += 1 << STEPPER_STEP_PINS_Y[j]
+            #dma.add_pulse(mask_y, ky - prevy)
-            #logging.debug("[TY] MASK: {} - TIME(us): {}".format(bin(mask_y), ky - prevy))
+            prevy = ky
-            dma.add_pulse(mask_y, ky - prevy)
+            prev = 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()
@@ -341,7 +427,7 @@ def move(generator):
         #  wait until long command finishes
         while dma.is_active():
             time.sleep(0.01)
-        dma.run(False)
+        #dma.run(False)
     else:
         logging.debug("finalize_stream ...")
         # stream mode can be activated only if previous command was finished.

tests/rects.gcode

@@ -1,92 +1,93 @@
 G91 f2000
+G28
 ; run
 M300 S30
-G1 x39 f2000
+G1 y39 f2000
-G1 y49 f2000
+G1 x49 f2000
-G1 x-39 f2000
+G1 y-39 f2000
-G1 y-49 f2000
+G1 x-49 f2000
 M300 S50
 M300 S30
-G1 x35 f2500
+G1 y35 f2500
-G1 y45 f2500
+G1 x45 f2500
-G1 x-35 f2500
+G1 y-35 f2500
-G1 y-45 f2500
+G1 x-45 f2500
 M300 S50
 M300 S30
-G1 x30 f3000
+G1 y30 f3000
-G1 y40 f3000
+G1 x40 f3000
-G1 x-30 f3000
+G1 y-30 f3000
-G1 y-40 f3000
+G1 x-40 f3000
 M300 S50
 M300 S30
-G1 x20 f3500
+G1 y20 f3500
-G1 y35 f3500
+G1 x35 f3500
-G1 x-20 f3500
+G1 y-20 f3500
-G1 y-35 f3500
+G1 x-35 f3500
 M300 S50
 M300 S30
-G1 x10 f4000
+G1 y10 f4000
-G1 y45 f4000
+G1 x45 f4000
-G1 x-10 f4000
+G1 y-10 f4000
-G1 y-45 f4000
+G1 x-45 f4000
 M300 S50
 M300 S30
-G1 x39 f2000
+G1 y39 f2000
-G1 y49 f2000
+G1 x49 f2000
-G1 x-39 f2000
+G1 y-39 f2000
-G1 y-49 f2000
+G1 x-49 f2000
 M300 S50
 M300 S30
-G1 x35 f2500
+G1 y35 f2500
-G1 y45 f2500
+G1 x45 f2500
-G1 x-35 f2500
+G1 y-35 f2500
-G1 y-45 f2500
+G1 x-45 f2500
 M300 S50
 M300 S30
-G1 x30 f3000
+G1 y30 f3000
-G1 y40 f3000
+G1 x40 f3000
-G1 x-30 f3000
+G1 y-30 f3000
-G1 y-40 f3000
+G1 x-40 f3000
 M300 S50
 M300 S30
-G1 x20 f3500
+G1 y20 f3500
-G1 y35 f3500
+G1 x35 f3500
-G1 x-20 f3500
+G1 y-20 f3500
-G1 y-35 f3500
+G1 x-35 f3500
 M300 S50
 M300 S30
-G1 x10 f4000
+G1 y10 f4000
-G1 y45 f4000
+G1 x45 f4000
-G1 x-10 f4000
+G1 y-10 f4000
-G1 y-45 f4000
+G1 x-45 f4000
 M300 S50
 M300 S30
-G1 x39 f2000
+G1 y39 f2000
-G1 y49 f2000
+G1 x49 f2000
-G1 x-39 f2000
+G1 y-39 f2000
-G1 y-49 f2000
+G1 x-49 f2000
 M300 S50
 M300 S30
-G1 x35 f2500
+G1 y35 f2500
-G1 y45 f2500
+G1 x45 f2500
-G1 x-35 f2500
+G1 y-35 f2500
-G1 y-45 f2500
+G1 x-45 f2500
 M300 S50
 M300 S30
-G1 x30 f3000
+G1 y30 f3000
-G1 y40 f3000
+G1 x40 f3000
-G1 x-30 f3000
+G1 y-30 f3000
-G1 y-40 f3000
+G1 x-40 f3000
 M300 S50
 M300 S30
-G1 x20 f3500
+G1 y20 f3500
-G1 y35 f3500
+G1 x35 f3500
-G1 x-20 f3500
+G1 y-20 f3500
-G1 y-35 f3500
+G1 x-35 f3500
 M300 S50
 M300 S30
-G1 x10 f4000
+G1 y10 f4000
-G1 y45 f4000
+G1 x45 f4000
-G1 x-10 f4000
+G1 y-10 f4000
-G1 y-45 f4000
+G1 x-45 f4000
 M300 S50