"""
    Robocar 4 Wheels - Omni Direction RoboCar black
"""
import machine, time

print("   ##### RoboCar 4 Wheels #####")

# rechte Brücke - steuert hinteren Motoren
hr_ena_pin = 26; hr_in1_pin = 25; hr_in2_pin = 17;  # OK
hl_enb_pin = 14; hl_in3_pin = 27; hl_in4_pin = 16;  # OK

# linke Brücke - steuert vorderen Motoren
vr_ena_pin = 18; vr_in1_pin = 23; vr_in2_pin = 19;  # OK
vl_enb_pin = 12; vl_in3_pin = 13; vl_in4_pin =  5;  # OK


hr_ena =  machine.PWM(machine.Pin(hr_ena_pin));     hr_ena.duty(0) 
hr_in1 =  machine.Pin(hr_in1_pin, machine.Pin.OUT); hr_in1.off()
hr_in2 =  machine.Pin(hr_in2_pin, machine.Pin.OUT); hr_in2.off()

hl_enb =  machine.PWM(machine.Pin(hl_enb_pin));     hl_enb.duty(0) 
hl_in3 =  machine.Pin(hl_in3_pin, machine.Pin.OUT); hl_in3.off()
hl_in4 =  machine.Pin(hl_in4_pin, machine.Pin.OUT); hl_in4.off()

vr_ena =  machine.PWM(machine.Pin(vr_ena_pin));     vr_ena.duty(0) 
vr_in1 =  machine.Pin(vr_in1_pin, machine.Pin.OUT); vr_in1.off()
vr_in2 =  machine.Pin(vr_in2_pin, machine.Pin.OUT); vr_in2.off()

vl_enb =  machine.PWM(machine.Pin(vl_enb_pin));     vl_enb.duty(0) 
vl_in3 =  machine.Pin(vl_in3_pin, machine.Pin.OUT); vl_in3.off()
vl_in4 =  machine.Pin(vl_in4_pin, machine.Pin.OUT); hl_in4.off()


def motor_hl(dv):   # OK
    print("- hinten links   :", dv)
    if dv>=0:
        hr_in1.on(); hr_in2.off(); hr_ena.duty(dv)
    else:
        hr_in1.off(); hr_in2.on(); hr_ena.duty(-dv)    

def motor_hr(dv):   # OK
    print("hinten rechts   :", dv)
    if dv>=0:
        hl_in3.off(); hl_in4.on(); hl_enb.duty(dv)
    else:
        hl_in3.on();  hl_in4.off(); hl_enb.duty(-dv)    

def motor_vr(dv):   # ok
    print("vorn rechts   :", dv)
    if dv>=0:
        vr_in1.off(); vr_in2.on(); vr_ena.duty(dv)
    else:
        vr_in1.on(); vr_in2.off(); vr_ena.duty(-dv)    

def motor_vl(dv):   # ok
    print("vorn links   :", dv)
    if dv>=0:
        vl_in4.on(); vl_in3.off();  vl_enb.duty(dv)
    else:
        vl_in4.off();  vl_in3.on(); vl_enb.duty(-dv)    


def halt():
    dv=0
    hr_ena.duty(dv)
    hl_enb.duty(dv)
    vr_ena.duty(dv)
    vl_enb.duty(dv)

   
def fahre(*args):
    if len(args)==1:
        dv = int(args[0])
        if abs(dv)<1024:
            motor_vl(dv); motor_vr(dv); motor_hl(dv); motor_hr(dv)
        else:
            print("-1023 ... 1023")
    elif len(args)==2:
        dvl = int(args[0]); dvr = int(args[1]);
        if abs(dvl)<1024 and abs(dvr)<1024 :
            motor_vl(dvl); motor_vr(dvr); motor_hl(dvl); motor_hr(dvr)
        else:
            print("-1023 ... 1023")
    elif len(args)==4:
        dv1 = int(args[0]); dv2 = int(args[1])
        dv3 = int(args[2]); dv4 = int(args[3])
        if abs(dv1)<1024 and abs(dv2)<1024  and abs(dv3)<1024  and abs(dv4)<1024 :
            motor_vl(dv1); motor_vr(dv2); motor_hl(dv3); motor_hr(dv4)
        else:
            print("-1023 ... 1023")

    else:
        print("falsche Parameterzahl")

# --- Ende fahren() ---

print("Befehle aus robocar4w:")
print("pwm_duty_wert von -1023 ... 1023")   
print("  motor_vl(888)")
print("  motor_vr(-888)")
print("  motor_hl(1023)")
print("  motor_hr(-1023)")
print("  fahre(999)")
print("  fahre(999, 888)")
print("  fahre(999, 888, 777, 666)")
print("  halt()")