a performance improvement for scanning

firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A.cpp

@@ -29,6 +29,7 @@
 */
 
 #include "TMC4361A.h"
+#include "TMC4361A_Constants.h"
 
 // => SPI wrapper
 // Send [length] bytes stored in the [data] array over SPI and overwrite [data]
@@ -158,6 +159,8 @@ void tmc4361A_init(TMC4361ATypeDef *tmc4361A, uint8_t channel, ConfigurationType
   tmc4361A->config->configIndex  = 0;
   tmc4361A->config->state        = CONFIG_READY;
 
+  tmc4361A->ramp_mode = TMC4361A_RAMP_SSHAPE; 
+
   int i;
   for (i = 0; i < TMC4361A_REGISTER_COUNT; i++)
   {

firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A.h

@@ -71,6 +71,7 @@ typedef struct
   int target_tolerance;
   int pid_tolerance;
 
+  int ramp_mode;	
 } TMC4361ATypeDef;
 
 typedef void (*tmc4361A_callback)(TMC4361ATypeDef*, ConfigState);

firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A_TMC2660_Utils.cpp

@@ -983,7 +983,7 @@ void tmc4361A_moveToExtreme(TMC4361ATypeDef *tmc4361A, int32_t vel, int8_t dir)
   -----------------------------------------------------------------------------
 */
 void tmc4361A_sRampInit(TMC4361ATypeDef *tmc4361A) {
-  tmc4361A_setBits(tmc4361A, TMC4361A_RAMPMODE, TMC4361A_RAMP_POSITION | TMC4361A_RAMP_SSHAPE); // positioning mode, s-shaped ramp
+  tmc4361A_setBits(tmc4361A, TMC4361A_RAMPMODE, TMC4361A_RAMP_POSITION | tmc4361A->ramp_mode); // positioning mode
   tmc4361A_rstBits(tmc4361A, TMC4361A_GENERAL_CONF, TMC4361A_USE_ASTART_AND_VSTART_MASK); // keep astart, vstart = 0
   tmc4361A_writeInt(tmc4361A, TMC4361A_BOW1, tmc4361A->rampParam[BOW1_IDX]); // determines the value which increases the absolute acceleration value.
   tmc4361A_writeInt(tmc4361A, TMC4361A_BOW2, tmc4361A->rampParam[BOW2_IDX]); // determines the value which decreases the absolute acceleration value.

firmware/octopi_firmware_v2/main_controller_teensy41/main_controller_teensy41.ino

@@ -25,7 +25,7 @@
 // byte[2]: how many micro steps - upper 8 bits
 // byte[3]: how many micro steps - lower 8 bits
 
-static const int CMD_LENGTH = 8;
+static const int CMD_LENGTH = 12;
 static const int MSG_LENGTH = 24;
 byte buffer_rx[512];
 byte buffer_tx[MSG_LENGTH];
@@ -67,6 +67,8 @@ static const int SET_PID_ARGUMENTS = 29;
 static const int SEND_HARDWARE_TRIGGER = 30;
 static const int SET_STROBE_DELAY = 31;
 static const int SET_AXIS_DISABLE_ENABLE = 32;
+static const int MOVETO_XY = 34;
+static const int SET_SRAMP_MOD = 36;
 static const int SET_PIN_LEVEL = 41;
 static const int INITFILTERWHEEL = 253;
 static const int INITIALIZE = 254;
@@ -301,12 +303,11 @@ uint16_t home_safety_margin[4] = {4, 4, 4, 4};
 // IntervalTimer does not work on teensy with SPI, the below lines are to be removed
 static const int TIMER_PERIOD = 500; // in us
 volatile int counter_send_pos_update = 0;
-volatile bool flag_send_pos_update = false;
 
 static const int interval_send_pos_update = 10000; // in us
 elapsedMicros us_since_last_pos_update;
 
-static const int interval_check_position = 10000; // in us
+static const int interval_check_position = 5000;   // in us
 elapsedMicros us_since_last_check_position;
 
 static const int interval_send_joystick_update = 30000; // in us
@@ -356,7 +357,7 @@ void onJoystickPacketReceived(const uint8_t* buffer, size_t size)
   }
   else
   {
-    focusPosition = focusPosition + (int32_t(uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 | uint32_t(buffer[2]) << 8 | uint32_t(buffer[3])) - focuswheel_pos);
+    focusPosition = focusPosition - (int32_t(uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 | uint32_t(buffer[2]) << 8 | uint32_t(buffer[3])) - focuswheel_pos);
     focuswheel_pos = int32_t(uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 | uint32_t(buffer[2]) << 8 | uint32_t(buffer[3]));
   }
 
@@ -911,6 +912,21 @@ void loop() {
             }
             break;
           }
