def startMapping(robot: cozmo.robot.Robot):
global coz
#global tkin
#tkin = CozmoView()
robot.enable_stop_on_cliff(True)
if coz == None:
coz = cozmoRobot(robot)
while currentState == 'mapping':
#add a base case
test = coz.moveASpace()
#tkin.updateMap(coz.safeSpaces)
#tkin.updateLoc((coz.currentX, coz.currentY))
coz.drawMap()
if not test:
coz.changeDirection(coz.searchingOrder[coz.searchingInt])
coz.searchingInt += 1
coz.searchingInt %= 4
coz.moveASpace()
#tkin.updateMap(coz.safeSpaces)
#tkin.updateLoc((coz.currentX, coz.currentY))
coz.drawMap()
coz.changeDirection(coz.searchingOrder[coz.searchingInt])
coz.searchingInt += 1
coz.searchingInt %= 4
return
def initial_settings(robot: cozmo.robot.Robot):
# Set Neutral Face fot Cozmo
robot.play_anim_trigger(cozmo.anim.Triggers.NeutralFace,
in_parallel=True).wait_for_completed()
# check if Cozmo is on charger
charger = robot.is_on_charger
# Set Cozmo volume
robot.set_robot_volume(robot_volume=0.3)
# Activate face expression estimation
robot.enable_facial_expression_estimation()
# Activate stop on cliff
robot.enable_stop_on_cliff(enable=True)
# Volts on battery
battery = robot.battery_voltage
# If Cozmo is on charger, drive off it to start FindFaces behavior
if charger is True:
robot.drive_off_charger_contacts(in_parallel=True).wait_for_completed()
robot.drive_straight(distance_mm(120),
speed_mmps(50),
in_parallel=True)
# Set lift to down position5
robot.move_lift(-3)
# Start "Battery check" animation
robot.say_text("Checking battery level",
in_parallel=True,
use_cozmo_voice=Voice,
duration_scalar=0.85).wait_for_completed()
# Cozmo announce that he is ready to play if battery is higher then 3.6 volts - If less than 3.6 volts, Cozmo as to be placed on charger
if battery > 3.69:
robot.play_anim_trigger(cozmo.anim.Triggers.VC_Alrighty,
in_parallel=True).wait_for_completed()
robot.say_text("My battery is good! I am ready to play!",
in_parallel=True,
play_excited_animation=False,
use_cozmo_voice=Voice,
duration_scalar=0.85).wait_for_completed()
robot.set_head_angle(degrees(30)).wait_for_completed()
else:
robot.play_anim_trigger(
cozmo.anim.Triggers.SparkFailure).wait_for_completed()
robot.play_anim_trigger(cozmo.anim.Triggers.NeedsSevereLowEnergyGetIn,
in_parallel=True).wait_for_completed()
robot.say_text(
"I need to charge my battery. Please, place me on the charger.",
in_parallel=True,
use_cozmo_voice=Voice,
duration_scalar=0.85).wait_for_completed()
def beginMapping(robot: cozmo.robot.Robot):
robot.enable_stop_on_cliff(True)
coz = cozmoRobot(robot)
while not paused:
test = coz.moveASpace()
if not test:
coz.changeDirection(coz.searchingOrder[coz.searchingInt])
coz.searchingInt += 1
coz.searchingInt %= 4
coz.moveASpace()
coz.changeDirection(coz.searchingOrder[coz.searchingInt])
coz.searchingInt += 1
coz.searchingInt %= 4
if (coz.currentX, coz.currentY) in coz.visited:
break
else:
coz.addToVisited()
def startMapping(robot: cozmo.robot.Robot):
global coz
robot.enable_stop_on_cliff(True)
if coz == None:
coz = cozmoRobot(robot)
while currentState == 'mapping':
#add a base case
test = coz.moveASpace()
if not test:
coz.changeDirection(coz.searchingOrder[coz.searchingInt])
coz.searchingInt += 1
coz.searchingInt %= 4
coz.moveASpace()
coz.changeDirection(coz.searchingOrder[coz.searchingInt])
coz.searchingInt += 1
coz.searchingInt %= 4
return
def cozmo_program2(robot: cozmo.robot.Robot):
threading.Thread(target=runMCLLoop, args=(robot,)).start()
threading.Thread(target=runPlotLoop, args=(robot,)).start()
robot.enable_stop_on_cliff(True)
# main loop
# TODO insert driving and navigation behavior HERE
t = 0
while True:
print("Straight")
#robot.drive_straight(distance_mm(300), speed_mmps(50)).wait_for_completed()
print("Turn")
#robot.turn_in_place(degrees(90), speed=degrees(20)).wait_for_completed()
print("Sleep")
pos()
time.sleep(0.1)
def cozmo_program(robot: cozmo.robot.Robot):
global posiblePos
timeInterval = 0.1
threading.Thread(target=runMCLLoop, args=(robot,)).start()
threading.Thread(target=runPlotLoop, args=(robot,)).start()
robot.enable_stop_on_cliff(True)
# main loop
# TODO insert driving and navigation behavior HERE
# t = 0
# targetPose=Frame2D.fromXYA(400,700,0.5*3.1416) # this is the target position
# relativeTarget = targetPose
differentTarget = []
targetPose = Frame2D.fromXYA(180, 320, 0.5 * 3.1416)
differentTarget.append(targetPose)
targetPose = Frame2D.fromXYA(400, 700, 0.5 * 3.1416)
differentTarget.append(targetPose)
# explore, move
action = "move1"
timer = 0
while True:
if robot.is_cliff_detected:
robot.drive_wheel_motors(l_wheel_speed=-25, r_wheel_speed=-25)
time.sleep(5)
robot.drive_wheel_motors(l_wheel_speed=15, r_wheel_speed=-15)
time.sleep(10.5)
if (action == "move1"):
#robot.turn_in_place(degrees(360), speed=degrees(13)).wait_for_completed()
robot.drive_wheel_motors(l_wheel_speed= 15, r_wheel_speed=-15)
time.sleep(15.6)
print("yeszzzz")
action = "move"
elif (action == "move"):
if (timer < 300):
timer += 1
print(timer ,"timer")
else:
timer = 0
action = "move1"
# spline approach
currentPose = pos()
relativeTarget = currentPose.inverse().mult(targetPose)
#print("relativeTarget" + str(relativeTarget))
velocity = target_pose_to_velocity_spline(
relativeTarget) # target pose to velocity is the method for spline driving
#print("velocity" + str(velocity))
trackSpeed = velocity_to_track_speed(velocity[0], velocity[1])
#print("trackSpeedCommand" + str(trackSpeed))
robot.drive_wheel_motors(l_wheel_speed=trackSpeed[0], r_wheel_speed=trackSpeed[1])
#print("currentPose" + str(currentPose))
print()
else:
print("error")
exit(-1)
print(action)
print()
time.sleep(timeInterval)
def run(robot_obj: cozmo.robot.Robot):
"""
Main script for the lane following algorithm. Starts all necessary components.
:param robot_obj: Reference to the robot
:type robot_obj: cozmo.robot.Robot
"""
global last_frame
# Create necessary instances and add them to instance manager
InstanceManager.add_instance("Robot", robot_obj)
corr_calculator_obj = CorrectionCalculator()
InstanceManager.add_instance("CorrectionCalculator", corr_calculator_obj)
preview_obj = PreviewUtils()
InstanceManager.add_instance("PreviewUtils", preview_obj)
drive_obj = DriveController()
InstanceManager.add_instance("DriveController", drive_obj)
behavior_obj = BehaviorController()
InstanceManager.add_instance("BehaviorController", behavior_obj)
trackloader_obj = TrackLoader()
InstanceManager.add_instance("TrackLoader", trackloader_obj)
navigator_obj = Navigator()
InstanceManager.add_instance("Navigator", navigator_obj)
sign_handler_obj = SignHandler()
InstanceManager.add_instance("SignHandler", sign_handler_obj)
core_engine_obj = CoreEngine()
InstanceManager.add_instance("CoreEngine", core_engine_obj)
# Setup robot with presets
robot_obj.enable_stop_on_cliff(False)
robot_obj.camera.image_stream_enabled = True
robot_obj.camera.color_image_enabled = False
robot_obj.set_head_light(False)
behavior_obj.move_head_down()
behavior_obj.move_lift_up()
robot_obj.wait_for_all_actions_completed()
# Setup camera event handler
# noinspection PyTypeChecker
robot_obj.add_event_handler(cozmo.camera.EvtNewRawCameraImage, save_last_frame)
# Start driving engine
drive_obj.start()
s = sched.scheduler(time.time, time.sleep)
# runs the frame analysis in the specified frequency
def run_analysis(sc):
if last_frame is not None:
TimingUtils.run_all_elapsed()
core_engine_obj.process_frame(image=last_frame)
s.enter(0.05, 1, run_analysis, (sc,))
s.enter(0.05, 1, run_analysis, (s,))
s.run()
# Setup hotkey listener
with keyboard.Listener(on_press=handle_hotkeys) as listener:
listener.join()
