Ch-6 clean
This commit is contained in:
@@ -12,13 +12,13 @@ last_y = 0
|
||||
|
||||
print("Entering main loop")
|
||||
while True:
|
||||
x = robot.left_encoder.read()
|
||||
y = robot.right_encoder.read()
|
||||
x = robot.left_encoder.read()
|
||||
y = robot.right_encoder.read()
|
||||
|
||||
x_diff = x - last_x
|
||||
y_diff = y - last_y
|
||||
x_diff = x - last_x
|
||||
y_diff = y - last_y
|
||||
|
||||
last_x = x
|
||||
last_y = y
|
||||
last_x = x
|
||||
last_y = y
|
||||
|
||||
mouse.move(x=x_diff, y=y_diff)
|
||||
mouse.move(x=x_diff, y=y_diff)
|
||||
|
||||
+20
-20
@@ -54,24 +54,24 @@ send:
|
||||
|
||||
assembled = adafruit_pioasm.assemble(program)
|
||||
|
||||
class QuadratureEncoder:
|
||||
def __init__(self, first_pin, second_pin, reversed=False):
|
||||
"""Encoder with 2 pins. Must use sequential pins on the board"""
|
||||
self.sm = rp2pio.StateMachine(
|
||||
assembled,
|
||||
frequency=0,
|
||||
first_in_pin=first_pin,
|
||||
jmp_pin=second_pin,
|
||||
in_pin_count=2
|
||||
)
|
||||
self.reversed = reversed
|
||||
self._buffer = array.array('i', [0])
|
||||
|
||||
def read(self):
|
||||
while self.sm.in_waiting:
|
||||
self.sm.readinto(self._buffer)
|
||||
if self.reversed:
|
||||
return -self._buffer[0]
|
||||
else:
|
||||
return self._buffer[0]
|
||||
|
||||
class QuadratureEncoder:
|
||||
def __init__(self, first_pin, second_pin, reversed=False):
|
||||
"""Encoder with 2 pins. Must use sequential pins on the board"""
|
||||
self.sm = rp2pio.StateMachine(
|
||||
assembled,
|
||||
frequency=0,
|
||||
first_in_pin=first_pin,
|
||||
jmp_pin=second_pin,
|
||||
in_pin_count=2,
|
||||
)
|
||||
self.reversed = reversed
|
||||
self._buffer = array.array("i", [0])
|
||||
|
||||
def read(self):
|
||||
while self.sm.in_waiting:
|
||||
self.sm.readinto(self._buffer)
|
||||
if self.reversed:
|
||||
return -self._buffer[0]
|
||||
else:
|
||||
return self._buffer[0]
|
||||
|
||||
@@ -13,12 +13,14 @@ left_motor = motor_B1, motor_B2
|
||||
right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=True)
|
||||
left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27)
|
||||
|
||||
|
||||
def stop():
|
||||
motor_A1.duty_cycle = 0
|
||||
motor_A2.duty_cycle = 0
|
||||
motor_B1.duty_cycle = 0
|
||||
motor_B2.duty_cycle = 0
|
||||
|
||||
|
||||
def set_speed(motor, speed):
|
||||
# Swap motor pins if we reverse the speed
|
||||
if speed < 0:
|
||||
@@ -26,15 +28,16 @@ def set_speed(motor, speed):
|
||||
speed = -speed
|
||||
else:
|
||||
direction = motor
|
||||
speed = min(speed, 1) # limit to 1.0
|
||||
max_speed = 2**16-1
|
||||
|
||||
speed = min(speed, 1) # limit to 1.0
|
||||
max_speed = 2 ** 16 - 1
|
||||
|
||||
direction[0].duty_cycle = int(max_speed * speed)
|
||||
direction[1].duty_cycle = 0
|
||||
|
||||
|
||||
def set_left(speed):
|
||||
set_speed(left_motor, speed)
|
||||
|
||||
|
||||
def set_right(speed):
|
||||
set_speed(right_motor, speed)
|
||||
|
||||
Reference in New Issue
Block a user