+        case MOVETO_XY:
+          {
+            long x_absolute_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
+            X_direction = sgn(x_absolute_position - tmc4361A_currentPosition(&tmc4361[x]));
+            X_commanded_target_position = x_absolute_position;
+            if (tmc4361A_moveTo(&tmc4361[x], X_commanded_target_position) == 0)
+              X_commanded_movement_in_progress = true;
+            long y_absolute_position = int32_t(uint32_t(buffer_rx[6]) << 24 | uint32_t(buffer_rx[7]) << 16 | uint32_t(buffer_rx[8]) << 8 | uint32_t(buffer_rx[9]));
+            Y_direction = sgn(y_absolute_position - tmc4361A_currentPosition(&tmc4361[y]));
+            Y_commanded_target_position = y_absolute_position;
+            if (tmc4361A_moveTo(&tmc4361[y], Y_commanded_target_position) == 0)
+              Y_commanded_movement_in_progress = true;
+            mcu_cmd_execution_in_progress = true;
+            break;
+          }
         case MOVETO_Z:
           {
             long absolute_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
@@ -938,6 +954,21 @@ void loop() {
             }
             break;
           }
+        case SET_SRAMP_MOD:
+          {
+            uint8_t axis = buffer_rx[2];
+            uint8_t mode = buffer_rx[3];
+            // map to w axis, in the host w axis index is 5
+            if (axis == AXIS_W)
+              axis = w;
+
+            // guard code
+            if (axis > STAGE_AXES)
+              return;
+
+            tmc4361[axis].ramp_mode = mode;
+            break;
+          }
         case SET_LIM:
           {
             switch (buffer_rx[2])
@@ -2181,9 +2212,9 @@ void loop() {
     buffer_tx[18] &= ~ (1 << BIT_POS_JOYSTICK_BUTTON); // clear the joystick button bit
     buffer_tx[18] = buffer_tx[18] | joystick_button_pressed << BIT_POS_JOYSTICK_BUTTON;
 
-   // Calculate and fill out the checksum.  NOTE: This must be after all other buffer_tx modifications are done!
-   uint8_t checksum = crc8ccitt(buffer_tx, MSG_LENGTH - 1);
-   buffer_tx[MSG_LENGTH - 1] = checksum;
+    // Calculate and fill out the checksum.  NOTE: This must be after all other buffer_tx modifications are done!
+    uint8_t checksum = crc8ccitt(buffer_tx, MSG_LENGTH - 1);
+    buffer_tx[MSG_LENGTH - 1] = checksum;
 
     if(!DEBUG_MODE)
       SerialUSB.write(buffer_tx,MSG_LENGTH);
@@ -2201,8 +2232,6 @@ void loop() {
       SerialUSB.print(", PG:");
       SerialUSB.println(digitalRead(pin_PG));
     }
-    flag_send_pos_update = false;
-
   }
 
   // keep checking position process at suitable frequence
@@ -2213,22 +2242,29 @@ void loop() {
     {
       X_commanded_movement_in_progress = false;
       mcu_cmd_execution_in_progress = false || Y_commanded_movement_in_progress || Z_commanded_movement_in_progress || W_commanded_movement_in_progress;
+      // It's important that we check positions next time around because
+      // once the axis reaches the target position, we need the PC to be notified instantly.
+      // Other axes are the same reason.
+      us_since_last_pos_update = interval_send_pos_update + 1;
     }
     if (Y_commanded_movement_in_progress && tmc4361A_currentPosition(&tmc4361[y]) == Y_commanded_target_position && !is_homing_Y && !tmc4361A_isRunning(&tmc4361[y], stage_PID_enabled[y]))
     {
       Y_commanded_movement_in_progress = false;
       mcu_cmd_execution_in_progress = false || X_commanded_movement_in_progress || Z_commanded_movement_in_progress || W_commanded_movement_in_progress;
+      us_since_last_pos_update = interval_send_pos_update + 1;
     }
     if (Z_commanded_movement_in_progress && tmc4361A_currentPosition(&tmc4361[z]) == Z_commanded_target_position && !is_homing_Z && !tmc4361A_isRunning(&tmc4361[z], stage_PID_enabled[z]))
     {
       Z_commanded_movement_in_progress = false;
       mcu_cmd_execution_in_progress = false || X_commanded_movement_in_progress || Y_commanded_movement_in_progress || W_commanded_movement_in_progress;
+      us_since_last_pos_update = interval_send_pos_update + 1;
     }
     if (enable_filterwheel == true) {
       if (W_commanded_movement_in_progress && tmc4361A_currentPosition(&tmc4361[w]) == W_commanded_target_position && !is_homing_W && !tmc4361A_isRunning(&tmc4361[w], stage_PID_enabled[w]))
       {
         W_commanded_movement_in_progress = false;
         mcu_cmd_execution_in_progress = false || X_commanded_movement_in_progress || Y_commanded_movement_in_progress || Z_commanded_movement_in_progress;
+        us_since_last_pos_update = interval_send_pos_update + 1;
       }
     }
   }
@@ -2270,28 +2306,6 @@ void loop() {
   }
 }
 
