"""
    Robocar 2 Wheels - Variante 1
"""
import machine, time

print("   ##### RoboCar 2 Wheels #####")

ena_pin = 23; in1_pin = 22; in2_pin = 21;  
enb_pin = 15; in3_pin =  4; in4_pin =  2;

ena =  machine.PWM(machine.Pin(ena_pin));     ena.duty(0) 
in1 =  machine.Pin(in1_pin, machine.Pin.OUT); in1.off()
in2 =  machine.Pin(in2_pin, machine.Pin.OUT); in2.off()
enb =  machine.PWM(machine.Pin(enb_pin));     enb.duty(0)
in3 =  machine.Pin(in3_pin, machine.Pin.OUT); in1.off()
in4 =  machine.Pin(in4_pin, machine.Pin.OUT); in2.off()

dt = 0.1 # Sekunden
ena.duty(90); time.sleep(dt); ena.duty(255); time.sleep(dt);  
in1.on();  time.sleep(dt); in2.on();  time.sleep(dt)

enb.duty(140); time.sleep(dt); enb.duty(255); time.sleep(dt)
in3.on();  time.sleep(dt); in4.on();  time.sleep(dt)

def motorA_vor(duty_value):                # rechter Motor
    in1.on(); in2.off(); ena.duty(duty_value)

def motorB_vor(duty_value):                # linker Motor
    in3.on(); in4.off(); enb.duty(duty_value)

def motorA_rueck(duty_value):
    in1.off(); in2.on(); ena.duty(duty_value)

def motorB_rueck(duty_value):
    in3.off(); in4.on(); enb.duty(duty_value)

def vor(duty_value):
    print("vor ", duty_value)
    motorA_vor(duty_value)
    motorB_vor(duty_value)

def rueck(duty_value):
    print("rueck ", duty_value)
    motorA_rueck(duty_value)
    motorB_rueck(duty_value)

def fahre(duty_value_A, duty_value_B):
    print("links_vor ", duty_value_A," ",duty_value_B)
    motorA_vor(duty_value)
    motorB_vor(duty_value)

def links_drehen(duty_value_A, duty_value_B):
    print("links_vor ", duty_value_A," ",duty_value_B)
    motorA_vor(duty_value_A)
    motorB_rueck(duty_value_B)

def fahren(*args):
    l = len(args) 
    print("Fahren: ", len(args), args)
    if l==0:
        pass
    elif l==1:
        if args[0]>=0:
           motorA_vor(args[0]) 
           motorB_vor(args[0]) 
        else:
           motorA_rueck(args[0]) 
           motorB_rueck(args[0]) 

    elif l==2:
        motorA_vor(args[0])
        motorB_vor(args[1])
    elif l==3:
        motorA_vor(args[0])
        motorB_vor(args[1])
        time.sleep(args[1])
    else:
        pass
      
dt = 2.5
vor(222); time.sleep(dt)
#rueck(77); time.sleep(dt)
fahren(33,22)