def init_positioners(self, genericPositioner=pos.Positioner()):
try:
response = self.canUSB.CAN_write(0, 'askID', [])
for i in range(0, len(response)):
self.positioners.append(copy.deepcopy(genericPositioner))
self.positioners[i].init(response[i],
self.canUSB,
waitInitComplete=False)
if len(self.positioners) < 1:
raise errors.PositionerError(
"No positioner in the bench") from None
for positioner in self.positioners:
#manually initialize each positioner's position
while not positioner.datum_initialized(self.canUSB):
time.sleep(0.005)
#Set the current position as the hardstop position
initial_position = {'Actual_alpha_pos': int(round(positioner.physics.incrementsPerRotation*positioner.model.offsetAlpha*180/np.pi/(DEFINES.DEGREES_PER_ROTATION),0)), \
'Actual_beta_pos': int(round(positioner.physics.incrementsPerRotation*positioner.model.offsetBeta*180/np.pi/(DEFINES.DEGREES_PER_ROTATION),0))}
self.canUSB.CAN_write(positioner.ID, 'set_actual_position',
initial_position)
positioner.initialized = True
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners initialization failed") from None
def calibrate_all_motors(self):
try:
# Launch motor calibration
for positioner in self.positioners:
positioner.calibrate_motor(self.canUSB)
# Wait until all the positioners finished their calibration
finishedIDs = []
while len(finishedIDs) < len(self.positioners):
for positioner in self.positioners:
if positioner.ID in finishedIDs:
continue
status = positioner.get_status(self.canUSB)
if not (status & (canUSB._OPT.STREG.MOTOR_CALIBRATION)):
finishedIDs.append(positioner.ID)
# Check the calibration went well
for positioner in self.positioners:
result = positioner.get_motor_calibration_error(self.canUSB)
if abs(
result[0]
) > DEFINES.CALIBRATION_MOTOR_MAXIMAL_ERROR or abs(
result[1]) > DEFINES.CALIBRATION_MOTOR_MAXIMAL_ERROR:
raise errors.OutOfRangeError(
f'Motor calibration error of positioner {positioner.ID:04d} is too big'
) from None
except (errors.CANError, errors.PositionerError,
errors.OutOfRangeError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners motor calibration failed") from None
def set_speed_all_positioners(self, alphaSpeed, betaSpeed):
try:
for positioner in self.positioners:
positioner.set_speed(self.canUSB, alphaSpeed, betaSpeed)
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners speed setting failed") from None
def set_current_all_positioners(self, alphaCurrent, betaCurrent):
try:
for positioner in self.positioners:
positioner.set_current(self.canUSB, alphaCurrent, betaCurrent)
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners current setting failed") from None
def stop_all_positioners(self):
try:
for positioner in self.positioners:
positioner.stop(self.canUSB)
except errors.CANError as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners could not be stopped. A manual shutdown is recommended"
) from None
def move_positioners_to_offset(self, approachDistance):
try:
alphaAngles = []
betaAngles = []
for positioner in self.positioners:
alphaAngles.append(-positioner.model.offsetAlpha * 180 / np.pi)
betaAngles.append(-positioner.model.offsetBeta * 180 / np.pi)
self.move_all_positioners_different_angles(alphaAngles, betaAngles,
approachDistance)
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners movement failed") from None
def calibrate_all_coggings(self):
try:
for positioner in self.positioners:
positioner.calibrate_cogging_torque(self.canUSB)
# Wait until all the positioners finished their calibration
finishedIDs = []
while len(finishedIDs) < len(self.positioners):
for positioner in self.positioners:
if positioner.ID in finishedIDs:
continue
status = positioner.get_status(self.canUSB)
if not (status & (canUSB._OPT.STREG.COGGING_CALIBRATION)):
finishedIDs.append(positioner.ID)
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners cogging torque calibration failed") from None
def move_all_positioners(self, alphaAngle, betaAngle):
tStart = -1
tEnd = 0
try:
for positioner in self.positioners:
movementTime = positioner.goto_position(
self.canUSB, alphaAngle, betaAngle)
if tStart < 0:
tStart = time.perf_counter(
) #Start chronometer after the first movemement started
tEnd = max(time.perf_counter() + movementTime, tEnd)
#Wait for the movements to finish
tRemaining = tEnd - time.perf_counter()
while time.perf_counter() < tEnd:
time.sleep(0.005)
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners movement failed") from None
def identify_positioners_in_bench(self):
try:
#Setup the ROI
if self.cameraXY.parameters.softROIrequired:
self.cameraXY.setMaxROI()
self.cameraXY.setExposure(
DEFINES.PC_CAMERA_XY_DEFAULT_EXPOSURE)
#Set the current and the speed
for positioner in self.positioners:
positioner.set_current(self.canUSB,
positioner.physics.maxCurrentAlpha,
positioner.physics.maxCurrentBeta)
positioner.set_speed(self.canUSB,
positioner.physics.maxRpmAlpha,
positioner.physics.maxRpmBeta)
#Move back to the origin
log.message(DEFINES.LOG_MESSAGE_PRIORITY_DEBUG_INFO, 0,
'Moving back to 0,0')
self.move_all_positioners(0, 0)
#Shut the current down
self.set_current_all_positioners(0, 0)
#Get the optimal exposure for all the bench slots and sort out slots without any centroid. If the slot has a centroid, grab it.
exposure = []
validSlots = []
validSlotsIDs = []
identificationCentroidsStart = []
identificationCentroidsEnd = []
identificationDistances = []
sortedExposures = []
sortedSlotsCenters = []
sortedPositioners = []
ROI = np.zeros(5)
if self.cameraXY.parameters.softROIrequired:
#Software ROI = get 1 image only. 1 unique exposure for all slots.
#Set hardware ROI to be the whole image
self.cameraXY.setMaxROI()
#get exposure and capture 1 image
completeExposure = self.cameraXY.getOptimalExposure(
DEFINES.PC_CAMERA_XY_DEFAULT_EXPOSURE)
log.message(
DEFINES.LOG_MESSAGE_PRIORITY_INFO, 0,
'Exposure for all slots is ' +
str(round(completeExposure, 0)))
#if completeExposure >= DEFINES.PC_CAMERA_XY_MAX_EXPOSURE:
#raise errors.CameraError("No light coming out the fibers") from None
#completeImage = self.cameraXY.getImage()
#treat each possible slot in the bench
#for i in range(0,self.nbSlots):
#Do a software crop of the unique image
#validityCenter = ( self.slotsCenters[i][0]/self.cameraXY.parameters.scaleFactor,\
#self.slotsCenters[i][1]/self.cameraXY.parameters.scaleFactor)
#validityRadius = (positioner.physics.lengthAlpha+positioner.physics.lengthBeta+DEFINES.PC_IMAGE_SOFT_ROI_MARGIN)/self.cameraXY.parameters.scaleFactor
#(currentImage,softROIoffsetX,softROIoffsetY) = self.cameraXY.computeValidSoftROI(completeImage, validityCenter, validityRadius)
#self.cameraXY.parameters.ROIoffsetX = softROIoffsetX
#self.cameraXY.parameters.ROIoffsetY = softROIoffsetY
#tempImgMax = np.max(currentImage)/DEFINES.PC_CAMERA_MAX_INTENSITY_RAW
#check validity of slot and get the centroid if valid
#if tempImgMax >= DEFINES.CC_CENTROID_DETECTION_THRESHOLD:
#image_ID = i
#identificationCentroidsStart.append(cc.compute_centroid(currentImage, self.cameraXY.parameters, image_ID))
#exposure.append(completeExposure)
#validSlots.append(self.slotsCenters[i])
#validSlotsIDs.append(self.slotIDs[i])
#else:
#for i in range(0,self.nbSlots):
#Set the Slot ROI
#ROI[0] = self.slotsCenters[i][0]/self.cameraXY.parameters.scaleFactor
#ROI[1] = self.slotsCenters[i][1]/self.cameraXY.parameters.scaleFactor
#ROI[2] = 2*(positioner.physics.lengthAlpha+positioner.physics.lengthBeta+DEFINES.PC_IMAGE_SOFT_ROI_MARGIN)/self.cameraXY.parameters.scaleFactor
#ROI[3] = ROI[2]
#ROI[4] = ROI[3]/2
#self.cameraXY.setROI(ROI)
#Get the exposure for the slot
#tempExposure = self.cameraXY.getOptimalExposure(DEFINES.PC_CAMERA_XY_DEFAULT_EXPOSURE)
#log.message(DEFINES.LOG_MESSAGE_PRIORITY_INFO,0,'Exposure for slot #'+str(self.slotIDs[i])+' is '+str(round(tempExposure,0)))
#check validity of slot and get the centroid if valid
#if tempExposure < DEFINES.PC_CAMERA_XY_MAX_EXPOSURE:
#image = self.cameraXY.getImage()
#image_ID = i
#identificationCentroidsStart.append(cc.compute_centroid(image, self.cameraXY.parameters, image_ID))
#exposure.append(tempExposure)
#validSlots.append(self.slotsCenters[i])
#validSlotsIDs.append(self.slotIDs[i])
#if len(validSlots) < 1:
#raise errors.CameraError("No light coming out the fibers") from None
#self.nbSlots = len(validSlots)
#self.slotsCenters = validSlots
#self.slotsExposures = exposure
#self.slotIDs = validSlotsIDs
#restart the current
#for positioner in self.positioners:
#positioner.set_current(self.canUSB, positioner.physics.maxCurrentAlpha, positioner.physics.maxCurrentBeta)
#move each positioner a different amount
#tEnd = 0
#tStart = -1
#currentAngle = DEFINES.PM_ANGLE_INCREMENT
#theoricIdentificationDistance = []
#for positioner in self.positioners:
#alphaAngle = 0
#betaAngle = currentAngle
#movementTime = positioner.goto_position(self.canUSB, alphaAngle, betaAngle)
#theoricIdentificationDistance.append(2*positioner.physics.lengthBeta*np.sin((currentAngle*np.pi/180)/2))
#currentAngle += DEFINES.PM_ANGLE_INCREMENT
#if tStart < 0:
#tStart = time.perf_counter() #Start chronometer after the first movmement started
#tEnd = max(time.perf_counter()+movementTime,tEnd)
#tRemaining = tEnd-time.perf_counter()
#log.message(DEFINES.LOG_MESSAGE_PRIORITY_INFO,0,f'Going to ({alphaAngle:7.2f},{betaAngle:7.2f}) in {tRemaining:5.2f} [s]', overwritable = True)
#while time.perf_counter()<tEnd:
#time.sleep(0.005)
#tTaken = time.perf_counter()-tStart
#log.message(DEFINES.LOG_MESSAGE_PRIORITY_INFO,0,f'Went to ({alphaAngle:7.2f},{betaAngle:7.2f}) in {tTaken:5.2f} [s]')
#Shut the current down
#self.set_current_all_positioners(0,0)
#get all the final positions of the positioners and compute the distances
#if self.cameraXY.parameters.softROIrequired:
#completeImage = self.cameraXY.getImage()
#treat each possible slot in the bench
#for i in range(0,self.nbSlots):
#Do a software crop of the unique image
#validityCenter = ( self.slotsCenters[i][0]/self.cameraXY.parameters.scaleFactor,\
#self.slotsCenters[i][1]/self.cameraXY.parameters.scaleFactor)
#validityRadius = (positioner.physics.lengthAlpha+positioner.physics.lengthBeta+DEFINES.PC_IMAGE_SOFT_ROI_MARGIN)/self.cameraXY.parameters.scaleFactor
#(currentImage,softROIoffsetX,softROIoffsetY) = self.cameraXY.computeValidSoftROI(completeImage, validityCenter, validityRadius)
#self.cameraXY.parameters.ROIoffsetX = softROIoffsetX
#self.cameraXY.parameters.ROIoffsetY = softROIoffsetY
#image_ID = i
#identificationCentroidsEnd.append(cc.compute_centroid(currentImage, self.cameraXY.parameters, image_ID))
#identificationDistances.append(np.sqrt((identificationCentroidsStart[i][0]-identificationCentroidsEnd[i][0])**2+(identificationCentroidsStart[i][1]-identificationCentroidsEnd[i][1])**2))
#else:
#for i in range(0,self.nbSlots):
#Set the Slot ROI
#ROI[0] = self.slotsCenters[i][0]/self.cameraXY.parameters.scaleFactor
#ROI[1] = self.slotsCenters[i][1]/self.cameraXY.parameters.scaleFactor
#ROI[2] = 2*(positioner.physics.lengthAlpha+positioner.physics.lengthBeta+DEFINES.PM_POSITIONER_WORKSPACE_MARGIN)/self.cameraXY.parameters.scaleFactor
#ROI[3] = ROI[2]
#ROI[4] = ROI[3]/2
#self.cameraXY.setROI(ROI)
#self.cameraXY.setExposure(self.slotsExposures[i])
#image = self.cameraXY.getImage()
#image_ID = i
#identificationCentroidsEnd.append(cc.compute_centroid(image, self.cameraXY.parameters, image_ID))
#identificationDistances.append(np.sqrt((identificationCentroidsStart[i][0]-identificationCentroidsEnd[i][0])**2+(identificationCentroidsStart[i][1]-identificationCentroidsEnd[i][1])**2))
#restart the current
#restart the current
#for positioner in self.positioners:
#positioner.set_current(self.canUSB, positioner.physics.maxCurrentAlpha, positioner.physics.maxCurrentBeta)
#move all the positioners to 0,0
#self.move_all_positioners(0,0)
#stop all positioners
#self.stop_all_positioners()
#Compute correlation between the slot and each positioner
#correlation = np.ones((len(identificationDistances), len(theoricIdentificationDistance)))
#for slot in range(0,len(identificationDistances)):
#for positionerIndex in range(0,len(theoricIdentificationDistance)):
#correlation[slot, positionerIndex] = abs(identificationDistances[slot]/theoricIdentificationDistance[positionerIndex]-1)
#sortedExposures = []
#sortedSlotsCenters = []
#sortedPositioners = []
#validSlotsIDs = []
#assignedSlots = 0
#Assign each positioner to the corresponding slot
#for slot in range(0,self.nbSlots):
#positionerIndex = np.argmin(correlation[slot,:])
#if positionerIndex < len(self.positioners) and identificationDistances[slot] > DEFINES.PM_IDENTIFICATION_MINIMAL_DISPLACEMENT:
#store the slots
#sortedSlotsCenters.append(self.slotsCenters[slot])
#validSlotsIDs.append(self.slotIDs[slot])
#sortedExposures.append(self.slotsExposures[slot])
#store the positioner corresponding to the slot
#self.positioners[positionerIndex].benchSlot = assignedSlots
#self.positioners[positionerIndex].model.centerX = sortedSlotsCenters[-1][DEFINES.CC_X_COORDINATE]
#self.positioners[positionerIndex].model.centerY = sortedSlotsCenters[-1][DEFINES.CC_Y_COORDINATE]
#sortedPositioners.append(self.positioners[positionerIndex])
#log.message(DEFINES.LOG_MESSAGE_PRIORITY_INFO,0,f'Slot #{validSlotsIDs[-1]} contains positioner with ID {sortedPositioners[-1].ID} ({100*(1-correlation[slot,positionerIndex]):.1f}% correlation)')
#assignedSlots += 1
#if len(sortedPositioners)>1:
#for positioner in sortedPositioners[0:-1]:
#if sortedPositioners[-1].ID == positioner.ID:
#raise errors.PositionerError(f'Multiple slots contain the same positioner ID ({sortedPositioners[-1].ID})') from None
slot = 1
sortedSlotsCenters.append(self.slotsCenters[slot])
validSlotsIDs.append(self.slotIDs[slot])
sortedExposures.append(completeExposure)
self.positioners[0].benchSlot = 0
self.positioners[0].model.centerX = sortedSlotsCenters[-1][
DEFINES.CC_X_COORDINATE]
self.positioners[0].model.centerY = sortedSlotsCenters[-1][
DEFINES.CC_Y_COORDINATE]
sortedPositioners.append(self.positioners[0])
self.slotsCenters = np.array(sortedSlotsCenters)
self.slotIDs = validSlotsIDs
self.slotsExposures = sortedExposures
self.positioners = sortedPositioners
self.nbSlots = len(sortedExposures)
except (errors.CameraError, errors.PositionerError,
errors.CANError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners identification failed") from None
def move_all_positioners_different_angles(self,
alphaAngle,
betaAngle,
approachDistance=0,
isInRad=False):
if len(alphaAngle) is not self.nbSlots or len(
betaAngle) is not self.nbSlots:
raise errors.PositionerError(
"Length of input angles does not match the number of positioners"
) from None
try:
if isInRad:
alphaAngle = 180 * alphaAngle / np.pi
betaAngle = 180 * betaAngle / np.pi
approachDistance = 180 * approachDistance / np.pi
if not approachDistance == 0:
tStart = -1
tEnd = 0
movementTime = 0
for slot in range(0, self.nbSlots):
if not np.isnan(alphaAngle[slot]) and not np.isnan(
betaAngle[slot]):
movementTime = self.positioners[slot].goto_position(
self.canUSB, alphaAngle[slot] - approachDistance,
betaAngle[slot] - approachDistance)
if tStart < 0:
tStart = time.perf_counter(
) #Start chronometer after the first movmement started
tEnd = max(time.perf_counter() + movementTime, tEnd)
#Wait for the movement to finish
tRemaining = tEnd - time.perf_counter()
while time.perf_counter() < tEnd:
time.sleep(0.005)
tStart = -1
tEnd = 0
movementTime = 0
for slot in range(0, self.nbSlots):
if not np.isnan(alphaAngle[slot]) and not np.isnan(
betaAngle[slot]):
movementTime = self.positioners[slot].goto_position(
self.canUSB, alphaAngle[slot], betaAngle[slot])
if tStart < 0:
tStart = time.perf_counter(
) #Start chronometer after the first movmement started
tEnd = max(time.perf_counter() + movementTime, tEnd)
#Wait for the movement to finish
tRemaining = tEnd - time.perf_counter()
while time.perf_counter() < tEnd:
time.sleep(0.005)
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners movement failed") from None