-/***************************************************
-
-                    timer interrupt
-
- ***************************************************/
-
-// timer interrupt
-/*
-  // IntervalTimer stops working after SPI.begin()
-  void timer_interruptHandler()
-  {
-  SerialUSB.println("timer event");
-  counter_send_pos_update = counter_send_pos_update + 1;
-  if(counter_send_pos_update==interval_send_pos_update/TIMER_PERIOD)
-  {
-    flag_send_pos_update = true;
-    counter_send_pos_update = 0;
-    SerialUSB.println("send pos update");
-  }
-  }
-*/
-
 /***************************************************************************************************/
 /*********************************************  utils  *********************************************/
 /***************************************************************************************************/

software/control/_def.py

@@ -100,7 +100,7 @@ class PosUpdate:
 
 class MicrocontrollerDef:
     MSG_LENGTH = 24
-    CMD_LENGTH = 8
+    CMD_LENGTH = 12
     N_BYTES_POS = 4
 
 
@@ -159,6 +159,8 @@ class CMD_SET:
     SEND_HARDWARE_TRIGGER = 30
     SET_STROBE_DELAY = 31
     SET_AXIS_DISABLE_ENABLE = 32
+    MOVETO_XY = 34
+    SET_SRAMP_MOD = 36
     SET_PIN_LEVEL = 41
     INITFILTERWHEEL = 253
     INITIALIZE = 254
@@ -424,6 +426,7 @@ def convert_to_enum(option: Union[str, "FocusMeasureOperator"]) -> "FocusMeasure
 
 # end of actuator specific configurations
 
+SCAN_STABILIZATION_TIME_MS = 160
 SCAN_STABILIZATION_TIME_MS_X = 160
 SCAN_STABILIZATION_TIME_MS_Y = 160
 SCAN_STABILIZATION_TIME_MS_Z = 20
