Rough, but working. Really needs electrical isolation/filtering

.idea/.gitignore

@@ -6,3 +6,5 @@
 # Datasource local storage ignored files
 /dataSources/
 /dataSources.local.xml
+# GitHub Copilot persisted chat sessions
+/copilot/chatSessions

software/motion_controller/accel_stepper.py

@@ -32,7 +32,7 @@ class AccelStepper:
         self._acceleration = 0.0
         self._sqrt_twoa = 1.0
         self._stepInterval = 0
-        self._minPulseWidth = 1
+        self._minPulseWidth = 0
         self._enablePin = 0xff
         self._lastStepTime = 0
         self._pin = [0, 0, 0, 0]

software/motion_controller/boot.py

@@ -0,0 +1,4 @@
+from motion_controller.main import machine_main
+from time import sleep
+
+machine_main()

software/motion_controller/main.py

@@ -1,8 +1,9 @@
 import machine
 from machine import Pin, ADC
 from neopixel import NeoPixel
-from motion_controller.accel_stepper import AccelStepper
+from motion_controller.accel_stepper import AccelStepper, constrain
 from utime import sleep_us, sleep_ms, ticks_ms
+from motion_controller.uencoder import EncoderKnob
 
 """
 CL57T(V4.0) Closed-Loop Stepper Driver
@@ -20,6 +21,7 @@ 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)
@@ -59,8 +61,8 @@ class ThrustController(MotionControllerBase):
 
         self.steps_per_mm = steps_per_mm
 
-        self.set_max_speed(self.mm_to_steps(200))
+        self.set_max_speed(self.mm_to_steps(10000))
-        self.set_acceleration(self.mm_to_steps(400))
+        self.set_acceleration(self.mm_to_steps(800))
         self.set_speed(0)
 
     def mm_to_steps(self, mm: float) -> int:
@@ -70,7 +72,7 @@ class ThrustController(MotionControllerBase):
 
     def home(self):
         print("Homing")
-        self.set_max_speed(self.mm_to_steps(10))
+        self.set_max_speed(self.mm_to_steps(100))
         self.move(self.mm_to_steps(-480))
 
         while self.homing_pin.value():
@@ -80,7 +82,7 @@ class ThrustController(MotionControllerBase):
         self.set_current_position(0)
         self.run_to_new_position(self.HOMING_BACKOFF)
 
-        self.set_max_speed(self.mm_to_steps(800))
+        self.set_max_speed(self.mm_to_steps(960))
 
     def move_to_mm(self, mm: float):
         self.move_to(self.mm_to_steps(mm))
@@ -103,11 +105,12 @@ class RotationController(MotionControllerBase):
 
     def __init__(self, step_pin: str, dir_pin: str, steps_per_revolution: float):
         super().__init__(step_pin, dir_pin, steps_per_revolution)
-        self.set_max_speed(self.revolutions_per_second_to_steps_per_second(2.2))
+        self.set_max_speed(self.revolutions_per_second_to_steps_per_second(10))
-        self.set_acceleration(self.revolutions_per_second_to_steps_per_second(100))
+        self.set_acceleration(self.revolutions_per_second_to_steps_per_second(1))
 
     def revolutions_per_second_to_steps_per_second(self, rps: float) -> int:
-        return int(self.steps_per_revolution * rps)
+        val = int(self.steps_per_revolution * rps)
+        return val
 
     def set_rps(self, rps: float):
         return self.set_speed(self.revolutions_per_second_to_steps_per_second(rps))
@@ -119,6 +122,51 @@ def set_color(neopixel, color):
         neopixel.write()
 
 
+class MachineStateManager:
+    MODE_THRUST = 0
+    MODE_ROTATION = 1
+
+    MODE_STOPPED = 0
+    MODE_RUNNING = 1
+
+    def __init__(self, np):
+        self.machine_current_state = self.MODE_STOPPED
+        self.encoder_mode = self.MODE_ROTATION
+
+        self.rotation_speed = 0
+        self.thrust_speed = 0.5
+        self.thrusts_per_second_delta = 0.1
+        self.rotations_per_second_delta = 0.02
+
+        self.np = np
+
+    def set_running(self, _):
+        if self.machine_current_state != self.MODE_RUNNING:
+            self.machine_current_state = self.MODE_RUNNING
+            self.encoder_mode = self.MODE_ROTATION
+
+        else:
+            self.encoder_mode = self.MODE_THRUST if self.encoder_mode == self.MODE_ROTATION else self.MODE_ROTATION
+
+        if self.encoder_mode == self.MODE_ROTATION:
+            set_color(self.np, (0, 255, 0))
+        else:
+            set_color(self.np, (0, 0, 255))
+
+
+    def set_stopped(self, _):
+        self.machine_current_state = self.MODE_STOPPED
+
+    def change_speed(self, amount):
+        if self.encoder_mode == self.MODE_ROTATION:
+            self.rotation_speed += (amount * self.rotations_per_second_delta)
+            self.rotation_speed = constrain(self.rotation_speed, -2, 2)
+
+        else:
+            new_speed = self.thrust_speed + (amount * self.thrusts_per_second_delta)
+            self.thrust_speed = constrain(new_speed, 1, 100)
+
+
 # To Do
 # Fix jitter when manual control
 # Increase speed
@@ -129,8 +177,9 @@ def set_color(neopixel, color):
 # Hook up 5V power
 
 def machine_main():
-    thrust = ThrustController("D10", "D9", 3200, 3200 / (2 * 17), "D21")
+    sleep_ms(2000)
-    rotation = RotationController("D8", "D7", 320000)
+    thrust = ThrustController("D10", "D9", 1000.0, 1000.0 / (2 * 17), "D21")
+    rotation = RotationController("D8", "D7", 20000.0)
 
     neopixel_pin = Pin("D6", Pin.OUT)  # Pin number for v1 of the above DevKitC, use pin 38 for v1.1
     np = NeoPixel(neopixel_pin, 3)  # "1" = one RGB led on the "led bus"
@@ -146,7 +195,7 @@ def machine_main():
     slider_off_offset = 800.0
     uint_remaining_range = uint_max - slider_off_offset
 
-    MAX_THRUST_MM = 120.0
+    MAX_THRUST_MM = 450.0
     MIN_THRUST_MM = 20.0
     THRUST_MM_DELTA = MAX_THRUST_MM - MIN_THRUST_MM
     MAX_TPS = 5
@@ -157,10 +206,22 @@ def machine_main():
 
     min_limit_pin = ADC(Pin("A1", Pin.IN))
     max_limit_pin = ADC(Pin("A2", Pin.IN))
-    thrust_per_second_pin = ADC(Pin("A4", Pin.IN))
+
-    rotation_revolutions_per_second_pin = ADC(Pin("A3", Pin.IN))
+    MODE_THRUST = 0
+    MODE_ROTATION = 1
+    thrusts_per_second_value = 0
+    thrusts_per_second_delta = 0.02
+    rotations_per_second_value = 0
+    rotation_per_second_delta = 0.02
+    encoder_mode = MODE_ROTATION
 
     max_rotations_per_second = 2.0
+
+    machine_state = MachineStateManager(np)
+    on_pin.irq(handler=machine_state.set_running, trigger=Pin.IRQ_FALLING)
+    off_pin.irq(handler=machine_state.set_stopped, trigger=Pin.IRQ_FALLING)
+    encoder = EncoderKnob("D17", "D18", btn_pin="D22", rotary_callback=machine_state.change_speed)
+
     rotation_deadband = 2000.0
     lower_rotation_enable = (uint_max / 2.0) - rotation_deadband
     upper_rotation_enable = (uint_max / 2.0) + rotation_deadband
@@ -175,29 +236,25 @@ def machine_main():
     last_move = EXTENDED
 
     last_thrust_value = 0
-    last_speed_percentage = 0
+    last_thrust_speed = 960
+    last_rotation_speed = 0
 
     last_ticks_ms = ticks_ms()
-    running = True
     last_running = False
     while True:
-        print_debug = (ticks_ms() - last_ticks_ms) > 1000
+        #  _debug = (ticks_ms() - last_ticks_ms) > 1000
-        if not off_pin.value():
-            running = False
-        elif not on_pin.value():
-            running = True
 
         min_thrust_val = min_limit_pin.read_u16()
         max_thrust_val = max_limit_pin.read_u16()
-        thrust_per_second_val = thrust_per_second_pin.read_u16()
+
-        rotation_revolutions_per_second_value = rotation_revolutions_per_second_pin.read_u16()
+        # if print_debug:
+        #     print(f"{machine_state.thrust_speed}:{machine_state.rotation_speed}:{machine_state.machine_current_state}")
+            # last_ticks_ms = ticks_ms()
 
         min_limit_percentage = (min_thrust_val - slider_off_offset) / uint_remaining_range
         max_limit_percentage = (max_thrust_val - slider_off_offset) / uint_remaining_range
-        if print_debug:
-            print(f"{min_thrust_val}:{max_thrust_val}|{thrust_per_second_val}:{rotation_revolutions_per_second_value}")
 
-        if running:
+        if machine_state.machine_current_state:
             # Thrust Depth Control
             if min_thrust_val < slider_off_offset:
                 if last_state == TPS_CONTROL:
@@ -222,62 +279,39 @@ def machine_main():
                 if not thrust.is_running():
                     new_endpoint_mm = slider_set_min_mm if last_move == EXTENDED else slider_set_max_mm
                     last_move = RETRACTED if last_move == EXTENDED else EXTENDED
-                    if print_debug:
+                    # if print_debug:
-                        print(f"New mm: {new_endpoint_mm}|last_move: {last_move}")
+                    #     print(f"New mm: {new_endpoint_mm}|last_move: {last_move}")
                     thrust.move_to_mm(new_endpoint_mm)
 
             # Thrust speed control
+            if machine_state.thrust_speed != last_thrust_speed:
+                # print(machine_state.thrust_speed)
+                thrust.set_max_speed(thrust.mm_to_steps(machine_state.thrust_speed) * 20)
+                last_thrust_speed = machine_state.thrust_speed
 
-            # Rotation speed control
+            # Rotation speed control if
-            if rotation_revolutions_per_second_value < lower_rotation_enable:
+            if machine_state.rotation_speed != last_rotation_speed:
-                speed_percentage = (
+                # print(machine_state.rotation_speed)
-                                                   lower_rotation_enable - rotation_revolutions_per_second_value
+                rotation.set_rps(machine_state.rotation_speed)
-                                           ) / uint_rotation_remaining_range
+                last_rotation_speed = machine_state.rotation_speed
-                speed = speed_percentage * max_rotations_per_second
 
-                if abs(last_speed_percentage - speed) < 0.01:
+            if machine_state.machine_current_state != last_running:
-                    rotation.set_rps(speed)
-                    last_speed_percentage = speed_percentage
-
-                if print_debug:
-                    print(f"{speed}")
-
-            elif rotation_revolutions_per_second_value > upper_rotation_enable:
-                speed_percentage = (
-                                                   rotation_revolutions_per_second_value - upper_rotation_enable
-                                           ) / uint_rotation_remaining_range
-                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"{speed}")
-
-            # if last_rotation_sp
-            else:
-                if last_speed_percentage != 0:
-                    rotation.set_speed(0)
-                    last_speed_percentage = 0
-
-            if running != last_running:
                 last_thrust_value = 0
                 set_color(np, (0, 255, 0))
-                last_running = running
+                last_running = machine_state.machine_current_state
 
         else:
-            if running != last_running:
+            if machine_state.machine_current_state != last_running:
                 set_color(np, (255, 255, 0))
-                last_running = running
+                last_running = machine_state.machine_current_state
                 thrust.move_to_mm(MIN_THRUST_MM)
                 rotation.set_speed(0)
 
         thrust.run()
         rotation.run_speed()
 
-        if print_debug:
+        # if print_debug:
-            last_ticks_ms = ticks_ms()
+        #     last_ticks_ms = ticks_ms()
 
 
 machine_main()

software/motion_controller/uencoder.py

@@ -0,0 +1,78 @@
+#
+# Minimal library for interfacing with encoder knobs such as the CYT1100
+#
+import machine
+from micropython import const
+
+
+# ENCODER
+# This Library assumes you are using a 3 pin encoder (2 outputs, and 1 ground).
+# The two outputs will be internally pulled up by the microcontroller. If the
+# Note: you can change the encoder direction by swapping the two output pins.
+
+
+# BUTTON
+# Some encoder knobs also allow for a down press (momentary button). These 
+# typically include two pins on the other side of the encoder. This library
+# assumes one of them will be grounded and the other will be internally pulled
+# up by the microcrontroller.
+
+
+
+class EncoderKnob(object):
+    """
+    Encoder Knob class. Handles reading the knob rotation and button press
+    """
+    def __init__(self, clk_pin, data_pin, btn_pin=None, rotary_callback=None, btn_callback=None):
+        """
+        @param clk_pin: one of the output pins from the encoder (will be internally pulled up)
+        @param data_pin: the other output pin of the encoder (will be internally pulled up)
+        @param btn_pin: (optional) if the encoder also includes a button (will be internally pulled up)
+        @param rotary_callback: (optional) Recommended, callback function accepting a single negative
+                                or positive integer for how much the knob rotated.
+        @param btn_callback: (optional) Recommended, callback accepting no arguments. Called every time
+                             the button is pressed (falling edge)
+        """
+        self.RotaryCallback = rotary_callback
+        self.ButtonCallback = btn_callback
+
+        self.Clk = machine.Pin(clk_pin, machine.Pin.IN, machine.Pin.PULL_UP)
+        self.Data = machine.Pin(data_pin, machine.Pin.IN, machine.Pin.PULL_UP)
+        if btn_pin:
+            self.Button = machine.Pin(btn_pin, machine.Pin.IN, machine.Pin.PULL_UP)
+            self.Button.irq(handler=self._handleButton, trigger=machine.Pin.IRQ_FALLING)
+        self.RawValue = 0
+        self.KnobValue = 0
+
+        self.LastState = 0
+
+        self.Clk.irq(handler=self._handlePins, trigger=machine.Pin.IRQ_RISING | machine.Pin.IRQ_FALLING)
+        self.Data.irq(handler=self._handlePins, trigger=machine.Pin.IRQ_RISING | machine.Pin.IRQ_FALLING)
+
+    def _handlePins(self, pin):
+        state = self.Clk.value()
+        change = 0
+        if state != self.LastState:
+            if self.Data.value() != state:
+                change = 1
+            else:
+                change = -1
+        
+            self.RawValue += change
+            self.KnobValue = int(self.RawValue / 2)
+
+            # The CYT1100 knob reads 2 pulses per click of the wheel
+            if change != 0 and self.RawValue % 2 == 0 and self.RotaryCallback:
+                self.RotaryCallback(change)
+        
+        self.LastState = state
+    
+    def _handleButton(self, pin):
+        if self.ButtonCallback:
+            self.ButtonCallback()
+    
+    def value(self):
+        """
+        @return: current value of the encoder
+        """
+        return self.KnobValue