caperren/ossm_overkill_edition
1
0
Fork 0
mirror of https://github.com/caperren/ossm_overkill_edition.git synced 2025-11-08 21:11:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

Rough, but working. Really needs electrical isolation/filtering

Browse Source
This commit is contained in:
caperren
2024-03-10 21:07:47 -07:00
parent 58e89db541
commit 507511e2d9
5 changed files with 186 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
software/motion_controller/main.py
View File

@@ -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()