From 58e89db541ae7c797c956704ebbfa7b09d55df79 Mon Sep 17 00:00:00 2001 From: caperren Date: Sat, 9 Mar 2024 15:14:28 -0800 Subject: [PATCH] Tuning --- software/motion_controller/main.py | 74 ++++++++++++++++++++---------- 1 file changed, 51 insertions(+), 23 deletions(-) diff --git a/software/motion_controller/main.py b/software/motion_controller/main.py index 904e758..018453b 100644 --- a/software/motion_controller/main.py +++ b/software/motion_controller/main.py @@ -18,9 +18,12 @@ e) Duty cycle of PUL signal is recommended 50%""" class MotionControllerBase(AccelStepper): + REV = 1 + FWD = 0 def __init__(self, step_pin: str, dir_pin: str, steps_per_revolution: float): self.step_pin = Pin(step_pin, Pin.OUT, value=0) self.dir_pin = Pin(dir_pin, Pin.OUT, value=0) + self._last_dir = self.REV self.steps_per_revolution = steps_per_revolution @@ -32,13 +35,17 @@ class MotionControllerBase(AccelStepper): self.step_pin.value(0) def fwd(self): - self.dir_pin.value(1) - sleep_us(2) + if self._last_dir != self.FWD: + self.dir_pin.value(1) + sleep_us(2) + self._last_dir = self.FWD self.one_step() def rev(self): - self.dir_pin.value(0) - sleep_us(2) + if self._last_dir != self.REV: + self.dir_pin.value(0) + sleep_us(2) + self._last_dir = self.REV self.one_step() @@ -139,9 +146,14 @@ def machine_main(): slider_off_offset = 800.0 uint_remaining_range = uint_max - slider_off_offset - max_mm = 120.0 - min_mm = 20.0 - mm_delta = max_mm - min_mm + MAX_THRUST_MM = 120.0 + MIN_THRUST_MM = 20.0 + THRUST_MM_DELTA = MAX_THRUST_MM - MIN_THRUST_MM + MAX_TPS = 5 + MAX_TPS_MM_PER_SECOND = THRUST_MM_DELTA * MAX_TPS + THRUST_MM_PER_SECOND_ACCEL = 2000 + thrust.set_max_speed(thrust.mm_to_steps(5*MAX_TPS_MM_PER_SECOND)) + thrust.set_acceleration(thrust.mm_to_steps(THRUST_MM_PER_SECOND_ACCEL)) min_limit_pin = ADC(Pin("A1", Pin.IN)) max_limit_pin = ADC(Pin("A2", Pin.IN)) @@ -162,10 +174,11 @@ def machine_main(): RETRACTED = 1 last_move = EXTENDED - last_manual_max_thrust_val = 0 + last_thrust_value = 0 + last_speed_percentage = 0 last_ticks_ms = ticks_ms() - running = False + running = True last_running = False while True: print_debug = (ticks_ms() - last_ticks_ms) > 1000 @@ -188,15 +201,16 @@ def machine_main(): # Thrust Depth Control if min_thrust_val < slider_off_offset: if last_state == TPS_CONTROL: - last_manual_max_thrust_val = 0 + last_thrust_value = 0 last_state = MANUAL_CONTROL - if abs(last_manual_max_thrust_val - max_thrust_val) > 1000: - final_mm = (mm_delta * max_limit_percentage) + min_mm + if abs(last_thrust_value - max_thrust_val) > 1000: + final_mm = (THRUST_MM_DELTA * max_limit_percentage) + MIN_THRUST_MM thrust.move_to_mm(final_mm) + last_thrust_value = final_mm else: - slider_set_min_mm = (min_limit_percentage * mm_delta) + min_mm - slider_set_max_mm = (max_limit_percentage * mm_delta) + min_mm + slider_set_min_mm = (min_limit_percentage * THRUST_MM_DELTA) + MIN_THRUST_MM + slider_set_max_mm = (max_limit_percentage * THRUST_MM_DELTA) + MIN_THRUST_MM slider_set_max_mm = max(slider_set_max_mm, slider_set_min_mm) if last_state == MANUAL_CONTROL: @@ -216,25 +230,39 @@ def machine_main(): # Rotation speed control if rotation_revolutions_per_second_value < lower_rotation_enable: - reverse_speed_percentage = ( + speed_percentage = ( lower_rotation_enable - rotation_revolutions_per_second_value ) / uint_rotation_remaining_range - rotation.set_rps(reverse_speed_percentage * max_rotations_per_second) + speed = speed_percentage * max_rotations_per_second + + if abs(last_speed_percentage - speed) < 0.01: + rotation.set_rps(speed) + last_speed_percentage = speed_percentage + if print_debug: - print(f"{reverse_speed_percentage}") + print(f"{speed}") elif rotation_revolutions_per_second_value > upper_rotation_enable: - forward_speed_percentage = ( + speed_percentage = ( rotation_revolutions_per_second_value - upper_rotation_enable ) / uint_rotation_remaining_range - rotation.set_rps(-forward_speed_percentage * max_rotations_per_second) + speed = -speed_percentage * max_rotations_per_second + + if abs(last_speed_percentage - speed) < 0.01: + rotation.set_rps(speed) + last_speed_percentage = speed_percentage + if print_debug: - print(f"{forward_speed_percentage}") + print(f"{speed}") + + # if last_rotation_sp else: - rotation.set_speed(0) + if last_speed_percentage != 0: + rotation.set_speed(0) + last_speed_percentage = 0 if running != last_running: - last_manual_max_thrust_val = 0 + last_thrust_value = 0 set_color(np, (0, 255, 0)) last_running = running @@ -242,7 +270,7 @@ def machine_main(): if running != last_running: set_color(np, (255, 255, 0)) last_running = running - thrust.move_to_mm(min_mm) + thrust.move_to_mm(MIN_THRUST_MM) rotation.set_speed(0) thrust.run()