commit 8e14d23abbee7fe6b0331d5992574ed1edd9e6ed · dunkirk.sh/myrus

-134

code.py

···

       1
       -
       import sys

     

       2
       -
       import board

     

       3
       -
       import time

     

       4
       -
       import digitalio

     

       5
       -
       import supervisor

     

       6
       -
       

     

       7
       -
       # Define pins for stepper motors

     

       8
       -
       motor1_step = digitalio.DigitalInOut(board.GP11)

     

       9
       -
       motor1_step.direction = digitalio.Direction.OUTPUT

     

       10
       -
       motor1_dir = digitalio.DigitalInOut(board.GP10)

     

       11
       -
       motor1_dir.direction = digitalio.Direction.OUTPUT

     

       12
       -
       motor2_step = digitalio.DigitalInOut(board.GP8)

     

       13
       -
       motor2_step.direction = digitalio.Direction.OUTPUT

     

       14
       -
       motor2_dir = digitalio.DigitalInOut(board.GP7)

     

       15
       -
       motor2_dir.direction = digitalio.Direction.OUTPUT

     

       16
       -
       enable = digitalio.DigitalInOut(board.GP9)

     

       17
       -
       enable.direction = digitalio.Direction.OUTPUT

     

       18
       -
       

     

       19
       -
       command_buffer = ""

     

       20
       -
       last_serial_check = time.monotonic()

     

       21
       -
       SERIAL_CHECK_INTERVAL = 0.1  # Check serial every 1ms

     

       22
       -
       

     

       23
       -
       # Enable motors

     

       24
       -
       enable.value = False

     

       25
       -
       print("Motors enabled")

     

       26
       -
       

     

       27
       -
       STEPS_PER_REVOLUTION = 200  # For a typical 1.8° stepper motor

     

       28
       -
       

     

       29
       -
       # Motor state tracking

     

       30
       -
       class MotorState:

     

       31
       -
           def __init__(self):

     

       32
       -
               self.running = False

     

       33
       -
               self.direction = True  # True for forward

     

       34
       -
               self.speed = 1.0

     

       35
       -
               self.last_step_time = 0

     

       36
       -
       

     

       37
       -
       motor1 = MotorState()

     

       38
       -
       motor2 = MotorState()

     

       39
       -
       

     

       40
       -
       def calculate_step_delay(speed):

     

       41
       -
           return 1 / (STEPS_PER_REVOLUTION * speed)

     

       42
       -
       

     

       43
       -
       def handle_command(command):

     

       44
       -
           parts = command.split()

     

       45
       -
           speed = 1  # Default speed

     

       46
       -
           if len(parts) > 1:

     

       47
       -
               try:

     

       48
       -
                   speed = float(parts[1])

     

       49
       -
               except ValueError:

     

       50
       -
                   print("Invalid speed value")

     

       51
       -
                   return

     

       52
       -
           command = parts[0]

     

       53
       -
       

     

       54
       -
           if command == "forward1":

     

       55
       -
               motor1.running = True

     

       56
       -
               motor1.direction = True

     

       57
       -
               motor1.speed = speed

     

       58
       -
               motor1_dir.value = True

     

       59
       -
               print(f"Moving motor 1 forward at {speed} rotations per second...")

     

       60
       -
       

     

       61
       -
           elif command == "forward2":

     

       62
       -
               motor2.running = True

     

       63
       -
               motor2.direction = True

     

       64
       -
               motor2.speed = speed

     

       65
       -
               motor2_dir.value = True

     

       66
       -
               print(f"Moving motor 2 forward at {speed} rotations per second...")

     

       67
       -
       

     

       68
       -
           elif command == "reverse1":

     

       69
       -
               motor1.running = True

     

       70
       -
               motor1.direction = False

     

       71
       -
               motor1.speed = speed

     

       72
       -
               motor1_dir.value = False

     

       73
       -
               print(f"Moving motor 1 in reverse at {speed} rotations per second...")

     

       74
       -
       

     

       75
       -
           elif command == "reverse2":

     

       76
       -
               motor2.running = True

     

       77
       -
               motor2.direction = False

     

       78
       -
               motor2.speed = speed

     

       79
       -
               motor2_dir.value = False

     

       80
       -
               print(f"Moving motor 2 in reverse at {speed} rotations per second...")

     

       81
       -
       

     

       82
       -
           elif command == "stop1":

     

       83
       -
               motor1.running = False

     

       84
       -
               print("Motor 1 stopped")

     

       85
       -
       

     

       86
       -
           elif command == "stop2":

     

       87
       -
               motor2.running = False

     

       88
       -
               print("Motor 2 stopped")

     

       89
       -
       

     

       90
       -
           elif command == "stop":

     

       91
       -
               motor1.running = False

     

       92
       -
               motor2.running = False

     

       93
       -
               enable.value = True

     

       94
       -
               print("All motors stopped")

     

       95
       -
       

     

       96
       -
           else:

     

       97
       -
               print("Unknown command. Use 'forward1/2 [speed]', 'reverse1/2 [speed]' or 'stop1/2'")

     

       98
       -
       

     

       99
       -
       def step_motor(motor_state, step_pin):

     

       100
       -
           if not motor_state.running:

     

       101
       -
               return

     

       102
       -
       

     

       103
       -
           current_time = time.monotonic()

     

       104
       -
           step_delay = calculate_step_delay(motor_state.speed)

     

       105
       -
       

     

       106
       -
           if (current_time - motor_state.last_step_time) >= step_delay:

     

       107
       -
               step_pin.value = not step_pin.value  # Toggle the pin

     

       108
       -
               motor_state.last_step_time = current_time

     

       109
       -
       

     

       110
       -
       while True:

     

       111
       -
           current_time = time.monotonic()

     

       112
       -
       

     

       113
       -
           # Check serial input less frequently

     

       114
       -
           if current_time - last_serial_check >= SERIAL_CHECK_INTERVAL:

     

       115
       -
               if supervisor.runtime.serial_bytes_available:

     

       116
       -
                   byte = sys.stdin.read(1)

     

       117
       -
       

     

       118
       -
                   if byte in ('\x08', '\x7f'):

     

       119
       -
                       if command_buffer:

     

       120
       -
                           command_buffer = command_buffer[:-1]

     

       121
       -
                           print('\x08 \x08', end='')

     

       122
       -
                   elif byte == '\n' or byte == '\r':

     

       123
       -
                       print()

     

       124
       -
                       if command_buffer:

     

       125
       -
                           handle_command(command_buffer.strip().lower())

     

       126
       -
                           command_buffer = ""

     

       127
       -
                   else:

     

       128
       -
                       command_buffer += byte

     

       129
       -
                       print(byte, end='')

     

       130
       -
               last_serial_check = current_time

     

       131
       -
       

     

       132
       -
           # Handle motor stepping

     

       133
       -
           step_motor(motor1, motor1_step)

     

       134
       -
           step_motor(motor2, motor2_step)

