# These macros are used by framework grab brick to
#   move 2 or 4 motors in the same time.

##############################################################################
##########                                                          ##########
##########                         MOVEMENT                         ##########
##########                                                          ##########
##############################################################################
def mvr2mot(mot1, val1, mot2, val2) '{

    printf("Moving relative \"%s\" %g    \"%s\" %g ..........", \
           mot1, val1, mot2, val2)
    waitmove ; get_angles

    mne1 = motor_num(mot1)
    mne2 = motor_num(mot2)

    aux1 = val1+0
    aux2 = val2+0

    A[mne1] += val1
    A[mne2] += val2

    move_em

    waitmove ; get_angles

    printf(" Done\n")
}'

def mvr4mot(mot1, val1, mot2, val2, mot3, val3, mot4, val4) '{

    printf("Moving reltaive (%s,%g) (%s,%g) (%s,%g) (%s,%g) ..........",  \
           mot1, val1, mot2, val2, mot3, val3, mot4, val4)
    waitmove ; get_angles

    mne1 = motor_num(mot1)
    mne2 = motor_num(mot2)
    mne3 = motor_num(mot3)
    mne4 = motor_num(mot4)

    aux1 = val1+0
    aux2 = val2+0
    aux3 = val3+0
    aux4 = val4+0

    A[mne1] += val1
    A[mne2] += val2
    A[mne3] += val3
    A[mne4] += val4

    move_em

    waitmove ; get_angles

    printf(" Done\n")
}'

def mv4mot(mot1, val1, mot2, val2, mot3, val3, mot4, val4) '{

    printf("Moving (%s,%g) (%s,%g) (%s,%g) (%s,%g) ..........",  \
           mot1, val1, mot2, val2, mot3, val3, mot4, val4)
    waitmove ; get_angles

    mne1 = motor_num(mot1)
    mne2 = motor_num(mot2)
    mne3 = motor_num(mot3)
    mne4 = motor_num(mot4)

    A[mne1] = val1
    A[mne2] = val2
    A[mne3] = val3
    A[mne4] = val4

    move_em

    waitmove ; get_angles

    printf(" Done\n")
}'