diff --git a/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A.cpp b/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A.cpp index 2e8b16272..6bcdd8f83 100644 --- a/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A.cpp +++ b/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++) { diff --git a/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A.h b/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A.h index e4fa92c56..df5cfff7e 100644 --- a/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A.h +++ b/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); diff --git a/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A_TMC2660_Utils.cpp b/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A_TMC2660_Utils.cpp index 37193e820..00ab67393 100644 --- a/firmware/octopi_firmware_v2/main_controller_teensy41/TMC4361A_TMC2660_Utils.cpp +++ b/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. diff --git a/firmware/octopi_firmware_v2/main_controller_teensy41/main_controller_teensy41.ino b/firmware/octopi_firmware_v2/main_controller_teensy41/main_controller_teensy41.ino index 7433b72bd..4d769c867 100644 --- a/firmware/octopi_firmware_v2/main_controller_teensy41/main_controller_teensy41.ino +++ b/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 *********************************************/ /***************************************************************************************************/ diff --git a/software/control/_def.py b/software/control/_def.py index 4d8f51566..3540ef9fc 100644 --- a/software/control/_def.py +++ b/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 diff --git a/software/control/core/core.py b/software/control/core/core.py index 36ca3e3ef..028b865a3 100644 --- a/software/control/core/core.py +++ b/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") diff --git a/software/control/core/multi_point_worker.py b/software/control/core/multi_point_worker.py index 8734c07e6..a7f8330f5 100644 --- a/software/control/core/multi_point_worker.py +++ b/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()) diff --git a/software/control/microcontroller.py b/software/control/microcontroller.py index fc1a11c0c..967fc400a 100644 --- a/software/control/microcontroller.py +++ b/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) diff --git a/software/squid/abc.py b/software/squid/abc.py index dd54e5ef2..5e945d7f2 100644 --- a/software/squid/abc.py +++ b/software/squid/abc.py @@ -163,6 +163,10 @@ def move_x_to(self, abs_mm: float, blocking: bool = True): def move_y_to(self, abs_mm: float, blocking: bool = True): pass + @abc.abstractmethod + def move_xy_to(self, x_abs_mm: float, y_abs_mm: float, blocking: bool = True): + pass + @abc.abstractmethod def move_z_to(self, abs_mm: float, blocking: bool = True): pass diff --git a/software/squid/stage/cephla.py b/software/squid/stage/cephla.py index eeef8fbc6..779b8a3dd 100644 --- a/software/squid/stage/cephla.py +++ b/software/squid/stage/cephla.py @@ -107,6 +107,18 @@ def move_y_to(self, abs_mm: float, blocking: bool = True): self._calc_move_timeout(abs_mm - self.get_pos().y_mm, self.get_config().Y_AXIS.MAX_SPEED) ) + def move_xy_to(self, x_abs_mm: float, y_abs_mm: float, blocking: bool = True): + self._microcontroller.move_xy_to_usteps( + self._config.X_AXIS.convert_real_units_to_ustep(x_abs_mm), + self._config.Y_AXIS.convert_real_units_to_ustep(y_abs_mm), + ) + + if blocking: + x_timeout = self._calc_move_timeout(x_abs_mm - self.get_pos().x_mm, self.get_config().X_AXIS.MAX_SPEED) + y_timeout = self._calc_move_timeout(y_abs_mm - self.get_pos().y_mm, self.get_config().Y_AXIS.MAX_SPEED) + + self._microcontroller.wait_till_operation_is_completed(max(x_timeout, y_timeout)) + def move_z_to(self, abs_mm: float, blocking: bool = True): # From Hongquan, we want the z axis to rest on the "up" (wrt gravity) direction of gravity. So if we # are moving in the negative (down) z direction, we need to move past our mark a bit then diff --git a/software/squid/stage/prior.py b/software/squid/stage/prior.py index 04a4d4766..564d546b9 100644 --- a/software/squid/stage/prior.py +++ b/software/squid/stage/prior.py @@ -229,6 +229,9 @@ def move_y_to(self, abs_mm: float, blocking: bool = True): else: threading.Thread(target=self.wait_for_stop, daemon=True).start() + def move_xy_to(self, x_abs_mm: float, y_abs_mm: float, blocking: bool = True): + pass + def move_z_to(self, abs_mm: float, blocking: bool = True): pass