def start_script(last_pos):
#pos=mm.getCurrentPosition()
pos=last_pos
#if(pos.y<cs.LY/2.):
mm.moveToPos(cs.CENTER_TOP)
mm.waitForMovementFinished()
px = 1-pos.x / cs.LX
if(px<0.5): spos=2*px*cs.MESH_MAGNET[1,:]+(1-2*px)*cs.MESH_MAGNET[0,:]
if(px>=0.5): spos= 2*(px-0.5)*cs.MESH_MAGNET[3,:]+2*(1-px)*cs.MESH_MAGNET[2,:]
#spos = cs.MESH_MAGNET[np.random.randint(0, 4), :]
#mm.waitForMovementFinished()
mm.moveToPos(Position(spos[0], spos[1]+cs.DROPOFF_ADVANCE_OFFSET_Y, cs.DROPOFF_ADVANCE_Z), True)
mm.waitForMovementFinished()
mm.setFeedratePercentage(50)
mm.moveToPos(Position(spos[0], spos[1]+10, spos[2]+10), True)
mm.waitForMovementFinished()
mm.setFeedratePercentage(30)
mm.moveToPos(Position(spos[0], spos[1], spos[2]), True)
mm.waitForMovementFinished()
mm.setFeedratePercentage(450)
time.sleep(1)
mm.rollDie()
if (args.find):
time.sleep(2)
dieRollResult = mr.pickupDie()
return dieRollResult
def getDropoffAdvancePosition(self, pos, stage=1):
if stage == 1:
p = Position(pos[0], pos[1] + cs.DROPOFF_ADVANCE_OFFSET_Y,
pos[2] + cs.DROPOFF_ADVANCE_OFFSET_Z)
elif stage == 2:
p = Position(pos[0], pos[1] + cs.DROPOFF_ADVANCE_OFFSET_Y2,
pos[2] + cs.DROPOFF_ADVANCE_OFFSET_Z2)
return p
def moveCloseToRamp(self, pos, segmented=False, moveto=True):
if moveto:
self.moveToPos(Position(pos.x,pos.y+cs.CRITICAL_AREA_APPROACH_Y,pos.z-cs.CRITICAL_AREA_APPROACH_Z), useSlowDownEnd=False, segmented=segmented)
self.moveToPos(pos,useSlowDownStart=False, segmented=segmented)
else:
p0 = MovementManager.currentPosition
self.moveToPos(Position(p0.x, p0.y + cs.CRITICAL_AREA_APPROACH_Y, p0.z - cs.CRITICAL_AREA_APPROACH_Z), useSlowDownEnd=False, segmented=segmented)
self.moveToPos(pos,useSlowDownStart=False, segmented=segmented)
def __init__(self):
self.com = Communicator()
self.feedratePercentage = 0
MovementManager.currentPosition = Position(-1, -1, -1)
if not MovementManager.isInitialized:
self.torMarlinVersion = "X"
self.hasCorrectVersion = self.checkTORMarlinVersion()
self.__initBoard()
MovementManager.isInitialized = True
def move(self, direction, steps):
posFrom = mm.currentPosition
posTo = None
deltaPos = None
if direction == "X":
deltaPos = Position(steps, 0, 0)
elif direction == "Y":
deltaPos = Position(0, steps, 0)
elif direction == "Z":
deltaPos = Position(0, 0, steps)
posTo = posFrom + deltaPos
if posTo is not None:
posTo.x = Utils.clamp(posTo.x, MANUAL_MOVEMENT_X_MIN,
MANUAL_MOVEMENT_X_MAX)
posTo.y = Utils.clamp(posTo.y, MANUAL_MOVEMENT_Y_MIN,
MANUAL_MOVEMENT_Y_MAX)
posTo.z = Utils.clamp(posTo.z, MANUAL_MOVEMENT_Z_MIN,
MANUAL_MOVEMENT_Z_MAX)
mm.moveToPos(posTo, True)
mm.waitForMovementFinished()
def toPosition(self):
print("WARNING: cord modification will not be used!")
print(" Cords: {}".format(self))
from tor.client.Position import Position #to avoid circular import - find better import strategy!
x = cs.LX - round(
(cs.LX**2 + self.lengths[0]**2 - self.lengths[1]**2) /
(2.0 * cs.LX))
y = cs.LY - round(
(cs.LY**2 + self.lengths[0]**2 - self.lengths[3]**2) /
(2.0 * cs.LY))
z = math.sqrt(max(self.lengths[0]**2 - x**2 - y**2, 0))
pos = Position(x, y, z)
print(" Position: {}".format(pos))
return pos
def moveToDropoffPosition(self,
dropoffPos,
speedupFactor1=1,
speedupFactor2=1):
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
if not isinstance(dropoffPos, Position):
dropoffPos = Position(dropoffPos[0], dropoffPos[1], dropoffPos[2])
dropoffAdvancePos = self.getDropoffAdvancePosition(dropoffPos, stage=1)
dropoffAdvancePos2 = self.getDropoffAdvancePosition(dropoffPos,
stage=2)
self.mm.moveToPos(dropoffAdvancePos, True)
self.mm.setFeedratePercentage(cs.FR_DROPOFF_ADVANCE * speedupFactor1)
self.mm.moveToPos(dropoffAdvancePos2, True)
self.mm.setFeedratePercentage(cs.FR_DROPOFF_ADVANCE_SLOW *
speedupFactor2)
self.mm.moveToPos(dropoffPos, True)
self.mm.waitForMovementFinished()
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
def moveToMagnetPoint(self, id, x, y, z):
self.addStatusText("Test point {}, move to position ({},{},{})".format(
id, x, y, z),
spacerLineBefore=True)
if cs.ON_RASPI:
pos = Position(x, y, z)
centerX = cs.LX / 2
if pos.x < centerX and mm.currentPosition.x > centerX or pos.x > centerX and mm.currentPosition.x < centerX:
mm.moveToPos(cs.BEFORE_PICKUP_POSITION, True)
mm.waitForMovementFinished()
mr.moveToDropoffPosition(pos)
mm.waitForMovementFinished()
time.sleep(2)
mm.rollDie()
time.sleep(0.1)
else:
time.sleep(2)
self.addStatusText("reached position ({},{},{})".format(x, y, z),
spacerLineAfter=True)
def moveToBedPoint(self, id, x, y, z, isCloseToRamp):
self.addStatusText("Test point {}, move to position ({},{},{})".format(
id, x, y, z),
spacerLineBefore=True)
if cs.ON_RASPI:
pos = Position(x, y, z)
if isCloseToRamp:
mm.moveCloseToRamp(cs.BEFORE_PICKUP_POSITION,
True,
moveto=False)
mm.waitForMovementFinished()
mm.moveCloseToRamp(pos, True)
mm.waitForMovementFinished()
else:
mm.moveToPos(pos, True)
mm.waitForMovementFinished()
else:
time.sleep(2)
self.addStatusText("reached position ({},{},{})".format(x, y, z),
spacerLineAfter=True)
def performUserAction(self, action, steps):
if action != "NONE":
log.info("perform action: {},{}".format(action, steps))
posFrom = self.mm.currentPosition
posTo = None
try:
steps = int(steps)
except ValueError:
steps = 0
if action == "DOWN":
log.info("move down")
posTo = posFrom + Position(0, 0, int(steps))
elif action == "UP":
log.info("move up")
posTo = posFrom + Position(0, 0, -int(steps))
elif action == "LEFT":
log.info("move left")
posTo = posFrom + Position(int(steps), 0, 0)
elif action == "RIGHT":
log.info("move right")
posTo = posFrom + Position(-int(steps), 0, 0)
elif action == "FRONT":
log.info("move front")
posTo = posFrom + Position(0, int(steps), 0)
elif action == "BACK":
log.info("move back")
posTo = posFrom + Position(0, -int(steps), 0)
elif action == "ROLL":
log.info("roll die")
self.mm.moveToPos(cs.AFTER_PICKUP_POSITION, True)
dropoffPosPercent = int(steps)
dropoffPos = self.getDropoffPosByPercent(
1.0 - (dropoffPosPercent / 100.0), invert=False)
#dropoffPos = cs.MESH_MAGNET[2]
self.rollDie(dropoffPos)
dieRollResult, processedImages = self.findDie()
if dieRollResult.found:
log.info("dieRollResult: {}".format(dieRollResult))
self.cm.sendDieRollResult(dieRollResult, userGenerated=True)
else:
log.info("die not found...")
else:
#log.warning("Action {} not known.".format(action))
time.sleep(0.7 * cs.ASK_EVERY_NTH_SECOND_FOR_JOB_USERMODE)
if posTo is not None:
validPos = self.getValidUserPosition(posTo)
self.mm.moveToPos(validPos, True)
self.mm.waitForMovementFinished()
def relativeBedCoordinatesToPosition(self, px, py):
p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11, p12 = self.loadPoints()
#TODO: @David check the out of range logic. die was found correctly at y=1.002
# why is y < 0 allowed? is the calculation correct for py < 0?
# for now just clip
minX = -0.1
maxX = 1.1
minY = -0.1
maxY = 1.1
if (px < minX or px > maxX or py < minY or py > maxY):
log.warning(
"Out of range in relativeBedCoordinatesToPosition: [x,y]=[{},{}]"
.format(px, py))
px = np.clip(px, minX, maxX)
py = np.clip(py, minY, maxY)
if (px < 0.5):
x = (1 - py) * (p4[0] + px *
(p6[0] - p4[0])) + py * (p1[0] + px *
(p3[0] - p1[0]))
y = (1 - 2 * px) * (p4[1] + py *
(p1[1] - p4[1])) + 2 * px * (p5[1] + py *
(p2[1] - p5[1]))
z = (1 - 2 * px) * ((1 - py) * p4[2] + py * p1[2]) + 2 * px * (
(1 - py) * p5[2] + py * p2[2])
else:
x = (1 - py) * (p6[0] + (1 - px) *
(p4[0] - p6[0])) + py * (p3[0] + (1 - px) *
(p1[0] - p3[0]))
y = 2 * (1 - px) * (p5[1] + py *
(p2[1] - p5[1])) + (2 * px -
1) * (p6[1] + py *
(p3[1] - p6[1]))
z = 2 * (1 - px) * (
(1 - py) * p5[2] + py * p2[2]) + (2 * px - 1) * (
(1 - py) * p6[2] + py * p3[2])
return Position(x, y, z)
def calibrateMeshpoints(type, p, pointsToCalibrate=None):
if pointsToCalibrate is None:
pointsToCalibrate = range(len(p))
elif not isinstance(pointsToCalibrate, list):
pointsToCalibrate = [pointsToCalibrate]
if type == "B" or type == "R":
for i in pointsToCalibrate:
finish = False
while not finish:
pos = Position(p[i, 0], p[i, 1], p[i, 2])
if type == 'B' and i > 2:
mm.moveToPos(cs.BEFORE_PICKUP_POSITION, True)
mm.waitForMovementFinished()
mm.moveCloseToRamp(pos, True)
mm.waitForMovementFinished()
else:
mm.moveToPos(pos, True)
mm.waitForMovementFinished()
if ((type == "B") and args.take_picture):
saveCurrentView(cs.WEB_DIRECTORY)
print("http://" + cm.clientIdentity["IP"])
print('Position OK? (y/n)')
if ((input() or 'y') == 'y'):
finish = True
else:
print('Current position')
print(pos)
print('New position')
p[i, 0] = input('x:') or p[i, 0]
p[i, 1] = input('y:') or p[i, 1]
p[i, 2] = input('z:') or p[i, 2]
if type == 'B' and i > 2:
mm.moveCloseToRamp(cs.BEFORE_PICKUP_POSITION,
segmented=True,
moveto=False)
mm.waitForMovementFinished()
elif type == "M":
for i in pointsToCalibrate:
print('Searching point', i + 1)
finish = False
while not finish:
#mm.setFeedratePercentage(50)
#mm.moveToPos(Position(p[i, 0], p[i, 1] + cs.DROPOFF_ADVANCE_OFFSET_Y, p[i, 2] + cs.DROPOFF_ADVANCE_OFFSET_Z))
#mm.waitForMovementFinished()
#mm.setFeedratePercentage(30)
#mm.moveToPos(Position(p[i, 0], p[i, 1], p[i, 2]))
#mm.waitForMovementFinished()
# mm.setFeedratePercentage(30)
mr.moveToDropoffPosition(Position(p[i, 0], p[i, 1], p[i, 2]))
mm.waitForMovementFinished()
time.sleep(2)
mm.rollDie()
print('Position OK? (y/n/c)')
answ = input() or 'y'
if (answ == 'y'):
finish = True
cm.saveMeshpoints("M", p)
mm.setFeedratePercentage(200)
mm.moveToPos(
Position(p[i,
0], p[i, 1] + cs.DROPOFF_ADVANCE_OFFSET_Y,
p[i, 2] + cs.DROPOFF_ADVANCE_OFFSET_Z), True)
mm.waitForMovementFinished()
mr.pickupDie()
mm.waitForMovementFinished()
elif (answ == 'n'):
print('Current position:', MovementManager.currentPosition)
mm.moveToPos(
Position(p[i,
0], p[i, 1] + cs.DROPOFF_ADVANCE_OFFSET_Y,
p[i, 2] + cs.DROPOFF_ADVANCE_OFFSET_Z), True)
mm.waitForMovementFinished()
print('New position')
p[i, 0] = input('x:') or p[i, 0]
p[i, 1] = input('y:') or p[i, 1]
p[i, 2] = input('z:') or p[i, 2]
else:
mm.doHoming()
mm.setFeedratePercentage(cs.FR_SLOW_MOVE)
mm.moveToPos(Position(120, 100, 50))
mm.waitForMovementFinished()
print(p[i, 0], p[i, 1], p[i, 2])
mm.setFeedratePercentage(50)
print('New position')
p[i, 0] = input('x:') or p[i, 0]
p[i, 1] = input('y:') or p[i, 1]
p[i, 2] = input('z:') or p[i, 2]
else:
print("mesh type " + type + " not known.")
def doDieRollAndPickupPerformance(self, startTime):
log.info("preparing for performance...")
self.pickupDie()
lm = LedManager()
lm.clear()
self.mm.setFeedratePercentage(cs.FR_SLOW_MOVE)
#TODO: check if USE_MAGNET_BETWEEN_P0P1=True, otherwise use left side
dropoffPos = cs.MESH_MAGNET[2]
dropoffAdvancePos = Position(
dropoffPos[0], dropoffPos[1] + cs.DROPOFF_ADVANCE_OFFSET_Y,
dropoffPos[2] + cs.DROPOFF_ADVANCE_OFFSET_Z)
self.mm.moveToPos(dropoffAdvancePos, True)
fr_factor = 2
fr_default_old = cs.FR_DEFAULT
cs.FR_DEFAULT = cs.FR_DEFAULT * fr_factor
log.info("in starting position...")
log.info("waiting for performance to start...")
startTimestamp = datetime.timestamp(startTime)
timings = np.cumsum([
0,
0.25, # turn on leds
6.0, # move to dropoff position
2, # roll die and mvoe to cs.CENTER_TOP
0.9 + 4.2, # take picture
0.3, # move to cs.BEFORE_PICKUP_POSITION
5.1, # find die on image
4.9 # pickup die
]) # move to starting position (dropoff advance position)
timestamps = [t + startTimestamp for t in timings]
log.info(timestamps)
step = 0
Utils.sleepUntilTimestampIndex(step, timestamps)
#cam = Camera(doWarmup=False)
lm.setAllLeds()
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
self.moveToDropoffPosition(cs.MESH_MAGNET[2],
speedupFactor1=1.5,
speedupFactor2=1)
#self.moveToDropoffPosition(cs.MESH_MAGNET[2], speedupFactor1=3, speedupFactor2=3)
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
self.mm.rollDie()
time.sleep(0.4)
self.mm.moveToPos(cs.CENTER_TOP, True)
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
#image = self.takePicture(cam)
image = self.takePicture()
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
self.mm.moveToPos(cs.BEFORE_PICKUP_POSITION, True)
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
dieRollResult, _ = self.dr.getDieRollResult(image,
returnOriginalImg=True)
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
if dieRollResult.found:
self.pickupDieFromPosition(dieRollResult.position)
else:
self.searchForDie()
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
self.mm.moveToPos(dropoffAdvancePos, True)
lm.clear()
cs.FR_DEFAULT = fr_default_old
mm.moveToPos(cs.PARKING_POSITION, True)
exit(0)
if args.ramp:
# overcome ramp
mm.moveToPos(cs.AFTER_PICKUP_POSITION, True)
mm.waitForMovementFinished()
calibrateMeshpoints("R", cs.MESH_RAMP, args.points)
cm.saveMeshpoints("R", cs.MESH_RAMP)
mm.moveToPos(cs.PARKING_POSITION, True)
exit(0)
if args.magnet:
mm.setFeedratePercentage(300)
mm.moveToPos(Position(120, 100, 100), True)
mm.waitForMovementFinished()
cm.loadMeshpoints()
calibrateMeshpoints("M", cs.MESH_MAGNET, args.points)
cm.saveMeshpoints("M", cs.MESH_MAGNET)
exit(0)
if args.camera:
finish = False
dr = DieRecognizer()
cm.loadSettings()
lm.setAllLeds()
mm.setTopLed(cs.LED_TOP_BRIGHTNESS)
def moveToXYPosDie(self, x, y, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
pos = Position(x, y, cs.PICKUP_Z)
self.moveToPos(pos, segmented, useSlowDownStart, )
def moveToXPosRamp(self, x, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
x = min(max(x, cs.RAMP_FORBIDDEN_X_MIN), cs.RAMP_FORBIDDEN_X_MAX)
pos = Position(x, cs.RAMP_DROPOFF_Y, cs.RAMP_DROPOFF_Z)
self.moveToPos(pos, segmented, useSlowDownStart, )
def searchForDie(self):
#starting position
'''
1 2 3
4 5 6
--------
7 8 9
10 11 12
M M
'''
p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11, p12 = self.loadPoints()
n_rows = 4 # rows per area
self.mm.moveToPos(Position(p1[0], p1[1], 100), True)
self.mm.moveToPos(Position(p1[0], p1[1], p1[2]), True)
self.mm.waitForMovementFinished()
#raster bottom
# mesh left side
x_mesh_l = np.linspace(p1[0], p4[0], n_rows)
y_mesh_l = np.linspace(p1[1], p4[1], n_rows)
z_mesh_l = np.linspace(p1[2], p4[2], n_rows)
#mesh center
x_mesh_c = np.linspace(p2[0], p5[0], n_rows)
y_mesh_c = np.linspace(p2[1], p5[1], n_rows)
z_mesh_c = np.linspace(p2[2], p5[2], n_rows)
#mesh right side
x_mesh_r = np.linspace(p3[0], p6[0], n_rows)
y_mesh_r = np.linspace(p3[1], p6[1], n_rows)
z_mesh_r = np.linspace(p3[2], p6[2], n_rows)
self.mm.setFeedratePercentage(cs.FR_SEARCH_BED)
for i in range(n_rows):
if (i % 2 == 0):
self.mm.moveToPos(
Position(x_mesh_l[i], y_mesh_l[i], z_mesh_l[i]), True)
#self.mm.waitForMovementFinished()
self.mm.moveToPos(Position(x_mesh_c[i], y_mesh_c[i],
z_mesh_c[i]),
True,
useSlowDownEnd=False)
#self.mm.waitForMovementFinished()
self.mm.moveToPos(Position(x_mesh_r[i], y_mesh_r[i],
z_mesh_r[i]),
True,
useSlowDownStart=False)
self.mm.waitForMovementFinished()
else:
self.mm.moveToPos(
Position(x_mesh_r[i], y_mesh_r[i], z_mesh_r[i]), True)
#self.mm.waitForMovementFinished()
self.mm.moveToPos(Position(x_mesh_c[i], y_mesh_c[i],
z_mesh_c[i]),
True,
useSlowDownEnd=False)
#self.mm.waitForMovementFinished()
self.mm.moveToPos(Position(x_mesh_l[i], y_mesh_l[i],
z_mesh_l[i]),
True,
useSlowDownStart=False)
self.mm.waitForMovementFinished()
self.mm.moveToPos(
Position(
MovementManager.currentPosition.x,
MovementManager.currentPosition.y +
cs.CRITICAL_AREA_APPROACH_Y,
MovementManager.currentPosition.z -
cs.CRITICAL_AREA_APPROACH_Z), True)
#### ramp ###
if (cs.SEARCH_RAMP):
#safely move away from ramp
self.mm.moveToPos(
Position(MovementManager.currentPosition.x,
MovementManager.currentPosition.y + 20, 100), True)
self.mm.waitForMovementFinished()
# mesh left side
x_mesh_l = np.linspace(p7[0], p10[0], n_rows)
y_mesh_l = np.linspace(p7[1], p10[1], n_rows)
z_mesh_l = np.linspace(p7[2], p10[2], n_rows)
#mesh center
x_mesh_c = np.linspace(p8[0], p11[0], n_rows)
y_mesh_c = np.linspace(p8[1], p11[1], n_rows)
z_mesh_c = np.linspace(p8[2], p11[2], n_rows)
#mesh right side
x_mesh_r = np.linspace(p9[0], p12[0], n_rows)
y_mesh_r = np.linspace(p9[1], p12[1], n_rows)
z_mesh_r = np.linspace(p9[2], p12[2], n_rows)
self.mm.setFeedratePercentage(cs.FR_SEARCH_RAMP)
for i in range(n_rows):
if (i % 2 == 0):
self.mm.moveToPos(
Position(x_mesh_l[i], y_mesh_l[i], z_mesh_l[i]), True)
self.mm.moveToPos(
Position(x_mesh_c[i], y_mesh_c[i], z_mesh_c[i]), True)
self.mm.moveToPos(
Position(x_mesh_r[i], y_mesh_r[i], z_mesh_r[i]), True)
self.mm.waitForMovementFinished()
else:
self.mm.moveToPos(
Position(x_mesh_r[i], y_mesh_r[i], z_mesh_r[i]), True)
self.mm.moveToPos(
Position(x_mesh_c[i], y_mesh_c[i], z_mesh_c[i]), True)
self.mm.moveToPos(
Position(x_mesh_l[i], y_mesh_l[i], z_mesh_l[i]), True)
self.mm.waitForMovementFinished()
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
self.mm.moveToPos(cs.AFTER_PICKUP_POSITION, True)
self.mm.waitForMovementFinished()
try:
if mode == 17:
from tor.client.Camera import Camera
if mode != 17 and mode != 18 and mode != 15 and mode != 11 and mode != 12 and mode != 10:
print("init board...")
mm = MovementManager()
time.sleep(0.5)
else:
mm = None
except:
mm = None
print("ERROR: could not connect to SKR board.")
if mode == 0:
diePosition = Position(100, 30, 210)
mm.moveToPos(cs.HOME_POSITION)
mm.moveToPos(diePosition)
mm.moveToPos(cs.HOME_POSITION)
dieX = 120
dieY = 160
mm.moveToXYPosDie(dieX, dieY)
mm.waitForMovementFinished(0.5)
mm.moveToXPosRamp(dieX)
elif mode == 1: #move to x y and pick up die
dieX = int(sys.argv[2])
dieY = int(sys.argv[3])
mm.moveToXYPosDie(dieX, dieY)
mm.waitForMovementFinished(0.5)
def moveToXYZ(self, x, y, z, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
pos = Position(x, y, z)
self.moveToPos(pos, segmented, useSlowDownStart, )
def getValidUserPosition(self, pos):
validPos = Position(clamp(pos.x, 0, cs.LX), clamp(pos.y, 160, cs.LY),
clamp(pos.z, 50, 220))
return validPos
dieRollResult.position=Point2D(0,0)
while(True):
i+=1
if (err == 3): # prevents bad runs
err = 0
i = 1
mm.doHoming()
mr.searchForDie()
result_old = dieRollResult.result
dieRollResult = start_script(dieRollResult.position)
print(dieRollResult.result)
if ((not dieRollResult.found) or (result_old == dieRollResult.result)):
err += 1
else:
err = 0
if(args.cal_on_run > 0):
if(np.mod(i,args.cal_on_run)==0):
mm.doHoming()
mm.waitForMovementFinished()
else:
start_script(Position(100,100,100))
exit(0)
pos = Position(args.position[0], args.position[1], args.position[2])
if pos.isValid():
mm.moveToPos(pos, args.segmented, useSlowDownStart=(not args.a), useSlowDownEnd=(not args.b))
LMAX = math.sqrt(LX**2 + LY**2 + LZ**2) RAMP_FORBIDDEN_X_MIN = 50 RAMP_FORBIDDEN_X_MAX = LX - 50 RAMP_DROPOFF_Y = 0 RAMP_DROPOFF_Z = 0 RAMP_END_Y = 160#115 RAMP_END_Z = LZ - 65 RAMP_START_Z = 50 RAMP_ALPHA = np.arctan((RAMP_END_Z - RAMP_START_Z) / RAMP_END_Y) MAGNET_RADIUS = 12 MAGNET_HEIGHT = 50 DICE_HEIGHT = 20 PICKUP_ABOVE_GROUND = DICE_HEIGHT PICKUP_Z = LZ - MAGNET_HEIGHT - PICKUP_ABOVE_GROUND AFTER_HOMING_POSITION = Position(230, 230, 30) BEFORE_PICKUP_POSITION = Position(LX/2, 200, 50) AFTER_PICKUP_POSITION = Position(LX/2, 200, 50) PARKING_POSITION = Position(LX/2, LY/2, 50) PULSE_MAGNET_TIME_MS = 100 WAIT_BEFORE_ROLL_TIME = 0.3 DIE_ROLL_TIME = 2 WAIT_ON_PICKUP_POS = 0.2 EXTRA_Z_FOR_PICKUP = 1.0 WAIT_BEFORE_TAKE_PICTURE_WHILE_HOMING = 1 USE_LEFT_DROPOFF_REGION = True USE_RIGHT_DROPOFF_REGION = True #cord correction for increasing pulley radius
class MovementManager:
isInitialized = False
currentPosition = Position(-1, -1, -1)
def __init__(self):
self.com = Communicator()
self.feedratePercentage = 0
MovementManager.currentPosition = Position(-1, -1, -1)
if not MovementManager.isInitialized:
self.torMarlinVersion = "X"
self.hasCorrectVersion = self.checkTORMarlinVersion()
self.__initBoard()
MovementManager.isInitialized = True
def __initBoard(self):
# Restore Settings
#self.sendGCode("M501")
# Set Feedrate Percentage
self.setFeedratePercentage(cs.FR_DEFAULT)
# enable all steppers
self.sendGCode("M17")
self.__updateCurrentPosition()
self.waitForMovementFinished()
def sendGCode(self, cmd):
log.debug("SEND: {}".format(cmd))
self.com.send(cmd)
msgs = self.com.recvUntilOk()
return msgs
def setFeedratePercentage(self, fr):
self.sendGCode("M220 S{}".format(fr))
self.feedratePercentage = fr
def getCordLengthGCode(self, cords):
cmd = ""
if cords.isValid():
cmd = "X{:f} Y{:f} Z{:f} E{:f}".format(cords.lengths[0], cords.lengths[1], cords.lengths[2], cords.lengths[3])
else:
log.warning("Cords are outside boundaries: {}".format(cords.lengths))
raise Exception("Cords are outside boundaries: ", cords.lengths)
return cmd
def setCurrentPositionGCode(self, cords):
cmd = "G92 " + self.getCordLengthGCode(cords)
self.sendGCode(cmd)
def __getCurrentPosition(self):
pos = cs.HOME_POSITION
cmd = "M114"
msgs = self.sendGCode(cmd)
if not cs.ON_RASPI:
msgs = ["X:347.8965 Y:246.0000 Z:0.0000 E:246.0000 Count X:0 Y:13921 Z:19687"]
pattern = "X:(\d+\.\d+) Y:(\d+\.\d+) Z:(\d+\.\d+) E:(\d+\.\d+) Count X:\d+ Y:\d+ Z:\d+"
for msg in msgs:
match = re.match(pattern, msg)
if match:
tmpCords = Cords([float(match.group(i)) for i in range(1, 5)])
#INFO: modified cord lengths are not used here...
pos = tmpCords.toPosition()
print("new position from SKR board: {}".format(pos))
return pos
def __updateCurrentPosition(self):
MovementManager.currentPosition = self.__getCurrentPosition()
def checkTORMarlinVersion(self):
versionOkay = False
msgs = self.sendGCode("M115")
if not cs.ON_RASPI:
msgs = ["FIRMWARE_NAME:Marlin 2.0.5.3 (GitHub) TOR_VERSION:1.1 SOURCE_CODE_URL:https://github.com/MarlinFirmware/Marlin PROTOCOL_VERSION:1.0 MACHINE_TYPE:The Transparency of Randomness EXTRUDER_COUNT:1 UUID:cede2a2f-41a2-4748-9b12-c55c62f367ff"]
pattern = ".*TOR_VERSION:(\d+\.\d+).*"
for msg in msgs:
match = re.match(pattern, msg)
if match:
self.torMarlinVersion = str(match.group(1))
if self.torMarlinVersion == cs.TOR_MARLIN_VERSION:
versionOkay = True
log.info("TOR-Marlin v{} installed.".format(self.torMarlinVersion))
else:
versionOkay = False
log.error("TOR-Marlin v{} installed, but v{} required.".format(self.torMarlinVersion, cs.TOR_MARLIN_VERSION))
return versionOkay
def toggleLED(self, ledId, isOn, r=0, b=0, g=0, brightness=255):
raise Exception("LEDs are not supported")
if not isOn:
r = b = g = 0
cmd = "M150 N{} R{} U{} B{} P{}".format(ledId, r, g, b, brightness)
self.sendGCode(cmd)
def enableMagnet(self):
cmd = "M42 P41 S255"
self.sendGCode(cmd)
def disableMagnet(self):
cmd = "M42 P41 S0"
self.sendGCode(cmd)
def pulseMagnet(self):
cmd = "M43 T I S41 L41 R1 W{}".format(cs.PULSE_MAGNET_TIME_MS)
self.sendGCode(cmd)
def setTopLed(self, brightness):
if brightness < 0:
brightness = 0
elif brightness > 255:
brightness = 255
cmd = "M42 P40 S{} I".format(brightness)
self.sendGCode(cmd)
'''
Homing modes
0: full homing with repeated advances (corresponds to first mode 2 then mode 3)
1: first go to center, then mode 0. do this only when position is nearly known!
2: first tighten all cords, then move to anchor (default X) while pulling on other cords
3: do repeated advances. need to be near anchor point
'''
def doHoming(self, mode=0):
cmd = cs.G_HOMING.format(mode)
self.sendGCode(cmd)
self.waitForMovementFinished()
self.setCurrentPositionGCode(cs.HOME_POSITION.toCordLengths())
MovementManager.currentPosition = cs.HOME_POSITION
def __moveToCords(self, cords, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
cmd = "G1 " + self.getCordLengthGCode(cords) + (" S" if segmented else "") + (" A" if not useSlowDownStart else "") + (" B" if not useSlowDownEnd else "")
self.sendGCode(cmd)
def moveToPos(self, pos, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
if not isinstance(pos, list):
pos = [pos]
for p in pos:
log.debug("MOVE{}: {} {} {}".format(" SEG" if segmented else "", p.x, p.y, p.z))
cords = p.toCordLengths()
self.__moveToCords(cords, segmented, useSlowDownStart, useSlowDownEnd)
MovementManager.currentPosition = p
def moveToXYZ(self, x, y, z, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
pos = Position(x, y, z)
self.moveToPos(pos, segmented, useSlowDownStart, )
# first move to AFTER_HOMING_POSITION in a NOT segmented move
def moveToPosAfterHoming(self, pos, segmented=False):
self.setFeedratePercentage(cs.FR_SLOW_MOVE)
self.moveToPos(cs.AFTER_HOMING_POSITION, segmented=False)
self.waitForMovementFinished()
self.setFeedratePercentage(cs.FR_DEFAULT)
self.moveToPos(pos, segmented=segmented)
# moveto parameter specifies if the movement is towards the ramp, moveto=False means movement is away from ramp
def moveCloseToRamp(self, pos, segmented=False, moveto=True):
if moveto:
self.moveToPos(Position(pos.x,pos.y+cs.CRITICAL_AREA_APPROACH_Y,pos.z-cs.CRITICAL_AREA_APPROACH_Z), useSlowDownEnd=False, segmented=segmented)
self.moveToPos(pos,useSlowDownStart=False, segmented=segmented)
else:
p0 = MovementManager.currentPosition
self.moveToPos(Position(p0.x, p0.y + cs.CRITICAL_AREA_APPROACH_Y, p0.z - cs.CRITICAL_AREA_APPROACH_Z), useSlowDownEnd=False, segmented=segmented)
self.moveToPos(pos,useSlowDownStart=False, segmented=segmented)
def moveToXYPosDie(self, x, y, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
pos = Position(x, y, cs.PICKUP_Z)
self.moveToPos(pos, segmented, useSlowDownStart, )
def moveToXPosRamp(self, x, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
x = min(max(x, cs.RAMP_FORBIDDEN_X_MIN), cs.RAMP_FORBIDDEN_X_MAX)
pos = Position(x, cs.RAMP_DROPOFF_Y, cs.RAMP_DROPOFF_Z)
self.moveToPos(pos, segmented, useSlowDownStart, )
def moveToXYPosDieAndRamp(self, x, y, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
self.moveToXYPosDie(x, y, segmented, useSlowDownStart, )
self.moveToXPosRamp(x, segmented, useSlowDownStart, )
def moveHome(self, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
self.moveToPos(cs.HOME_POSITION, segmented, useSlowDownStart, )
def moveToParkingPosition(self, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
self.moveToPos(cs.PARKING_POSITION, segmented, useSlowDownStart, )
def moveToAllCorners(self, segmented=False, useSlowDownStart=True, useSlowDownEnd=True):
self.moveToPos(cs.CORNER_X, segmented, useSlowDownStart, )
time.sleep(0.5)
self.moveToPos(cs.CENTER_TOP, segmented, useSlowDownStart, )
self.moveToPos(cs.CORNER_Z, segmented, useSlowDownStart, )
time.sleep(0.5)
self.moveToPos(cs.CENTER_TOP, segmented, useSlowDownStart, )
self.moveToPos(cs.CORNER_E, segmented, useSlowDownStart, )
time.sleep(0.5)
self.moveToPos(cs.CENTER_TOP, segmented, useSlowDownStart, )
self.moveToPos(cs.CORNER_Y, segmented, useSlowDownStart, )
time.sleep(0.5)
self.moveToPos(cs.CENTER_TOP, segmented, useSlowDownStart, )
self.moveToPos(cs.CORNER_X, segmented, useSlowDownStart, )
time.sleep(0.5)
self.moveHome(segmented, useSlowDownStart, )
def rollDie(self):
log.info("die is now rolled...")
self.pulseMagnet()
def waitForMovementFinished(self, sleepTime=0):
self.sendGCode("M400")
self.sendGCode("M118 A1 move finished")
time.sleep(sleepTime)