from machine import *
import time
import sys

time.sleep(1)

pins = [0, 1, 2, 3]

pin = Pin(16, Pin.OUT)
f = PWM(pin)
f.freq(100)
f.duty_u16(50)

#F,30,1,1,1,1,0

g1 = Pin(pins[0], Pin.OUT)
g2 = Pin(pins[1], Pin.OUT)
g3 = Pin(pins[2], Pin.OUT)
g4 = Pin(pins[3], Pin.OUT)

servoo = PWM(Pin(15, Pin.OUT))
servoo.freq(50)
time.sleep(5)
servoo.duty_u16(int(((0 / 180) * 97 + 26) / 1023 * 65535))
time.sleep(1)

def nn():
    while True:
        stringg = sys.stdin.readline().strip()
        parts = stringg.split(',')
        if len(parts) == 7:
            return parts
        print("invalid input")

def ee(grip1, grip2, grip3, grip4, speed, angle):
    global g1, g2, g3, g4, f, servoo
    g1.value(int(grip1))
    g2.value(int(grip2))
    g3.value(int(grip3))
    g4.value(int(grip4))
    num = int(angle)
    servoo.duty_u16(int(((num / 180) * 97 + 26) / 1023 * 65535))
    f.duty_u16(int(int(speed) * 655.35))

while True:
    motion, speed, grip1, grip2, grip3, grip4, angle = nn()
    ee(grip1, grip2, grip3, grip4, int(speed), int(angle))