import numpy as np
import time
import tor.client.ClientSettings as cs
from tor.base.DieRecognizer import DieRecognizer
from tor.client.Camera import Camera
from tor.client.MovementManager import MovementManager
from tor.client.MovementRoutines import MovementRoutines
from tor.client.LedManager import LedManager
parser = argparse.ArgumentParser()
parser.add_argument("-t", dest="theater", action="store_true")
parser.add_argument("-all", dest="all", action="store_true")
args = parser.parse_args()
mm = MovementManager()
lm = LedManager()
mr = MovementRoutines()
print("TOR-Marlin v{} installed, v{} required.".format(mm.torMarlinVersion,
cs.TOR_MARLIN_VERSION))
if not mm.hasCorrectVersion:
exit(0)
print("Test top led ...")
for i in range(0, 256, 5):
mm.setTopLed(i)
time.sleep(0.1)
mm.setTopLed(0)
print("Test led strip ...")
print(welcomeMessage.format(cm.clientId, cm.clientIdentity["IP"], cm.clientIdentity["Material"], cm.clientIdentity["Position"]))
print("#######################")
### load custom settings from file and server
ccsModuleName = "tor.client.CustomClientSettings." + cm.clientIdentity["Material"]
try:
import importlib
customClientSettings = importlib.import_module(ccsModuleName)
print("Custom config file loaded.")
except:
print("No CustomClientSettings found.")
cm.loadSettings()
cm.loadMeshpoints()
mm = MovementManager()
mr = MovementRoutines()
lm = LedManager()
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()
import argparse
import tor.client.ClientSettings as cs
parser = argparse.ArgumentParser()
parser.add_argument("color", nargs='*', default=[255, 255, 255], type=int)
parser.add_argument("-b", dest="brightness", default=255, type=int)
parser.add_argument("-t", dest="topLed", action="store_true")
parser.add_argument("-s", dest="segment", default="RLB")
args = parser.parse_args()
if args.topLed:
from tor.client.MovementManager import MovementManager
mm = MovementManager()
mm.setTopLed(args.brightness)
else:
from tor.client.LedManager import LedManager
lm = LedManager(args.brightness)
r = args.color[0]
g = args.color[1]
b = args.color[2]
if "L" in args.segment:
leds = cs.LEDS_LEFT
lm.setLeds(leds, r, g, b)
if "R" in args.segment:
leds = cs.LEDS_RIGHT
lm.setLeds(leds, r, g, b)
if "B" in args.segment:
leds = cs.LEDS_BACK
lm.setLeds(leds, r, g, b)
import importlib
customClientSettings = importlib.import_module(ccsModuleName)
print("Custom config file loaded.")
except:
print("No CustomClientSettings found.")
if cs.ON_RASPI:
cm.loadSettings()
cm.loadMeshpoints()
######################
### Initialization ###
######################
if cs.ON_RASPI:
lm = LedManager()
mm = MovementManager()
mr = MovementRoutines()
########################
### imports on raspi ###
########################
if cs.ON_RASPI:
import cv2
class CalibrationPoint(QWidget):
def __init__(self):
super().__init__()
self.Id = -1
try:
import importlib
customClientSettings = importlib.import_module(ccsModuleName)
print("Custom config file loaded.")
except:
print("No CustomClientSettings found.")
cm.loadSettings()
cm.loadMeshpoints()
######################
### Initialization ###
######################
lm = LedManager()
mm = MovementManager()
mr = MovementRoutines()
def saveCurrentView(path):
cam = Camera()
dr = DieRecognizer()
if (args.led):
lm.setAllLeds()
mm.setTopLed(cs.LED_TOP_BRIGHTNESS)
image = cam.takePicture()
cam.close()
if (args.led):
lm.clear()
mm.setTopLed(cs.LED_TOP_BRIGHTNESS_OFF)
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
ccsModuleName = "tor.client.CustomClientSettings." + cm.clientIdentity[
"Material"]
try:
import importlib
customClientSettings = importlib.import_module(ccsModuleName)
print("Custom config file loaded.")
except:
log.warning("No CustomClientSettings found.")
cm.loadSettings()
cm.loadMeshpoints()
log.setLevel(cs.LOG_LEVEL)
dr = DieRecognizer()
mm = MovementManager()
if not mm.hasCorrectVersion:
log.warning(
"Incompatible version of TOR-Marlin installed. TORClient will now quit."
)
exit(0)
mr = MovementRoutines(cm)
if cs.ON_RASPI:
try:
lm = LedManager()
except:
raise Exception("Could not create LedManager.")
####################
### main program ###
parser = argparse.ArgumentParser()
parser.add_argument("-l", dest="moveLeft", action="store_true")
parser.add_argument("-off", dest="motorOff", action="store_true")
parser.add_argument("-all", dest="all", action="store_true")
args = parser.parse_args()
def move(direction):
mm.sendGCode("M17")
zeroPos = Cords([0, 0, 0, 0])
mm.__setCurrentPosition(zeroPos)
dest = 200 * direction
pos = Cords([0, 0, dest, dest])
mm.__moveToCords(pos)
mm = MovementManager()
time.sleep(0.5)
mm.setFeedratePercentage(500)
if args.all or not args.motorOff:
move(1)
if args.moveLeft or args.all:
move(-1)
if args.motorOff or args.all:
mm.sendGCode("M18")
from tor.client.Cords import Cords
from tor.client.LedManager import LedManager
from tor.client.MovementManager import MovementManager
from tor.client.Position import Position
mode = 18
if len(sys.argv) > 1:
mode = int(sys.argv[1])
print("mode: ", mode)
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
def __init__(self, cm=None):
self.cm = cm
self.mm = MovementManager()
self.dr = DieRecognizer()
class MovementRoutines:
def __init__(self, cm=None):
self.cm = cm
self.mm = MovementManager()
self.dr = DieRecognizer()
def loadPoints(self):
return np.concatenate((cs.MESH_BED, cs.MESH_RAMP))
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 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()
def pickupDieFromPosition(self, pos):
log.debug("die position: {}".format(pos))
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
self.mm.moveToPos(cs.BEFORE_PICKUP_POSITION, True)
self.mm.waitForMovementFinished()
pickupPos = self.relativeBedCoordinatesToPosition(pos.x, pos.y)
if pickupPos.y > cs.RAMP_CRITICAL_Y:
pickupPos.z += cs.EXTRA_Z_FOR_PICKUP
log.debug("pickupPos: {}".format(pickupPos))
#move to pick-up position
if pickupPos.y < cs.RAMP_CRITICAL_Y:
self.mm.moveCloseToRamp(pickupPos, segmented=True)
else:
self.mm.moveToPos(pickupPos, True)
self.mm.waitForMovementFinished()
time.sleep(cs.WAIT_ON_PICKUP_POS)
#move away from pick-up position
if pickupPos.y < cs.RAMP_CRITICAL_Y:
self.mm.moveCloseToRamp(cs.AFTER_PICKUP_POSITION,
segmented=True,
moveto=False)
else:
self.mm.moveToPos(cs.AFTER_PICKUP_POSITION, True)
self.mm.waitForMovementFinished()
def takePicture(self, cam=None):
if cam is None:
cam = Camera()
log.info("turn on top LED")
self.mm.setTopLed(cs.LED_TOP_BRIGHTNESS)
image = cam.takePicture()
self.dr.writeImage(image, "test.jpg", directory=cs.WEB_DIRECTORY)
log.info("turn off top LED")
self.mm.setTopLed(cs.LED_TOP_BRIGHTNESS_OFF)
cam.close()
return image
'''take a picture and locate the die'''
def findDie(self, cam=None):
image = self.takePicture(cam)
dieRollResult, processedImages = self.dr.getDieRollResult(
image, returnOriginalImg=True)
return dieRollResult, processedImages
'''
take a picture and locate the die while homing is performed
INFO: homing and camera can not be done in concurrent threads because both operations
need to access the SKR board (G28 for homing and M42 for top LED)
'''
def findDieWhileHoming(self):
log.info("do homing mode 2")
self.mm.doHoming(mode=2)
time.sleep(cs.WAIT_BEFORE_TAKE_PICTURE_WHILE_HOMING)
cam = Camera(doWarmup=True)
log.info("prepare camera: {}".format(cam))
log.info("take picture while homing...")
image = self.takePicture(cam)
#log.info("image {}".format(image))
log.info("do homing mode 3")
self.mm.doHoming(mode=3)
dieRollResult, processedImages = self.dr.getDieRollResult(
image, returnOriginalImg=True)
log.info("result: {}".format(dieRollResult))
return dieRollResult, processedImages
def pickupDieWhileHoming(self):
dieRollResult, processedImages = self.findDieWhileHoming()
self.mm.waitForMovementFinished()
self.mm.moveToPosAfterHoming(cs.CENTER_TOP, True)
if dieRollResult.found:
self.pickupDieFromPosition(dieRollResult.position)
return dieRollResult
def pickupDie_takeImage(self, cam=None):
dieRollResult = DieRollResult()
if cs.USE_IMAGE_RECOGNITION:
dieRollResult, processedImages = self.findDie(cam)
log.info("result: {}".format(dieRollResult.result))
if cs.STORE_IMAGES:
directory = "found" if dieRollResult.found else "fail"
self.dr.writeImage(processedImages[1], directory=directory)
self.dr.writeImage(processedImages[0],
directory=cs.WEB_DIRECTORY,
fileName='current_view.jpg')
self.dr.writeRGBArray(processedImages[0], directory=directory)
return dieRollResult
def pickupDie_pickup(self, dieRollResult, onSendResult=None):
if dieRollResult.found:
log.info("dieRollResult: {}".format(dieRollResult))
if cs.SHOW_DIE_RESULT_WITH_LEDS:
lm = LedManager()
lm.showResult(dieRollResult.result)
if onSendResult is not None:
onSendResult(dieRollResult)
self.pickupDieFromPosition(dieRollResult.position)
else:
log.info('Die not found, now searching...')
self.searchForDie()
def pickupDie(self, onSendResult=None, cam=None):
dieRollResult = self.pickupDie_takeImage(cam)
self.pickupDie_pickup(dieRollResult, onSendResult)
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 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 getDropoffPosByPercent(self, percent, invert=False):
dropoffPos = cs.MESH_MAGNET[3, :]
percent = np.clip(percent, 0.0, 1.0)
if invert:
px = np.clip(1 - percent, 0.0, 1.0)
else:
px = percent
if px < 0.5:
if not cs.USE_MAGNET_BETWEEN_P0P1:
px = 1 - px
elif not cs.USE_MAGNET_BETWEEN_P2P3:
px = 1 - px
if cs.ALWAYS_USE_PX >= 0 and cs.ALWAYS_USE_PX <= 3:
#log.info("use fixed magnet point")
dropoffPos = cs.MESH_MAGNET[cs.ALWAYS_USE_PX, :]
else:
if px < 0.5:
dropoffPos = 2 * px * cs.MESH_MAGNET[1, :] + (
1 - 2 * px) * cs.MESH_MAGNET[0, :]
else:
dropoffPos = 2 * (px - 0.5) * cs.MESH_MAGNET[3, :] + 2 * (
1 - px) * cs.MESH_MAGNET[2, :]
return dropoffPos
def getDropoffPosByXCoordinate(self, x, invert=False):
percent = x / cs.LX
return self.getDropoffPosByPercent(percent, invert)
def run(self, lastPickupX, onSendResult=None):
# move to dropoff position
dropoffPos = self.getDropoffPosByPercent(lastPickupX, invert=True)
self.moveToDropoffPosition(dropoffPos)
#TODO: why is this not working?
#cam = Camera(doWarmup=False)
# roll die
time.sleep(cs.WAIT_BEFORE_ROLL_TIME)
self.mm.rollDie()
time.sleep(cs.DIE_ROLL_TIME / 2.0)
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
self.mm.moveToPos(cs.CENTER_TOP, True)
time.sleep(cs.DIE_ROLL_TIME / 2.0)
def doQuickRoll(self):
dropoffPos = self.getDropoffPosByPercent(0.1, invert=True)
self.moveToDropoffPosition(dropoffPos)
time.sleep(cs.WAIT_BEFORE_ROLL_TIME)
self.mm.rollDie()
time.sleep(cs.WAIT_BEFORE_ROLL_TIME)
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
self.mm.moveToPos(cs.CENTER_TOP, True)
def rollDie(self, dropoffPos):
self.moveToDropoffPosition(dropoffPos)
time.sleep(cs.WAIT_BEFORE_ROLL_TIME)
self.mm.rollDie()
time.sleep(cs.DIE_ROLL_TIME / 2.0)
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
self.mm.moveToPos(cs.CENTER_TOP, True)
self.mm.waitForMovementFinished()
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
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 doTestPerformance(self, startTime):
log.info("start performance")
startTimestamp = datetime.timestamp(startTime)
timings = np.cumsum([0, 2, 6.5, 3, 20, 2])
timestamps = [t + startTimestamp for t in timings]
log.info(timestamps)
lm = LedManager()
step = 0
Utils.sleepUntilTimestampIndex(step, timestamps)
for i in range(15):
lm.setLeds(range(0, i * 5), 255, 255, 255)
time.sleep(0.05)
lm.clear()
time.sleep(0.05)
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
for i in range(10):
lm.setLeds(range(0, i * 5), i * 10, i * 3, i)
time.sleep(0.5)
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
self.mm.moveToPos(cs.BEFORE_PICKUP_POSITION, True)
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
self.searchForDie()
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
self.mm.moveToPos(cs.BEFORE_PICKUP_POSITION, True)
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
def doPositionTestWithTiming(self, startTime, clientIdentity, x, y, z):
log.info("preparing for performance...")
lm = LedManager()
lm.clear()
self.mm.setTopLed(cs.LED_TOP_BRIGHTNESS_OFF)
log.info("waiting for performance to start...")
startTimestamp = datetime.timestamp(startTime)
timings = np.cumsum([
clientIdentity["Position"] * 0.2, # wait for position to be next
27 * 0.2 + 1
]) # light up
timestamps = [t + startTimestamp for t in timings]
log.info(timestamps)
step = 0
Utils.sleepUntilTimestampIndex(step, timestamps)
p = int(clientIdentity["Position"])
f = x + y + z
r = cs.LED_STRIP_DEFAULT_COLOR[0] + 10 * f
g = cs.LED_STRIP_DEFAULT_COLOR[1] - 8 * f
b = cs.LED_STRIP_DEFAULT_COLOR[2]
lm.setAllLeds(r=r, g=g, b=b)
step += 1
Utils.sleepUntilTimestampIndex(step, timestamps)
lm.clear()
def doLightTest(self, startTime):
log.info("preparing for performance...")
lm = LedManager()
lm.clear()
self.mm.setTopLed(cs.LED_TOP_BRIGHTNESS_OFF)
log.info("waiting for performance to start...")
startTimestamp = datetime.timestamp(startTime)
timings = np.cumsum([0])
timestamps = [t + startTimestamp for t in timings]
log.info(timestamps)
step = 0
Utils.sleepUntilTimestampIndex(step, timestamps)
for r in range(0, 150, 10):
for g in range(0, 150, 10):
for b in range(0, 150, 10):
lm.setAllLeds(r=r, g=g, b=b)
log.info("r={}, g={}, b={}".format(r, g, b))
time.sleep(1)
def doRollDie(self, startTime):
log.info("preparing for performance...")
log.info("waiting for performance to start...")
startTimestamp = datetime.timestamp(startTime)
timings = np.cumsum([0])
timestamps = [t + startTimestamp for t in timings]
log.info(timestamps)
step = 0
Utils.sleepUntilTimestampIndex(step, timestamps)
dropoffPos = cs.MESH_MAGNET[2, :]
self.moveToDropoffPosition(dropoffPos)
# roll die
time.sleep(cs.WAIT_BEFORE_ROLL_TIME)
self.mm.rollDie()
time.sleep(cs.DIE_ROLL_TIME / 2.0)
self.mm.setFeedratePercentage(cs.FR_DEFAULT)
self.mm.moveToPos(cs.CENTER_TOP, True)
time.sleep(cs.DIE_ROLL_TIME / 2.0)