52 lines
1.4 KiB
Python
52 lines
1.4 KiB
Python
|
from i2c_handler import I2CSlaveHandler, Packet
|
||
|
|
from servo import Servo
|
||
|
|
|
||
|
|
s1 = Servo(0)
|
||
|
|
s1.middle()
|
||
|
|
|
||
|
|
def servo_Map(x, in_min, in_max, out_min, out_max):
|
||
|
|
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
|
||
|
|
|
||
|
|
def servo_Angle(angle, servo):
|
||
|
|
if angle < 0:
|
||
|
|
angle = 0
|
||
|
|
if angle > 180:
|
||
|
|
angle = 180
|
||
|
|
servo.goto(round(servo_Map(angle,0,180,0,1024))) # Convert range value to angle value
|
||
|
|
|
||
|
|
def move_body_parts(angle, servo):
|
||
|
|
for i in range(90, 90 + angle, 5):
|
||
|
|
servo_Angle(i, servo)
|
||
|
|
utime.sleep(0.05)
|
||
|
|
for i in range(90 + angle, 90 - angle, -5):
|
||
|
|
servo_Angle(i, servo)
|
||
|
|
utime.sleep(0.05)
|
||
|
|
for i in range(90 - angle, 90, 5):
|
||
|
|
servo_Angle(i, servo)
|
||
|
|
utime.sleep(0.05)
|
||
|
|
|
||
|
|
class ServoHandler(I2CSlaveHandler):
|
||
|
|
def process_packet(self, packet: Packet):
|
||
|
|
"""
|
||
|
|
Example: Override process_packet to perform actions based on packet command/type.
|
||
|
|
"""
|
||
|
|
if packet[0] == 0x01:
|
||
|
|
if packet[2] == 0x00:
|
||
|
|
s1.middle()
|
||
|
|
if packet[2] == 0x01:
|
||
|
|
s1.move_body_parts(15, s1)
|
||
|
|
else:
|
||
|
|
print('Unknown command')
|
||
|
|
|
||
|
|
# Usage example
|
||
|
|
def main():
|
||
|
|
handler = ServoHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41)
|
||
|
|
print("ServoHandler I2C slave started")
|
||
|
|
try:
|
||
|
|
handler.handle()
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
handler.deinit()
|
||
|
|
print("I2C slave stopped")
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|