def measure(self, f_run):
# track 10 periods of the stimulus
t_run = 10.0 / f_run
calibrations = 0
fileNumber = 0
times = []
errors = []
angles = []
while ((self.minAngle > self.targetMinAngle)
and (self.maxAngle < self.targetMaxAngle)):
if calibrations > 0:
print "The robot's camera should be 10 cm from the \
screen in the middle of the calibration track"
print self.minAngle, self.maxAngle
self.calibrate()
calibrations = calibrations + 1
t_start = time.time()
t_end = t_start + t_run
while time.time() < t_end:
# wait for new blocks
while not blocksAreNew(self.oldCount, self.oldBlocks, \
self.newCount, self.newBlocks):
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
# track the target
error = self.newBlocks[0].m_x - self.pixyCenterX
self.servoPos = self.servoPos + self.gimbal.update(error)
if self.servoPos > self.servoMaxPos:
self.servoPos = self.servoMaxPos
elif self.servoPos < self.servoMinPos:
self.servoPos = self.servoMinPos
self.racerBot.setServoPosition(self.servoPos)
times.append(time.time() - t_start)
errors.append(error)
angles.append(self.servoPos)
# get new frame
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
fileName = "calibrationCurve" + str(fileNumber) + ".csv"
while os.path.exists(fileName):
fileNumber = fileNumber + 1
fileName = "calibrationCurve" + str(fileNumber) + ".csv"
with open(fileName, "w+") as myCsv:
csvWriter = csv.writer(myCsv, delimiter=',')
csvWriter.writerows([times, errors, angles])
def calibrate(self):
self.minAngle = 0
self.maxAngle = 0
# wait for a blob with signature 2 (Red) to appear
while not ((self.newCount > 0) and
(self.newBlocks[0].m_signature == self.calibrationSignature)
and (blocksAreNew(self.oldCount, self.oldBlocks,
self.newCount, self.newBlocks))):
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
# track the min and max position of the servo, making sure the
# error is not too big
while self.newBlocks[0].m_signature == self.calibrationSignature:
# wait for new blocks
while not blocksAreNew(self.oldCount, self.oldBlocks, \
self.newCount, self.newBlocks):
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
# track the calibration target
error = self.newBlocks[0].m_x - self.pixyCenterX
self.servoPos = self.servoPos + self.calibrationController.update(
error)
if self.servoPos > self.servoMaxPos:
self.servoPos = self.servoMaxPos
elif self.servoPos < self.servoMinPos:
self.servoPos = self.servoMinPos
self.racerBot.setServoPosition(self.servoPos)
if error < 20:
if self.servoPos < self.minAngle:
self.minAngle = self.servoPos
elif self.servoPos > self.maxAngle:
self.maxAngle = self.servoPos
# get new frame
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
def updateBlocks(self): copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks) self.oldCount = self.newCount self.newCount = self.bot.pixy.ccc_get_blocks (100, self.newBlocks)
def visualPredication(self, predicationFunction):
times = []
errors = []
angles = []
t_start = time.time()
self.newCount = self.racerBot.pixy.ccc_get_blocks(100, self.newBlocks)
# wait for a red blob to show up
while not ((self.count > 0) and (self.newBlocks[0].m_signature == 1)):
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
# while we do see a red blob, track it
while ((self.count > 0) and (self.newBlocks[0].m_signature == 1)):
error = self.newBlocks[0].m_x - self.pixyCenterX
self.servoPos = self.servoPos + self.gimbal.update(error)
if self.servoPos > self.servoMaxPos:
self.servoPos = self.servoMaxPos
elif self.servoPos < self.servoMinPos:
self.servoPos = self.servoMinPos
self.racerBot.setServoPosition(self.servoPos)
times.append(time.time() - t_start)
errors.append(error)
angles.append(self.servoPos)
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
# once the blob disappears, invoke the function supplied by the students
while not ((self.count > 0) and (self.newBlocks[0].m_signature == 1)):
time_now = time.time() - t_start
try:
pos = predicationFunction(times, time_now, errors, angles)
except:
return None
# move to the position demanded by the students
self.racerBot.setServoPosition(pos)
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
# at this point the blob should have reappeared and we are just checking how good the tracking was
# check if the blob is already in view
if self.newBlocks[0].m_signature == targetSignature:
error = self.newBlocks[0].m_x - self.pixyCenterX
return error
else:
return float('Inf')
time_now = time.time()
max_time = 10
while (time.time() - time_now < max_time):
copyBlockArray(self.newCount, self.newBlocks, self.oldBlocks)
self.oldCount = self.newCount
self.newCount = self.racerBot.pixy.ccc_get_blocks(
100, self.newBlocks)
if self.newBlocks[0].m_signature == targetSignature:
error = self.newBlocks[0].m_x - self.pixyCenterX
return 'late', error
return 'noShow', None