+82

firmware/code.py

···

       1
       +
       import board

     

       2
       +
       import time

     

       3
       +
       import digitalio

     

       4
       +
       import supervisor

     

       5
       +
       

     

       6
       +
       class MotorState:

     

       7
       +
           def __init__(self, step_pin, dir_pin, min_delay=0.001):

     

       8
       +
               self.step_pin = step_pin

     

       9
       +
               self.dir_pin = dir_pin

     

       10
       +
               self.position = 0

     

       11
       +
               self.min_delay = min_delay

     

       12
       +
       

     

       13
       +
           def step(self):

     

       14
       +
               self.step_pin.value = True

     

       15
       +
               time.sleep(self.min_delay)

     

       16
       +
               self.step_pin.value = False

     

       17
       +
               time.sleep(self.min_delay)

     

       18
       +
       

     

       19
       +
       # Define pins for stepper motors

     

       20
       +
       motor1_step = digitalio.DigitalInOut(board.GP11)

     

       21
       +
       motor1_step.direction = digitalio.Direction.OUTPUT

     

       22
       +
       motor1_dir = digitalio.DigitalInOut(board.GP10)

     

       23
       +
       motor1_dir.direction = digitalio.Direction.OUTPUT

     

       24
       +
       motor2_step = digitalio.DigitalInOut(board.GP8)

     

       25
       +
       motor2_step.direction = digitalio.Direction.OUTPUT

     

       26
       +
       motor2_dir = digitalio.DigitalInOut(board.GP7)

     

       27
       +
       motor2_dir.direction = digitalio.Direction.OUTPUT

     

       28
       +
       enable = digitalio.DigitalInOut(board.GP9)

     

       29
       +
       enable.direction = digitalio.Direction.OUTPUT

     

       30
       +
       

     

       31
       +
       # Enable motors

     

       32
       +
       enable.value = False

     

       33
       +
       print("Motors enabled")

     

       34
       +
       

     

       35
       +
       STEPS_PER_ROT = 200 * 16 # Steps per rotation

     

       36
       +
       

     

       37
       +
       motor1 = MotorState(motor1_step, motor1_dir, 0.0001)

     

       38
       +
       motor2 = MotorState(motor2_step, motor2_dir, 0.0001)

     

       39
       +
       

     

       40
       +
       while True:

     

       41
       +
           if supervisor.runtime.serial_bytes_available:

     

       42
       +
               command = input().strip().lower()

     

       43
       +
               parts = command.split()

     

       44
       +
       

     

       45
       +
               if len(parts) != 2:

     

       46
       +
                   print("Invalid command. Use '[motor#] [position/zero]'")

     

       47
       +
                   continue

     

       48
       +
       

     

       49
       +
               motor_num = int(parts[0])

     

       50
       +
               target = parts[1]

     

       51
       +
       

     

       52
       +
               if motor_num not in [1,2]:

     

       53
       +
                   print("Invalid motor number. Use 1 or 2")

     

       54
       +
                   continue

     

       55
       +
       

     

       56
       +
               motor = motor1 if motor_num == 1 else motor2

     

       57
       +
       

     

       58
       +
               try:

     

       59
       +
                   if target == "zero":

     

       60
       +
                       motor.position = 0

     

       61
       +
                       print(f"Motor {motor_num} position zeroed")

     

       62
       +
                       continue

     

       63
       +
       

     

       64
       +
                   target_pos = float(target) * STEPS_PER_ROT

     

       65
       +
                   steps = int(target_pos - motor.position)

     

       66
       +
       

     

       67
       +
                   if steps > 0:

     

       68
       +
                       motor.dir_pin.value = True

     

       69
       +
                   else:

     

       70
       +
                       motor.dir_pin.value = False

     

       71
       +
                       steps = -steps

     

       72
       +
       

     

       73
       +
                   print(f"Moving motor {motor_num} to position {target} rotations...")

     

       74
       +
       

     

       75
       +
                   for _ in range(steps):

     

       76
       +
                       motor.step()

     

       77
       +
       

     

       78
       +
                   motor.position = target_pos

     

       79
       +
                   print(f"Motor {motor_num} movement complete")

     

       80
       +
       

     

       81
       +
               except ValueError:

     

       82
       +
                   print("Invalid position. Use number or 'zero'")