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)