def transition(time, next_state):
"""Transition function to help state machine."""
if str(D.robots[Robo.name]) == "stop":
print "robot lost target"
Robo.speed = (0, 0)
Robo.move(0, 0)
elif str(D.robots[Robo.name]) == "wait":
print "robot waiting on other robots"
Robo.speed = (0, 0)
Robo.move(0, 0)
elif Robo.state == "not active":
print "robot has not been activated"
Robo.speed = (0, 0)
Robo.move(0, 0)
if D.data.wheeldropCaster:
print "Wheel drop!"
print "Stopping..."
Robo.speed = (0, 0)
Robo.move(0, 0)
HandleData.make_others_wait()
#Robo.mode = "ambivalent"
rospy.signal_shutdown("robot picked up... so we're shutting down")
else:
rospy.Timer(rospy.Duration(time), next_state, oneshot=True)
def state_turn_to_goal(timer_event=None):
# Fields
print "turning to goal"
error, idealX, maxSpeed, porp_diff = 20, 320, 110, .25
targetXDiff = D.target_coord[0] - idealX
speed = targetXDiff * porp_diff
# If target isn't a residual, turn if needed
if D.target_size > D.min_T_size:
# If we hit the sweet spot, don't turn anymore
if abs(speed) < 5:
Robo.speed = (0, 0)
Robo.move(0, 0)
transition(0.1, state_follow)
else:
Robo.speed = (speed, -speed)
Robo.move(speed, -speed)
transition(0.1, state_follow)
# Whelp, we lost the target
else:
Robo.speed = (0, 0)
Robo.move(0, 0)
HandleData.make_others_wait()
transition(0.1, state_seek)
def transition(time, next_state):
"""Transition function to help state machine."""
if str(D.robots[Robo.name]) == "stop":
print "robot lost target"
Robo.speed = (0,0)
Robo.move(0,0)
elif str(D.robots[Robo.name]) == "wait":
print "robot waiting on other robots"
Robo.speed = (0,0)
Robo.move(0,0)
elif Robo.state == "not active":
print "robot has not been activated"
Robo.speed = (0,0)
Robo.move(0,0)
if D.data.wheeldropCaster:
print "Wheel drop!"
print "Stopping..."
Robo.speed = (0,0)
Robo.move(0,0)
HandleData.make_others_wait()
#Robo.mode = "ambivalent"
rospy.signal_shutdown("robot picked up... so we're shutting down")
else:
rospy.Timer(rospy.Duration(time), next_state, oneshot=True)
def state_turn_to_goal(timer_event=None):
# Fields
print "turning to goal"
error, idealX, maxSpeed, porp_diff = 20, 320, 110, .25
targetXDiff = D.target_coord[0] - idealX
speed = targetXDiff * porp_diff
# If target isn't a residual, turn if needed
if D.target_size > D.min_T_size:
# If we hit the sweet spot, don't turn anymore
if abs(speed) < 5:
Robo.speed =(0,0)
Robo.move(0,0)
transition(0.1, state_follow)
else:
Robo.speed = (speed,-speed)
Robo.move(speed,-speed)
transition(0.1, state_follow)
# Whelp, we lost the target
else:
Robo.speed =(0,0)
Robo.move(0,0)
HandleData.make_others_wait()
transition(0.1, state_seek)
def activate():
"""Activates robot, or after a deactivation, reactivates."""
if Robo.state != "active":
Robo.state = "active"
print "Robot is now active"
else:
Robo.state = "not active"
print "Robot is no longer active"
Robo.speed = (0, 0)
Robo.move(0, 0)
HandleData.make_others_wait()
def activate():
"""Activates robot, or after a deactivation, reactivates."""
if Robo.state != "active":
Robo.state = "active"
print "Robot is now active"
else:
Robo.state = "not active"
print "Robot is no longer active"
Robo.speed = (0,0)
Robo.move(0,0)
HandleData.make_others_wait()
def state_follow(timer_event=None):
print "following"
idealSize, constantOfP, maxSpeed, error = Robo.ideal_dist, 1/100.0, 110, 20
newSpeed = constantOfP * (idealSize - D.target_size)
if newSpeed > maxSpeed:
newSpeed = maxSpeed
elif newSpeed < -maxSpeed:
newSpeed = -maxSpeed
# If we see a target
if D.target_size > D.min_T_size:
# If it is about to leave the frame, emergency direction change!
if D.target_coord[0] < 100 or D.target_coord[0] > 540:
transition(0.1,state_turn_to_goal)
#If we are alarmingly far from it, charge forward
elif Robo.ideal_dist - D.target_size > (Robo.ideal_dist / 3.0):
Robo.speed = (Robo.max_speed, Robo.max_speed)
Robo.move(Robo.max_speed, Robo.max_speed)
transition(0.1, state_follow)
# If we see the target and it's not too far, check angles
else:
# If the target is pointed in the right direction we can move
if abs(D.target_coord[0] - 320) < error:
print "in the right direction too"
# You hit the sweet spot, so don't mess it up.
if abs(newSpeed) < 5:
Robo.speed = (0,0)
Robo.move(0,0)
transition(0.1, state_follow)
else:
# OK move now
Robo.speed = (int(newSpeed), int(newSpeed))
Robo.move(int(newSpeed), int(newSpeed))
transition(0.1, state_follow)
# We see a target but aren't orientated correctly
else:
transition(0.1,state_turn_to_goal)
# We don't see a target so we need to make everyone wait on us.
else:
Robo.speed = (0,0)
Robo.move(0,0)
HandleData.make_others_wait()
transition(0.1, state_seek)
def state_follow(timer_event=None):
print "following"
idealSize, constantOfP, maxSpeed, error = Robo.ideal_dist, 1 / 100.0, 110, 20
newSpeed = constantOfP * (idealSize - D.target_size)
if newSpeed > maxSpeed:
newSpeed = maxSpeed
elif newSpeed < -maxSpeed:
newSpeed = -maxSpeed
# If we see a target
if D.target_size > D.min_T_size:
# If it is about to leave the frame, emergency direction change!
if D.target_coord[0] < 100 or D.target_coord[0] > 540:
transition(0.1, state_turn_to_goal)
#If we are alarmingly far from it, charge forward
elif Robo.ideal_dist - D.target_size > (Robo.ideal_dist / 3.0):
Robo.speed = (Robo.max_speed, Robo.max_speed)
Robo.move(Robo.max_speed, Robo.max_speed)
transition(0.1, state_follow)
# If we see the target and it's not too far, check angles
else:
# If the target is pointed in the right direction we can move
if abs(D.target_coord[0] - 320) < error:
print "in the right direction too"
# You hit the sweet spot, so don't mess it up.
if abs(newSpeed) < 5:
Robo.speed = (0, 0)
Robo.move(0, 0)
transition(0.1, state_follow)
else:
# OK move now
Robo.speed = (int(newSpeed), int(newSpeed))
Robo.move(int(newSpeed), int(newSpeed))
transition(0.1, state_follow)
# We see a target but aren't orientated correctly
else:
transition(0.1, state_turn_to_goal)
# We don't see a target so we need to make everyone wait on us.
else:
Robo.speed = (0, 0)
Robo.move(0, 0)
HandleData.make_others_wait()
transition(0.1, state_seek)
