def stop(self): msg_pwm = pwm_micro() msg_pwm.speed_left = 0.0 msg_pwm.speed_right = 0.0 msg_pwm.direction_left = 1 msg_pwm.direction_right = 1 msg_pwm.enable_left = 0 msg_pwm.enable_right = 0 self.pub_pwm.publish(msg_pwm) pass
def main_loop(self):
while not rospy.is_shutdown():
msg_pwm = pwm_micro()
if self.__go_left:
rospy.loginfo('go left!!!')
msg_pwm.direction_left = self.__direction_left
msg_pwm.enable_left = 1
if self.__copy_of_current_dist_left < self.__distance_to_go_to_left - 0.1 - self.__desired_speed_left or self.__copy_of_current_dist_left > self.__distance_to_go_to_left + 0.1 + self.__desired_speed_left :
msg_pwm.speed_left = self.__desired_speed_left
elif self.__copy_of_current_dist_left < self.__distance_to_go_to_left - 0.1 or self.__copy_of_current_dist_left > self.__distance_to_go_to_left + 0.1:
msg_pwm.speed_left = self.__desired_speed_left/2.0
elif self.__copy_of_current_dist_left < self.__distance_to_go_to_left - 0.05 or self.__copy_of_current_dist_left > self.__distance_to_go_to_left + 0.05:
msg_pwm.speed_left = 0.1
else:
msg_pwm.speed_left = 0.0
if (self.__copy_of_current_dist_left > self.__distance_to_go_to_left - 0.025 and self.__copy_of_current_dist_left < self.__distance_to_go_to_left + 0.025):
self.__go_left = False
else:
msg_pwm.speed_left = 0.0
msg_pwm.direction_left = 1
msg_pwm.enable_left = 0
if self.__go_right:
rospy.loginfo('go right!!!')
msg_pwm.direction_right = self.__direction_right
msg_pwm.enable_right = 1
if self.__copy_of_current_dist_right < self.__distance_to_go_to_right - 0.1 - self.__desired_speed_right or self.__copy_of_current_dist_right > self.__distance_to_go_to_right + 0.1 + self.__desired_speed_right:
msg_pwm.speed_right = self.__desired_speed_right
elif self.__copy_of_current_dist_right < self.__distance_to_go_to_right - 0.1 or self.__copy_of_current_dist_right > self.__distance_to_go_to_right + 0.1:
msg_pwm.speed_right = self.__desired_speed_right/2.0
elif self.__copy_of_current_dist_right < self.__distance_to_go_to_right - 0.05 or self.__copy_of_current_dist_right > self.__distance_to_go_to_right + 0.05:
msg_pwm.speed_right = 0.1
else:
msg_pwm.speed_right = 0.0
if (self.__copy_of_current_dist_right > self.__distance_to_go_to_right - 0.025 and self.__copy_of_current_dist_right < self.__distance_to_go_to_right + 0.025):
self.__go_right = False
else:
msg_pwm.speed_right = 0.0
msg_pwm.direction_right = 1
msg_pwm.enable_right = 0
self.pub_pwm.publish(msg_pwm)
rospy.sleep(0.02)
pass
def main_loop(self):
rospy.loginfo('distance_test node running')
while not rospy.is_shutdown():
msg_pwm = pwm_micro()
if not self.__stop_criteria_left.endReachedTest(self.__copy_of_current_dist_left, self.__desired_speed_left):
# rospy.loginfo('go left!!!')
msg_pwm.direction_left = self.__direction_left
msg_pwm.enable_left = 1
# tempSpeed = self.__desired_speed_left/self.__stop_criteria_left.breakingDegree(self.__desired_speed_left,self.__copy_of_current_dist_left)
# if tempSpeed < 0.1:
# tempSpeed = 0.1
tempSpeed = self.__desired_speed_left
msg_pwm.speed_left = tempSpeed
else:
msg_pwm.speed_left = 0.0
msg_pwm.direction_left = 1
msg_pwm.enable_left = 0
if not self.__stop_criteria_right.endReachedTest(self.__copy_of_current_dist_right, self.__desired_speed_right):
# rospy.loginfo('go right!!!')
msg_pwm.direction_right = self.__direction_right
msg_pwm.enable_right = 1
# tempSpeed = self.__desired_speed_right/self.__stop_criteria_right.breakingDegree(self.__desired_speed_right, self.__copy_of_current_dist_right)
# if tempSpeed < 0.1:
# tempSpeed = 0.1
tempSpeed = self.__desired_speed_right
msg_pwm.speed_right = tempSpeed
else:
msg_pwm.speed_right = 0.0
msg_pwm.direction_right = 1
msg_pwm.enable_right = 0
self.pub_pwm.publish(msg_pwm)
rospy.sleep(0.02)
pass