@@ -440,6 +443,14 @@ def convert_to_enum(option: Union[str, "FocusMeasureOperator"]) -> "FocusMeasure
 DEFAULT_SAVING_PATH = str(Path.home()) + "/Downloads"
 FILE_ID_PADDING = 0
 
+# 0: RAMP_HOLD
+# 1: RAMP_TRAPEZ
+# 2: RAMP_SSHAPE
+X_AXIS_SRAMP_MODE = 2
+Y_AXIS_SRAMP_MODE = 2
+Z_AXIS_SRAMP_MODE = 2
+W_AXIS_SRAMP_MODE = 2
+
 
 class PLATE_READER:
     NUMBER_OF_ROWS = 8

software/control/core/core.py

@@ -1173,8 +1173,7 @@ def gen_focus_map(self, coord1, coord2, coord3):
 
         for coord in [coord1, coord2, coord3]:
             print(f"Navigating to coordinates ({coord[0]},{coord[1]}) to sample for focus map")
-            self.stage.move_x_to(coord[0])
-            self.stage.move_y_to(coord[1])
+            self.stage.move_xy_to(coord[0], coord[1])
 
             print("Autofocusing")
             self.autofocus(True)
@@ -2370,8 +2369,7 @@ def run_acquisition(self, acquire_current_fov=False):
                 self.autofocusController.set_focus_map_use(True)
 
                 # Return to center position
-                self.stage.move_x_to(x_center)
-                self.stage.move_y_to(y_center)
+                self.stage.move_xy_to(x_center, y_center)
 
             except ValueError:
                 self._log.exception("Invalid coordinates for autofocus plane, aborting.")
@@ -2448,8 +2446,7 @@ def _on_acquisition_completed(self):
             if "current" in self.scanCoordinates.region_centers:
                 region_center = self.scanCoordinates.region_centers["current"]
                 try:
-                    self.stage.move_x_to(region_center[0])
-                    self.stage.move_y_to(region_center[1])
+                    self.stage.move_xy_to(region_center[0], region_center[1])
                     self.stage.move_z_to(region_center[2])
                 except:
                     self._log.error("Failed to move to center of current region")

software/control/core/multi_point_worker.py

