commit fa78c0c6e31fa645e110192e301af6557a3b275c · dunkirk.sh/myrus

+111 -32
code.py
···

       1
       1
       +
       import sys

     

       1
       2
        
       import board

     

       2
       3
        
       import time

     

       3
       4
        
       import digitalio

     

       5
       5
       +
       import supervisor

     

       4
       6
        
       

     

       5
       7
        
       # Define pins for stepper motors

     

       6
       6
       -
       motor1_step = digitalio.DigitalInOut(board.GP10)

     

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

     

       7
       9
        
       motor1_step.direction = digitalio.Direction.OUTPUT

     

       8
       8
       -
       motor1_dir = digitalio.DigitalInOut(board.GP9)

     

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

     

       9
       11
        
       motor1_dir.direction = digitalio.Direction.OUTPUT

     

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

     

       11
       13
        
       motor2_step.direction = digitalio.Direction.OUTPUT

     

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

     

       13
       15
        
       motor2_dir.direction = digitalio.Direction.OUTPUT

     

       14
       14
       -
       enable = digitalio.DigitalInOut(board.GP1)

     

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

     

       15
       17
        
       enable.direction = digitalio.Direction.OUTPUT

     

       16
       18
        
       

     

       19
       19
       +
       command_buffer = ""

     

       20
       20
       +
       last_serial_check = time.monotonic()

     

       21
       21
       +
       SERIAL_CHECK_INTERVAL = 0.1  # Check serial every 1ms

     

       22
       22
       +
       

     

       17
       23
        
       # Enable motors

     

       18
       24
        
       enable.value = False

     

       19
       25
        
       print("Motors enabled")

     

       20
       26
        
       

     

       21
       21
       -
       # Function to step motor

     

       22
       22
       -
       def step(step_pin):

     

       23
       23
       -
           step_pin.value = True

     

       24
       24
       -
           time.sleep(0.000001)  # 1 microsecond

     

       25
       25
       -
           step_pin.value = False

     

       26
       26
       -
           time.sleep(0.000001)  # 1 microsecond

     

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

     

       28
       28
       +
       

     

       29
       29
       +
       # Motor state tracking

     

       30
       30
       +
       class MotorState:

     

       31
       31
       +
           def __init__(self):

     

       32
       32
       +
               self.running = False

     

       33
       33
       +
               self.direction = True  # True for forward

     

       34
       34
       +
               self.speed = 1.0

     

       35
       35
       +
               self.last_step_time = 0

     

       36
       36
       +
       

     

       37
       37
       +
       motor1 = MotorState()

     

       38
       38
       +
       motor2 = MotorState()

     

       39
       39
       +
       

     

       40
       40
       +
       def calculate_step_delay(speed):

     

       41
       41
       +
           return 1 / (STEPS_PER_REVOLUTION * speed)

     

       42
       42
       +
       

     

       43
       43
       +
       def handle_command(command):

     

       44
       44
       +
           parts = command.split()

     

       45
       45
       +
           speed = 1  # Default speed

     

       46
       46
       +
           if len(parts) > 1:

     

       47
       47
       +
               try:

     

       48
       48
       +
                   speed = float(parts[1])

     

       49
       49
       +
               except ValueError:

     

       50
       50
       +
                   print("Invalid speed value")

     

       51
       51
       +
                   return

     

       52
       52
       +
           command = parts[0]

     

       53
       53
       +
       

     

       54
       54
       +
           if command == "forward1":

     

       55
       55
       +
               motor1.running = True

     

       56
       56
       +
               motor1.direction = True

     

       57
       57
       +
               motor1.speed = speed

     

       58
       58
       +
               motor1_dir.value = True

     

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

     

       60
       60
       +
       

     

       61
       61
       +
           elif command == "forward2":

     

       62
       62
       +
               motor2.running = True

     

       63
       63
       +
               motor2.direction = True

     

       64
       64
       +
               motor2.speed = speed

     

       65
       65
       +
               motor2_dir.value = True

     

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

     

       67
       67
       +
       

     

       68
       68
       +
           elif command == "reverse1":

     

       69
       69
       +
               motor1.running = True

     

       70
       70
       +
               motor1.direction = False

     

       71
       71
       +
               motor1.speed = speed

     

       72
       72
       +
               motor1_dir.value = False

     

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

     

       74
       74
       +
       

     

       75
       75
       +
           elif command == "reverse2":

     

       76
       76
       +
               motor2.running = True

     

       77
       77
       +
               motor2.direction = False

     

       78
       78
       +
               motor2.speed = speed

     

       79
       79
       +
               motor2_dir.value = False

     

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

     

       81
       81
       +
       

     

       82
       82
       +
           elif command == "stop1":

     

       83
       83
       +
               motor1.running = False

     

       84
       84
       +
               print("Motor 1 stopped")

     

       85
       85
       +
       

     

       86
       86
       +
           elif command == "stop2":

     

       87
       87
       +
               motor2.running = False

     

       88
       88
       +
               print("Motor 2 stopped")

     

       27
       89
        
       

     

       28
       28
       -
       while True:

     

       29
       29
       -
           # Set direction forward

     

       30
       30
       -
           motor1_dir.value = True

     

       31
       31
       -
           motor2_dir.value = True

     

       32
       32
       -
           print("Moving motors forward...")

     

       90
       90
       +
           elif command == "stop":

     

       91
       91
       +
               motor1.running = False

     

       92
       92
       +
               motor2.running = False

     

       93
       93
       +
               enable.value = True

     

       94
       94
       +
               print("All motors stopped")

     

       33
       95
        
       

     

       34
       34
       -
           # Run motors forward for 1 second

     

       35
       35
       -
           start = time.time()

     

       36
       36
       -
           while time.time() - start < 1:

     

       37
       37
       -
               step(motor1_step)

     

       38
       38
       -
               step(motor2_step)

     

       39
       39
       -
               time.sleep(0.001)

     

       96
       96
       +
           else:

     

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

     

       98
       98
       +
       

     

       99
       99
       +
       def step_motor(motor_state, step_pin):

     

       100
       100
       +
           if not motor_state.running:

     

       101
       101
       +
               return

     

       102
       102
       +
       

     

       103
       103
       +
           current_time = time.monotonic()

     

       104
       104
       +
           step_delay = calculate_step_delay(motor_state.speed)

     

       105
       105
       +
       

     

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

     

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

     

       108
       108
       +
               motor_state.last_step_time = current_time

     

       40
       109
        
       

     

       41
       41
       -
           print("Forward movement complete")

     

       110
       110
       +
       while True:

     

       111
       111
       +
           current_time = time.monotonic()

     

       42
       112
        
       

     

       43
       43
       -
           # Set direction reverse

     

       44
       44
       -
           motor1_dir.value = False

     

       45
       45
       -
           motor2_dir.value = False

     

       46
       46
       -
           print("Moving motors in reverse...")

     

       113
       113
       +
           # Check serial input less frequently

     

       114
       114
       +
           if current_time - last_serial_check >= SERIAL_CHECK_INTERVAL:

     

       115
       115
       +
               if supervisor.runtime.serial_bytes_available:

     

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

     

       47
       117
        
       

     

       48
       48
       -
           # Run motors reverse for 1 second

     

       49
       49
       -
           start = time.time()

     

       50
       50
       -
           while time.time() - start < 1:

     

       51
       51
       -
               step(motor1_step)

     

       52
       52
       -
               step(motor2_step)

     

       53
       53
       -
               time.sleep(0.001)

     

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

     

       119
       119
       +
                       if command_buffer:

     

       120
       120
       +
                           command_buffer = command_buffer[:-1]

     

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

     

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

     

       123
       123
       +
                       print()

     

       124
       124
       +
                       if command_buffer:

     

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

     

       126
       126
       +
                           command_buffer = ""

     

       127
       127
       +
                   else:

     

       128
       128
       +
                       command_buffer += byte

     

       129
       129
       +
                       print(byte, end='')

     

       130
       130
       +
               last_serial_check = current_time

     

       54
       131
        
       

     

       55
       55
       -
           print("Reverse movement complete")

     

       132
       132
       +
           # Handle motor stepping

     

       133
       133
       +
           step_motor(motor1, motor1_step)

     

       134
       134
       +
           step_motor(motor2, motor2_step)