mirror of
https://github.com/caperren/ossm_overkill_edition.git
synced 2025-11-08 13:01:14 +00:00
318 lines
10 KiB
Python
318 lines
10 KiB
Python
import machine
|
|
from machine import Pin, ADC
|
|
from neopixel import NeoPixel
|
|
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
|
|
10
|
|
Figure 10: Sequence chart of control signals
|
|
Remark:
|
|
a) t1: ENA must be ahead of DIR by at least 200ms. Usually, ENA+ and ENA- are NC (not connected). See
|
|
“Connector P1 Configurations” for more information.
|
|
b) t2: DIR must be ahead of PUL effective edge by 2us to ensure correct direction;
|
|
c) t3: Pulse width not less than 1us;
|
|
d) t4: Low level width not less than 1us;
|
|
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
|
|
|
|
super().__init__(self.fwd, self.rev)
|
|
|
|
def one_step(self):
|
|
self.step_pin.value(1)
|
|
sleep_us(1)
|
|
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()
|
|
|
|
|
|
class ThrustController(MotionControllerBase):
|
|
""""""
|
|
HOMING_BACKOFF = 100
|
|
|
|
def __init__(self, step_pin: str, dir_pin: str, steps_per_revolution: float, steps_per_mm: float, homing_pin: str):
|
|
self.homing_pin = Pin(homing_pin, Pin.IN, Pin.PULL_UP)
|
|
super().__init__(step_pin, dir_pin, steps_per_revolution)
|
|
|
|
self.steps_per_mm = steps_per_mm
|
|
|
|
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:
|
|
# 2mm pitch * 17 teeth per rev = 34mm/rev
|
|
|
|
return int(mm * self.steps_per_mm)
|
|
|
|
def home(self):
|
|
print("Homing")
|
|
self.set_max_speed(self.mm_to_steps(100))
|
|
self.move(self.mm_to_steps(-480))
|
|
|
|
while self.homing_pin.value():
|
|
self.run()
|
|
print("Homing complete")
|
|
|
|
self.set_current_position(0)
|
|
self.run_to_new_position(self.HOMING_BACKOFF)
|
|
|
|
self.set_max_speed(self.mm_to_steps(960))
|
|
|
|
def move_to_mm(self, mm: float):
|
|
self.move_to(self.mm_to_steps(mm))
|
|
|
|
def set_min_position(self, min_position: float):
|
|
pass
|
|
|
|
def set_max_position(self, max_position: float):
|
|
pass
|
|
|
|
def set_max_thrust_distance(self, max_distance: float):
|
|
pass
|
|
|
|
def set_max_thrust_per_second(self, thrust_per_second: float):
|
|
pass
|
|
|
|
|
|
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(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:
|
|
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))
|
|
|
|
|
|
def set_color(neopixel, color):
|
|
for pixel in range(3):
|
|
neopixel[pixel] = 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
|
|
# Calibrate distances (why are they wrong?)
|
|
# Remove and tighten spur gear
|
|
# Fix speed for rotation on knob
|
|
# Set knob centers
|
|
# Hook up 5V power
|
|
|
|
def machine_main():
|
|
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"
|
|
|
|
on_pin = Pin("D20", Pin.IN, Pin.PULL_UP)
|
|
off_pin = Pin("D19", Pin.IN, Pin.PULL_UP)
|
|
|
|
set_color(np, (255, 0, 0))
|
|
thrust.home()
|
|
set_color(np, (255, 255, 0))
|
|
|
|
uint_max = 65535.0
|
|
slider_off_offset = 800.0
|
|
uint_remaining_range = uint_max - slider_off_offset
|
|
|
|
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_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))
|
|
|
|
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
|
|
uint_rotation_remaining_range = lower_rotation_enable
|
|
|
|
MANUAL_CONTROL = 0
|
|
TPS_CONTROL = 1
|
|
last_state = MANUAL_CONTROL
|
|
|
|
EXTENDED = 0
|
|
RETRACTED = 1
|
|
last_move = EXTENDED
|
|
|
|
last_thrust_value = 0
|
|
last_thrust_speed = 960
|
|
last_rotation_speed = 0
|
|
|
|
last_ticks_ms = ticks_ms()
|
|
last_running = False
|
|
while True:
|
|
# _debug = (ticks_ms() - last_ticks_ms) > 1000
|
|
|
|
min_thrust_val = min_limit_pin.read_u16()
|
|
max_thrust_val = max_limit_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 machine_state.machine_current_state:
|
|
# Thrust Depth Control
|
|
if min_thrust_val < slider_off_offset:
|
|
if last_state == TPS_CONTROL:
|
|
last_thrust_value = 0
|
|
last_state = MANUAL_CONTROL
|
|
|
|
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 * 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:
|
|
last_move = EXTENDED
|
|
last_state = TPS_CONTROL
|
|
# if print_debug:
|
|
# print(f"slider_set_min_mm mm: {slider_set_min_mm}|slider_set_max_mm: {slider_set_max_mm}|last_state: {last_state}")
|
|
|
|
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}")
|
|
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
|
|
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 machine_state.machine_current_state != last_running:
|
|
last_thrust_value = 0
|
|
set_color(np, (0, 255, 0))
|
|
last_running = machine_state.machine_current_state
|
|
|
|
else:
|
|
if machine_state.machine_current_state != last_running:
|
|
set_color(np, (255, 255, 0))
|
|
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()
|
|
|
|
|
|
machine_main()
|