def update_state_evade():
global next_state,sonar_difference_handlers
initialize_sonar_handlers()
rospy.loginfo("Evading obstacle")
sonar_difference_handlers[1][0]=update_state_align
motor.act(command='FORWARD',dutycycle=50)
next_state="evade"
def force_stop():
global is_map,STOP_FLAG
initialize_sonar_handlers()
is_map=False
STOP_FLAG=True
rospy.loginfo("Stoping motor")
motor.act("STOP",dutycycle)
def update_state_forward():
global next_state,sonar_difference_handlers
initialize_sonar_handlers()
rospy.loginfo("Going forward")
sonar_difference_handlers[0][2]=update_state_obstacle
motor.act(command="FORWARD",dutycycle=60)
next_state="forward"
def normal_backward(command):
global STOP_FLAG
STOP_FLAG=False
global sonar_difference_handlers
#stop when back sensor return 1
sonar_difference_handlers[2][2]=force_stop
rospy.loginfo("Normal backward")
motor.act("BACKWARD",dutycycle)
def normal_forward(command):
global sonar_difference_handlers
global STOP_FLAG
STOP_FLAG=False
#stop when front sensor return 1
sonar_difference_handlers[0][2]=force_stop
rospy.loginfo("Normal forward")
motor.act("FORWARD",dutycycle)
def update_state_obstacle():
global next_state
initialize_sonar_handlers()
rospy.loginfo("Encountered obstacle")
sonar_difference_handlers[0][0]=update_state_evade
sonar_difference_handlers[0][2]=update_state_obstacle
motor.act(command='LEFT',dutycycle=50)
next_state="evade"
def direct_backward(command):
global STOP_FLAG
STOP_FLAG=False
global sonar_difference_handlers
#stop when front sensor return 1
sonar_difference_handlers[0][2]=stop
rospy.loginfo("Direct backward")
turnto(180)
if not STOP_FLAG:
motor.act("FORWARD",dutycycle)
def rotate():
global target
global current
if ((target+360-current)%360)<=180:
print ('right')
while True:
motor.act(command='RIGHT', dutycycle=50)
elif ((target+360-current)%360)>180:
print ('left')
while True:
motor.act(command='RIGHT', dutycycle=50)
def turnto(target_angle):
direction = None
while difference(target_angle,angle)>15 and not STOP_FLAG:
error = difference(target_angle,angle)
if is_right(target_angle,angle):
motor.act(command='RIGHT',dutycycle=50)
rospy.loginfo("Aligning, turning right, error: "+str(error))
time.sleep(error*0.005)
elif not is_right(target_angle,angle):
motor.act(command='LEFT',dutycycle=50)
rospy.loginfo("Aligning, turning left, error: "+str(error))
time.sleep(error*0.005)
motor.act(command="STOP",dutycycle=90)
time.sleep(0.4)
def normal_right(command):
global STOP_FLAG
STOP_FLAG=False
rospy.loginfo("Normal turn right")
motor.act("RIGHT",dutycycle)
def normal_left(command):
global STOP_FLAG
STOP_FLAG=False
rospy.loginfo("Normal turn left")
motor.act("LEFT",dutycycle)
def update_state_align():
global next_state
initialize_sonar_handlers()
motor.act(command='STOP',dutycycle=50)
next_state="align"