Tuning

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):
+        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):
+        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:
+                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()