import meControll

arm = meControll.meArm()


arm.getPos()


getCube1 = [b'S1', b'T50', b'R100', b'L50', b'R120', b'L30', b'R140', b'G180', b'R100']
getCube2 = [b'S1', b'T50', b'R100', b'L50', b'R120', b'L30', b'R140', b'L20', b'R160',  b'G180', b'R100']
gotoDropoff = [b'S1', b'T140', b'R100', b'L40', b'R145', b'G90', b'R100',  b'D1']
gotoDropoff2 = [b'S1', b'T140', b'R100', b'L50', b'R133', b'G90', b'R100',  b'D1']

gotoCubePickup0 = getCube1[0:-2]

saveHome = [b'S0', b'R-30', b'D1', b'S1']

# getCube2 = [b'S1', b'T140', b'R100', b'L40', b'R145', b'G180', b'R100',  b'D1']

arm.addMove(getCube1)
arm.addMove(gotoDropoff)
arm.addMove(getCube2)
arm.addMove(gotoDropoff2)
arm.addMove(gotoCubePickup0)
arm.addMove(saveHome)
print(f"Homing")
arm.execMove(5)
print(f"Move0: {arm.execMove(0)} moves")
print(f"Move1: {arm.execMove(1)} moves")
print(f"Move2: {arm.execMove(2)} moves")
print(f"Move3: {arm.execMove(3)} moves")
print(f"Move4: {arm.execMove(4)} moves")
# arm.execMove(2)