From 507511e2d9130d5f4ee534b0378ee1a6674f930f Mon Sep 17 00:00:00 2001 From: caperren Date: Sun, 10 Mar 2024 21:07:47 -0700 Subject: [PATCH] Rough, but working. Really needs electrical isolation/filtering --- .idea/.gitignore | 2 + software/motion_controller/accel_stepper.py | 2 +- software/motion_controller/boot.py | 4 + software/motion_controller/main.py | 168 ++++++++++++-------- software/motion_controller/uencoder.py | 78 +++++++++ 5 files changed, 186 insertions(+), 68 deletions(-) create mode 100644 software/motion_controller/boot.py create mode 100644 software/motion_controller/uencoder.py diff --git a/.idea/.gitignore b/.idea/.gitignore index 13566b8..a9d7db9 100644 --- a/.idea/.gitignore +++ b/.idea/.gitignore @@ -6,3 +6,5 @@ # Datasource local storage ignored files /dataSources/ /dataSources.local.xml +# GitHub Copilot persisted chat sessions +/copilot/chatSessions diff --git a/software/motion_controller/accel_stepper.py b/software/motion_controller/accel_stepper.py index 1a6a345..ca0a3fa 100644 --- a/software/motion_controller/accel_stepper.py +++ b/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] diff --git a/software/motion_controller/boot.py b/software/motion_controller/boot.py new file mode 100644 index 0000000..5efe41c --- /dev/null +++ b/software/motion_controller/boot.py @@ -0,0 +1,4 @@ +from motion_controller.main import machine_main +from time import sleep + +machine_main() diff --git a/software/motion_controller/main.py b/software/motion_controller/main.py index 018453b..c6b9269 100644 --- a/software/motion_controller/main.py +++ b/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,10 +21,11 @@ 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._last_dir = self.REV self.steps_per_revolution = steps_per_revolution @@ -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_acceleration(self.mm_to_steps(400)) + self.set_max_speed(self.mm_to_steps(10000)) + 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_acceleration(self.revolutions_per_second_to_steps_per_second(100)) + self.set_max_speed(self.revolutions_per_second_to_steps_per_second(10)) + 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") - rotation = RotationController("D8", "D7", 320000) + sleep_ms(2000) + 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,21 +195,33 @@ 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 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_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)) - 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 - if not off_pin.value(): - running = False - elif not on_pin.value(): - running = True + # _debug = (ticks_ms() - last_ticks_ms) > 1000 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: - print(f"New mm: {new_endpoint_mm}|last_move: {last_move}") + # if print_debug: + # 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 - if rotation_revolutions_per_second_value < lower_rotation_enable: - speed_percentage = ( - lower_rotation_enable - rotation_revolutions_per_second_value - ) / uint_rotation_remaining_range - speed = speed_percentage * max_rotations_per_second + # Rotation speed control if + if machine_state.rotation_speed != last_rotation_speed: + # print(machine_state.rotation_speed) + rotation.set_rps(machine_state.rotation_speed) + last_rotation_speed = machine_state.rotation_speed - if abs(last_speed_percentage - speed) < 0.01: - 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: + if machine_state.machine_current_state != 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: - last_ticks_ms = ticks_ms() + # if print_debug: + # last_ticks_ms = ticks_ms() machine_main() diff --git a/software/motion_controller/uencoder.py b/software/motion_controller/uencoder.py new file mode 100644 index 0000000..72de327 --- /dev/null +++ b/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 \ No newline at end of file