class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') self.obstacledetector = ObstacleDetector('/ultrasonic') def trace(self): left, right = self.line_detector.get_left_right() angle = self.steer(left, right) speed = self.accelerate(angle) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 - 320 if abs(mid) > 40: angle = mid // 1.5 else: angle = mid // 3.5 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def accelerate(self, angle): left, mid, right = self.obstacledetector.get_distance() if (left < 140 and mid < 100 and left > 0 and mid > 0): if left + mid < 130: exit() return 0 return 50 def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): angle = self.line_detector.get_left_right() angle = self.steer(angle) speed = self.accelerate(angle) self.driver.drive(angle + 90, speed + 90) def steer(self, angle): if abs(angle) < 7: angle *= 1.2 else: angle *= 3.25 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def accelerate(self, angle): if abs(angle) > 7: return 20 return 25 def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): left, right = self.line_detector.get_left_right() angle = self.steer(left, right) speed = self.accelerate(angle) self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): print(left, right) mid = (left + right) // 2 - 320 angle = mid // 1.7 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def accelerate(self, angle): if angle <= -10 or angle >= 10: speed = 20 else: speed = 25 return speed def exit(self): print('finished')
class AutoDrive: def __init__(self): rospy.init_node('xycar_driver') self.line_detector = LineDetector('/usb_cam/image_raw') self.driver = MotorDriver('/xycar_motor_msg') def trace(self): result = self.line_detector.get_left_right() if result: if result == 'left': angle = 30 elif result == 'right': angle = -30 else: left, right = result angle = self.steer(left, right) speed = self.accelerate(angle) else: angle = 0 speed = 0 self.driver.drive(angle + 90, speed + 90) def steer(self, left, right): mid = (left + right) // 2 - 320 if abs(mid) > 40: angle = mid // 1.5 else: angle = mid // 3.5 angle = max(-50, angle) if angle < 0 else min(50, angle) return angle def accelerate(self, angle): return 31 def exit(self): print('finished')