119 lines
No EOL
3.6 KiB
Python
119 lines
No EOL
3.6 KiB
Python
class MotionController:
|
|
# Command mapping for each axis
|
|
AXIS_COMMANDS = {
|
|
'X': {'pos': 'E', 'neg': 'W', 'stop': 'e'},
|
|
'Y': {'pos': 'S', 'neg': 'N', 'stop': 'n'},
|
|
'Z': {'pos': 'U', 'neg': 'D', 'stop': 'u'}
|
|
}
|
|
|
|
# Speed values matching Arduino speedArr
|
|
SPEED_VALUES = [0, 1, 2, 5, 10, 30, 50, 70]
|
|
|
|
def __init__(self, arduino, on_command_sent=None):
|
|
"""
|
|
Args:
|
|
arduino: Arduino instance for sending commands
|
|
on_command_sent: Optional callback(cmd) when command is sent
|
|
"""
|
|
self.arduino = arduino
|
|
self.on_command_sent = on_command_sent
|
|
|
|
# Axis state
|
|
self.axis_direction = {'X': 1, 'Y': 1, 'Z': 1}
|
|
self.axis_moving = {'X': False, 'Y': False, 'Z': False}
|
|
|
|
# Speed state
|
|
self.current_speed_index = 5
|
|
|
|
def send_command(self, cmd):
|
|
"""Send command to Arduino and notify callback"""
|
|
if self.arduino:
|
|
self.arduino.send_command(cmd)
|
|
if self.on_command_sent:
|
|
self.on_command_sent(cmd)
|
|
|
|
# === Axis Movement ===
|
|
|
|
def get_direction(self, axis):
|
|
"""Get current direction for axis (1 = positive, -1 = negative)"""
|
|
return self.axis_direction[axis]
|
|
|
|
def toggle_direction(self, axis):
|
|
"""Toggle direction and return new direction"""
|
|
self.axis_direction[axis] *= -1
|
|
return self.axis_direction[axis]
|
|
|
|
def is_moving(self, axis):
|
|
"""Check if axis is currently moving"""
|
|
return self.axis_moving[axis]
|
|
|
|
def start_movement(self, axis):
|
|
"""Start continuous movement on an axis"""
|
|
commands = self.AXIS_COMMANDS[axis]
|
|
cmd = commands['pos'] if self.axis_direction[axis] == 1 else commands['neg']
|
|
self.send_command(cmd)
|
|
self.axis_moving[axis] = True
|
|
|
|
def stop_axis(self, axis):
|
|
"""Stop movement on a specific axis"""
|
|
commands = self.AXIS_COMMANDS[axis]
|
|
self.send_command(commands['stop'])
|
|
self.axis_moving[axis] = False
|
|
|
|
def stop_all(self):
|
|
"""Emergency stop all axes"""
|
|
self.send_command('!')
|
|
for axis in self.axis_moving:
|
|
self.axis_moving[axis] = False
|
|
|
|
def move_z_up(self):
|
|
"""Start Z axis moving up"""
|
|
self.send_command('U')
|
|
|
|
def move_z_down(self):
|
|
"""Start Z axis moving down"""
|
|
self.send_command('D')
|
|
|
|
def stop_z(self):
|
|
"""Stop Z axis"""
|
|
self.send_command('u')
|
|
|
|
# === Speed Control ===
|
|
|
|
def get_speed_value(self, index):
|
|
"""Get actual speed value for index"""
|
|
if 0 <= index < len(self.SPEED_VALUES):
|
|
return self.SPEED_VALUES[index]
|
|
return 0
|
|
|
|
def set_speed(self, index):
|
|
"""Set speed by index (0-6)"""
|
|
self.current_speed_index = index
|
|
if index == 0:
|
|
self.send_command("0")
|
|
elif index <= 6:
|
|
self.send_command(str(index))
|
|
|
|
def set_speed_preset(self, preset):
|
|
"""Set speed by preset name: 'slow', 'medium', 'fast'"""
|
|
presets = {'slow': 'L', 'medium': 'M', 'fast': 'F'}
|
|
if preset in presets:
|
|
self.send_command(presets[preset])
|
|
|
|
# === Status Commands ===
|
|
|
|
def request_status(self):
|
|
"""Request status from Arduino"""
|
|
self.send_command('?')
|
|
|
|
def zero_all(self):
|
|
"""Zero all axis positions"""
|
|
self.send_command('Z')
|
|
|
|
def go_origin(self):
|
|
"""Move to origin position"""
|
|
self.send_command('G')
|
|
|
|
def toggle_mode(self):
|
|
"""Toggle between serial/joystick mode"""
|
|
self.send_command('T') |