@@ -370,20 +370,17 @@ def update_coordinates_dataframe(self, region_id, z_level, pos: squid.abc.Pos, f
         self.coordinates_pd = pd.concat([self.coordinates_pd, new_row], ignore_index=True)
 
     def move_to_coordinate(self, coordinate_mm):
-        print("moving to coordinate", coordinate_mm)
         x_mm = coordinate_mm[0]
-        self.stage.move_x_to(x_mm)
-        self._sleep(SCAN_STABILIZATION_TIME_MS_X / 1000)
-
         y_mm = coordinate_mm[1]
-        self.stage.move_y_to(y_mm)
-        self._sleep(SCAN_STABILIZATION_TIME_MS_Y / 1000)
+        have_z = len(coordinate_mm) >= 3
+        self.stage.move_xy_to(x_mm, y_mm, blocking=not have_z)
 
-        # check if z is included in the coordinate
-        if len(coordinate_mm) == 3:
+        if have_z:
             z_mm = coordinate_mm[2]
             self.move_to_z_level(z_mm)
 
+        self._sleep(SCAN_STABILIZATION_TIME_MS / 1000)
+
     def move_to_z_level(self, z_mm):
         print("moving z")
         self.stage.move_z_to(z_mm)
@@ -646,7 +643,6 @@ def acquire_camera_image(
         camera_illumination_time = self.camera.get_exposure_time()
         if self.liveController.trigger_mode == TriggerMode.SOFTWARE:
             self.liveController.turn_on_illumination()
-            self.wait_till_operation_is_completed()
             camera_illumination_time = None
         elif self.liveController.trigger_mode == TriggerMode.HARDWARE:
             if "Fluorescence" in config.name and ENABLE_NL5 and NL5_USE_DOUT:
@@ -719,7 +715,6 @@ def acquire_rgb_image(self, config, file_ID, current_path, current_round_images,
                 if self.liveController.trigger_mode == TriggerMode.SOFTWARE:
                     # TODO(imo): use illum controller
                     self.liveController.turn_on_illumination()
-                    self.wait_till_operation_is_completed()
 
                 # read camera frame
                 self.camera.send_trigger(illumination_time=self.camera.get_exposure_time())

software/control/microcontroller.py

@@ -505,6 +505,10 @@ def __init__(self, serial_device: AbstractCephlaMicroSerial, reset_and_initializ
             self.log.debug("Resetting and initializing microcontroller.")
             self.reset()
             time.sleep(0.5)
+
+            # Need set the parameter before initialization
+            self.initizlize_sramp_of_axes()
+
             if USE_SQUID_FILTERWHEEL:
                 self.init_filter_wheel()
                 time.sleep(0.5)
@@ -561,6 +565,17 @@ def reset(self):
         # here.
         self._cmd_id = 0
 
+    def initizlize_sramp_of_axes(self):
+        # initialize axes sramp mode
+        self.set_sramp_mode(AXIS.X, X_AXIS_SRAMP_MODE)
+        self.wait_till_operation_is_completed()
+        self.set_sramp_mode(AXIS.Y, Y_AXIS_SRAMP_MODE)
+        self.wait_till_operation_is_completed()
+        self.set_sramp_mode(AXIS.Z, Z_AXIS_SRAMP_MODE)
+        self.wait_till_operation_is_completed()
+        self.set_sramp_mode(AXIS.W, W_AXIS_SRAMP_MODE)
+        self.wait_till_operation_is_completed()
+
     def initialize_drivers(self):
         self._cmd_id = 0
         cmd = bytearray(self.tx_buffer_length)
@@ -684,6 +699,24 @@ def move_y_to_usteps(self, usteps):
         cmd[5] = payload & 0xFF
         self.send_command(cmd)
 
+    def move_xy_to_usteps(self, xusteps, yusteps):
+        xpayload = self._int_to_payload(xusteps, 4)
+        ypayload = self._int_to_payload(yusteps, 4)
+
+        cmd = bytearray(self.tx_buffer_length)
+        cmd[1] = CMD_SET.MOVETO_XY
+        cmd[2] = xpayload >> 24
+        cmd[3] = (xpayload >> 16) & 0xFF
+        cmd[4] = (xpayload >> 8) & 0xFF
+        cmd[5] = xpayload & 0xFF
+
+        cmd[6] = ypayload >> 24
+        cmd[7] = (ypayload >> 16) & 0xFF
+        cmd[8] = (ypayload >> 8) & 0xFF
+        cmd[9] = ypayload & 0xFF
+
+        self.send_command(cmd)
+
     def move_z_usteps(self, usteps):
         self._move_axis_usteps(usteps, CMD_SET.MOVE_Z)
 
@@ -961,6 +994,9 @@ def configure_squidfilter(self):
         self.wait_till_operation_is_completed()
         self.set_max_velocity_acceleration(AXIS.W, MAX_VELOCITY_W_mm, MAX_ACCELERATION_W_mm)
         self.wait_till_operation_is_completed()
+        # initialize axes sramp mode
+        self.set_sramp_mode(AXIS.W, W_AXIS_SRAMP_MODE)
+        self.wait_till_operation_is_completed()
 
     def ack_joystick_button_pressed(self):
         cmd = bytearray(self.tx_buffer_length)
@@ -994,6 +1030,13 @@ def set_pin_level(self, pin, level):
         cmd[3] = level
         self.send_command(cmd)
 
+    def set_sramp_mode(self, axis, mode):
+        cmd = bytearray(self.tx_buffer_length)
+        cmd[1] = CMD_SET.SET_SRAMP_MOD
+        cmd[2] = axis
+        cmd[3] = mode
+        self.send_command(cmd)
+
     def turn_on_AF_laser(self):
         self.set_pin_level(MCU_PINS.AF_LASER, 1)