this repo has no description
feat: move firmware to firmware dir
+82
-134
-134
code.py
-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
+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'")