def run_function_and_log(self,
motionFunction,
runSeconds,
timeScale=1,
logFilename=None):
'''
Runs the robot with the given motion function from 0 to
runSeconds, logging time and position to logFileName
'''
# Reset before measuring
self.robot.readyPosition()
wiiTrack = WiiTrackFastClient("localhost", 8080)
sleep(.5)
beginPos = wiiTrack.getPosition()
if beginPos is None:
# Robot walked out of sensor view
self.manual_reset(
'Robot has walked outisde sensor view. Please place back in center and push enter to retry.'
)
raise Exception
if not self.robot.shimmy():
print 'Shimmy failed :('
#self.manual_reset('Shimmy failed. Fix and push enter to retry.')
#####HACK !!!!!!
#raise Exception
ff = open(logFilename, 'a')
try:
self.robot.run(motionFunction,
runSeconds=runSeconds,
resetFirst=False,
interpBegin=1,
interpEnd=2,
logFile=ff,
extraLogInfoFn=lambda: getLogPosString(wiiTrack))
except RobotFailure as ee:
ff.close()
print ee
override = self.manual_reset(
'Robot run failure. Fix something and push enter to retry.')
raise Exception
ff.close()
endPos = wiiTrack.getPosition()
if endPos is None:
# Robot walked out of sensor view
override = self.manual_reset(
'Robot has ended outisde sensor view. Please place back in center\nand push enter to retry.'
)
raise Exception
distance_walked = self.calculate_distance(beginPos, endPos)
print 'Total Distance: %.2f' % distance_walked
