def drive_remotely(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
soc = metalog.createLoggedSocket('remote', headerFormat=None) # TODO fix headerFormat
soc.bind( ('',PORT) )
if not metalog.replay:
soc.soc.setblocking(0)
soc.soc.settimeout( SOCKET_TIMEOUT )
remote_cmd_log_name = metalog.getLog('remote_cmd')
if metalog.replay:
robot.remote = DummySensor()
function = SourceLogger(None, remote_cmd_log_name).get
else:
robot.remote = RemoteThread(soc)
function = SourceLogger(robot.remote.get_data, remote_cmd_log_name).get
max_speed = None
robot.remote_data = None
robot.register_data_source('remote', function, remote_data_extension)
robot.remote.start()
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
print("Waiting for remote commands ...")
while robot.remote_data is None:
robot.update()
print("received", robot.remote_data.strip())
moving = False
turning = False
while robot.remote_data != 'END\n':
robot.update()
if robot.remote_data == 'STOP\n' and (moving or turning):
if moving:
robot.canproxy.stop()
print("STOP")
moving = False
if turning:
robot.canproxy.stop_turn()
print("STOP turn")
turning = False
elif robot.remote_data == 'UP\n' and not moving:
if max_speed is None:
robot.canproxy.go()
print("GO")
else:
robot.set_desired_speed(max_speed)
print("GO", max_speed)
moving = True
elif robot.remote_data == 'DOWN\n' and not moving:
if max_speed is None:
robot.canproxy.go_back()
print("GO back")
else:
robot.set_desired_speed(-max_speed)
print("GO back", max_speed)
moving = True
elif robot.remote_data == 'LEFT\n':
robot.canproxy.set_turn_raw(200)
print("Left")
turning = True
elif robot.remote_data == 'RIGHT\n':
robot.canproxy.set_turn_raw(-200)
print("Right")
turning = True
elif robot.remote_data == 'SPEED0\n':
max_speed = None
elif robot.remote_data.startswith('SPEED'):
max_speed = int(robot.remote_data[len('SPEED'):])/10.0
print("Max Speed", max_speed)
print("received", robot.remote_data.strip())
robot.canproxy.stop_turn()
robot.remote.requestStop()
robot.wait(3.0)
def drive_remotely(metalog):
assert metalog is not None
can_log_name = metalog.getLog('can')
if metalog.replay:
if metalog.areAssertsEnabled():
can = CAN(ReplayLog(can_log_name), skipInit=True)
else:
can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
else:
can = CAN()
can.relog(can_log_name)
can.resetModules(configFn=setup_faster_update)
robot = JohnDeere(can=can)
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
robot.localization = None # TODO
soc = metalog.createLoggedSocket('remote', headerFormat=None) # TODO fix headerFormat
soc.bind( ('',PORT) )
if not metalog.replay:
soc.soc.setblocking(0)
soc.soc.settimeout( SOCKET_TIMEOUT )
remote_cmd_log_name = metalog.getLog('remote_cmd')
if metalog.replay:
robot.remote = DummySensor()
function = SourceLogger(None, remote_cmd_log_name).get
else:
robot.remote = RemoteThread(soc)
function = SourceLogger(robot.remote.get_data, remote_cmd_log_name).get
max_speed = None
robot.remote_data = None
robot.register_data_source('remote', function, remote_data_extension)
robot.remote.start()
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
print "Waiting for remote commands ..."
while robot.remote_data is None:
robot.update()
print "received", robot.remote_data.strip()
moving = False
turning = False
while robot.remote_data != 'END\n':
robot.update()
if robot.remote_data == 'STOP\n' and (moving or turning):
if moving:
robot.canproxy.stop()
print "STOP"
moving = False
if turning:
robot.canproxy.stop_turn()
print "STOP turn"
turning = False
elif robot.remote_data == 'UP\n' and not moving:
if max_speed is None:
robot.canproxy.go()
print "GO"
else:
robot.set_desired_speed(max_speed)
print "GO", max_speed
moving = True
elif robot.remote_data == 'DOWN\n' and not moving:
if max_speed is None:
robot.canproxy.go_back()
print "GO back"
else:
robot.set_desired_speed(-max_speed)
print "GO back", max_speed
moving = True
elif robot.remote_data == 'LEFT\n':
robot.canproxy.set_turn_raw(200)
print "Left"
turning = True
elif robot.remote_data == 'RIGHT\n':
robot.canproxy.set_turn_raw(-200)
print "Right"
turning = True
elif robot.remote_data == 'SPEED0\n':
max_speed = None
elif robot.remote_data.startswith('SPEED'):
max_speed = int(robot.remote_data[len('SPEED'):])/10.0
print "Max Speed", max_speed
print "received", robot.remote_data.strip()
robot.canproxy.stop_turn()
robot.remote.requestStop()
robot.wait(3.0)
