63 lines
1.8 KiB
Python
63 lines
1.8 KiB
Python
|
from i2c_handler import I2CSlaveHandler, Packet
|
||
|
|
from machine import Pin
|
||
|
|
|
||
|
|
# -high + low -> nach vorne
|
||
|
|
# andersrum nach hinten
|
||
|
|
|
||
|
|
in1_1 = Pin(18, Pin.OUT) # front right -
|
||
|
|
in1_2 = Pin(19, Pin.OUT) # front right +
|
||
|
|
in1_3 = Pin(20, Pin.OUT) # front left -
|
||
|
|
in1_4 = Pin(21, Pin.OUT) # front left +
|
||
|
|
|
||
|
|
in2_1 = Pin(13, Pin.OUT) # back right -
|
||
|
|
in2_2 = Pin(12, Pin.OUT) # back right +
|
||
|
|
in2_3 = Pin(11, Pin.OUT) # back left -
|
||
|
|
in2_4 = Pin(10, Pin.OUT) # back left +
|
||
|
|
|
||
|
|
def set_motor(direction, pin1, pin2):
|
||
|
|
if direction == 0x00: # off
|
||
|
|
pin1.value(0)
|
||
|
|
pin2.value(0)
|
||
|
|
elif direction == 0x01: # forward
|
||
|
|
pin1.value(1)
|
||
|
|
pin2.value(0)
|
||
|
|
elif direction == 0x02: # backward
|
||
|
|
pin1.value(0)
|
||
|
|
pin2.value(1)
|
||
|
|
|
||
|
|
class MotorHandler(I2CSlaveHandler):
|
||
|
|
def process_packet(self, packet: Packet):
|
||
|
|
"""
|
||
|
|
Example: Override process_packet to perform actions based on packet command/type.
|
||
|
|
"""
|
||
|
|
if packet[0] == 0x01:
|
||
|
|
# Motor directions
|
||
|
|
fl, fr, bl, br = packet[2], packet[3], packet[4], packet[5]
|
||
|
|
|
||
|
|
# Map motors to their control pins
|
||
|
|
motors = {
|
||
|
|
'fl': (fl, in1_3, in1_4),
|
||
|
|
'fr': (fr, in1_1, in1_2),
|
||
|
|
'bl': (bl, in2_3, in2_4),
|
||
|
|
'br': (br, in2_1, in2_2)
|
||
|
|
}
|
||
|
|
|
||
|
|
# Set each motor
|
||
|
|
for name, (direction, pin1, pin2) in motors.items():
|
||
|
|
set_motor(direction, pin1, pin2)
|
||
|
|
|
||
|
|
else:
|
||
|
|
print("Unknown command")
|
||
|
|
|
||
|
|
# Usage example
|
||
|
|
def main():
|
||
|
|
handler = MotorHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41)
|
||
|
|
print("MotorHandler I2C slave started")
|
||
|
|
try:
|
||
|
|
handler.handle()
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
handler.deinit()
|
||
|
|
print("I2C slave stopped")
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|