def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
self.imu = ImuRead('diagnostics')
self.before = [0, 0, 0]
self.before_sonic = 0
self.startTime = time.time()
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
self.imu = ImuRead('/diagnostics')
self.b_l = 0
self.b_r = 0
self.origin = False
def __init__(self):
rospy.init_node('drive')
self.pub = rospy.Publisher('xycar_motor_msg',
Int32MultiArray,
queue_size=1)
self.position = LineDetector()
self.color = ColorDetector()
self.lPos = -1
self.rPos = -1
self.speed = 128
self.imu = ImuRead('/diagnostics')
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.driver = MotorDriver('/xycar_motor_msg')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.imu = ImuRead('/diagnostics')
self.information = {
"speed": -1,
"signal": "",
"=>": "",
"pitch": -1,
"roll": -1,
"yaw": -1
}
class pay(object):
def __init__(self):
self.imu = ImuRead('/diagnostics')
def distancecalculator(self, imudistance):
r, p, y = self.imu.get_data()
distance = y
pay = 3000
a = distance // 1
pay += a * 100
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.obstacle_detector = ObstacleDetector('/ultrasonic')
self.imu = ImuRead('/diagnostics')
def trace(self):
obs_l, obs_m, obs_r = self.obstacle_detector.get_distance()
line_l, line_r = self.line_detector.detect_lines()
self.line_detector.show_images(line_l, line_r)
r, p, y = self.imu.get_data()
angle = self.steer(line_l, line_r)
speed = self.accelerate(angle, obs_l, obs_m, obs_r)
self.driver.drive(angle + 90, speed + 90)
print(speed + 90)
print(r, p, y)
def steer(self, left, right):
if left == -1:
if 320 < right < 390:
angle = -50
else:
angle = (550 - right) / (-3)
elif right == -1:
if 250 < left < 320:
angle = 50
else:
angle = (left - 90) / 3
else:
angle = 0
return angle
def accelerate(self, angle, left, mid, right):
print(mid)
if mid < 40:
speed = 0
return speed
if angle <= -40 or angle >= 40:
speed = 30
elif angle <= -25 or angle >= 25:
speed = 40
else:
speed = 50
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')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.imu = ImuRead('/diagnostics')
self.information = {
"speed": -1,
"signal": "",
"=>": "",
"pitch": -1,
"roll": -1,
"yaw": -1
}
def trace(self):
obs_l, obs_m, obs_r = self.obstacle_detector.get_distance()
line_l, line_r = self.line_detector.detect_lines()
# color_red, color_green, color_blue = self.line_detector.detect_colors()
self.line_detector.show_images(line_l, line_r)
self.information["roll"], self.information["pitch"], self.information[
"yaw"] = self.imu.get_data()
# self.line_detector.show_colors(color_red, color_green, color_blue)
angle = self.steer(line_l, line_r)
speed = self.accelerate(angle, obs_l, obs_m, obs_r)
self.information["speed"] = speed + 90
# print('R (%.1f), P (%.1f), Y (%.1f)' % (self.r, self.p, self.y))
self.driver.drive(angle + 90, speed + 90)
# if self.obstacle_stop(obs_l, obs_m, obs_r) == 0:
# self.driver.drive(90, 90)
# else:
# self.driver.drive(angle + 90, speed + 90)
def steer(self, left, right):
if left == -1:
if 320 < right < 390:
angle = -50
else:
angle = (550 - right) / (-3)
elif right == -1:
if 250 < left < 320:
angle = 50
else:
angle = (left - 90) / 3
else:
angle = 0
return angle
def accelerate(self, angle, left, mid, right):
# print(mid)
if 0 < mid < 85:
speed = 0
return speed
if angle <= -40 or angle >= 40:
speed = 30
elif angle <= -25 or angle >= 25:
speed = 40
else:
speed = 50
return speed
def printInformation(self):
print("speed : " + str(self.information["speed"]) + " ",
"signal : " + str(self.information["signal"]) + " ",
"=> " + str(self.information["=>"]))
print("pitch : " + str(self.information["pitch"]) + " ",
"roll : " + str(self.information["roll"]) + " ",
"yaw : " + str(self.information["yaw"]))
print("")
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
self.imu = ImuRead('/diagnostics')
self.b_l = 0
self.b_r = 0
self.origin = False
def trace(self):
obs_l, obs_m, obs_r = self.obstacle_detector.get_distance()
line_l, line_r = self.line_detector.detect_lines()
self.line_detector.show_images(line_l, line_r)
r, p = self.imu.get_data()
angle = self.steer(line_l, line_r)
speed = self.accelerate(angle, obs_l, obs_m, obs_r, r, p)
if self.origin and p > -6.5:
speed = -7
angle = 0
elif ((r > 4 and r < 175.0) or
(r < -4 and r > -175.0)) and self.origin == False:
speed = 28 # 30
angle = -5
elif p <= -6.5 and p > -7.5:
speed = -1
angle = 0
elif p <= -7.5 and p > -8.5:
speed = 24
angle = 0
elif p <= -8.5 and p >= -15:
speed = 26 # 28
angle = 0
elif p < -15:
speed = 34.5
angle = 0
self.origin = True
'''
elif (r>0 and r<175.0) or (r<0 and r>-175.0):
speed = 30
angle = -5
elif line_l == -2:
speed = 0
angle = 0
print("it stopped")
'''
print("angle-speed:", angle, speed)
print("roll:", r, p)
# print('R (%.1f) P (%.1f), Y (%.1f)' % (r, p, y))
self.driver.drive(angle + 90, speed + 90)
def steer(self, left, right):
if left == -1 and right == 641:
left = self.b_l
right = self.b_r
mid = (left + right) // 2
if mid < 250:
angle = -82
elif mid >= 250 and mid < 260:
angle = -70
elif mid >= 260 and mid < 270:
angle = -40
# angle - -55
elif mid >= 270 and mid < 280:
# angle = -50
angle = -30
elif mid >= 280 and mid < 285:
angle = -10
elif mid > 390:
angle = 73
elif mid >= 380 and mid < 390:
angle = 50
elif mid >= 370 and mid < 380:
angle = 38
elif mid >= 360 and mid < 370:
angle = 28
elif mid >= 355 and mid < 360:
angle = 1
else:
angle = 0
self.b_l = left
self.b_r = right
print(self.b_l, self.b_r)
print(left, right)
return angle
def accelerate(self, angle, left, mid, right, roll, pitch):
after = time.time()
print(mid, after - now)
# if mid < 90: #and (after-now) > 70.5:
# speed = 20
# if mid < 85: #and (after-now) > 70.5:
# speed = 12
# if mid < 80: #and (after-now) > 70.5:
# speed = 7
# if mid < 85: #and (after-now) > 70.5:
# speed = 4
if mid < 95 and (after - now) < 20.5:
speed = 0
elif angle == 10 or angle == -10:
speed = 32
elif angle < -30 or angle > 30:
speed = 34
else:
speed = 35
return speed
def exit(self):
print('finished')
def __init__(self):
self.imu = ImuRead('/diagnostics')
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
self.imu = ImuRead('/diagnostics')
self.angle = 0
self.speed = 0
def trace(self):
redSignal = self.line_detector.trafficLight()
print(redSignal)
if redSignal:
print(redSignal)
self.forward()
r, p, y = self.imu.get_data()
obs_l, obs_m, obs_r, obs_sl, obs_sr, B = self.obstacle_detector.get_distance(
)
line_l, line_r = self.line_detector.detect_lines()
#self.line_detector.show_images(line_l,line_r)
self.angle = self.steer(line_l, line_r)
speed = self.accelerate(self.angle, obs_l, obs_m, obs_r, p, obs_sl,
obs_sr, B)
self.driver.drive(self.angle + 90, speed + 90)
#print(p)
if (p < -5):
time.sleep(3)
def steer(self, left, right):
if self.angle < 0 and (100 <= left <= 150):
return self.angle
self.angle = 0
if left == -1:
self.angle = -55
elif left < 17:
self.angle = -48
elif 17 <= left < 50:
self.angle = -19
elif left > 130:
self.angle = 47
elif 130 >= left > 100:
self.angle = 18
#print(left)
return self.angle
def accelerate(self, angle, left, mid, right, p, side_left, side_right, B):
if mid < 40 and side_left < 40 and side_right < 40:
self.backward()
if p < -5:
return 0
if self.speed > 0 and mid == 0:
return self.speed
elif (angle <= -25 or angle >= 35):
self.speed = 37
else:
self.speed = 46
#print(mid)
return 25
def exit(self):
print('finished')
def backward(self):
for i in range(2):
self.driver.drive(90, 90)
time.sleep(0.1)
self.driver.drive(90, 60)
time.sleep(0.1)
for i in range(50):
self.driver.drive(35, 70)
time.sleep(0.1)
def forward(self):
for i in range(30):
self.driver.drive(35, 120)
time.sleep(0.1)
class AutoDrive:
def __init__(self):
rospy.init_node('xycar_driver')
self.line_detector = LineDetector('/usb_cam/image_raw')
self.obstacle_detector = ObstacleDetector('/ultrasonic')
self.driver = MotorDriver('/xycar_motor_msg')
self.imu = ImuRead('diagnostics')
self.before = [0, 0, 0]
self.before_sonic = 0
self.startTime = time.time()
def trace(self):
obs_l, obs_m, obs_r = self.obstacle_detector.get_distance()
line_l, line_r, line_s = self.line_detector.detect_lines()
self.line_detector.show_images(line_l, line_r)
r, p, y = self.imu.get_data()
angle = self.steer(line_l, line_r, r, p, y)
speed = self.accelerate(angle, obs_l, obs_m, obs_r, r, p, y, line_s)
self.driver.drive(angle + 90, speed + 90)
# print(r, p, y, "imu")
#print(self.read)
def steer(self, left, right, r, p, y):
mid = (left + right) // 2
if left == -1 and right == -1:
left, right, mid = self.before[0], self.before[1], self.before[2]
if left > 0 and right > 0 and mid < 150:
left, right, mid = self.before[0], self.before[1], self.before[2]
# if right != -1 and (self.before[2] - mid) > 5:
# mid = (mid + self.before[2]) * 1 // 2 + 5
# print((mid + self.before[2]) * 1 // 2 + 5)
if left == -1:
if mid - 300 < -50:
angle = -50
else:
angle = mid - 300
elif right == -1:
if mid - 15 > 50:
angle = 50
else:
angle = mid - 15
elif mid > 325:
if mid - 325 > 50:
angle = 50
else:
angle = mid - 325
elif mid < 295:
if mid - 295 < -50:
angle = -50
else:
angle = mid - 295
else:
angle = 0
if p > 5:
angle = 0
elif p < -5:
angle = 0
self.before[0], self.before[1], self.before[2] = left, right, mid
return angle
def accelerate(self, angle, left, mid, right, r, p, y, stop):
if angle > 0:
angle = -angle
#if mid < 110:
# speed = 0
if angle < -30 or angle > 30:
speed = 50
elif angle < -10:
speed = 50
else:
speed = 50
if p > 7:
speed = -40
elif p < -7:
speed = 35
print(left, mid, right, self.before_sonic - mid)
self.before_sonic = mid
if stop == 1:
speed = 0
return speed
def exit(self):
print('finished')
class AutoDrive:
def __init__(self):
rospy.init_node('drive')
self.pub = rospy.Publisher('xycar_motor_msg',
Int32MultiArray,
queue_size=1)
self.position = LineDetector()
self.color = ColorDetector()
self.lPos = -1
self.rPos = -1
self.speed = 128
self.imu = ImuRead('/diagnostics')
def exit(self):
print("finished")
def auto_drive(self, Angle, Speed):
drive_info = [Angle, Speed]
drive_info = Int32MultiArray(data=drive_info)
self.pub.publish(drive_info)
def trace(self):
offset = 10
self.pos = self.position.detect_lines()
self.lPos, self.rPos = self.pos[0], self.pos[1]
#self.position.show_images(self.lPos, self.rPos)
if self.lPos > 320:
self.lPos = -1
if self.rPos < 320:
self.rPos = -1
if abs(self.lPos - self.rPos) <= 80:
if self.rPos >= 380:
x_location = 90 - (self.rPos - 400 - offset) * 0.6
if x_location < 40:
x_location = 40
self.auto_drive(x_location, self.speed)
elif self.lPos <= 280:
x_location = 90 + (240 - self.lPos - offset) * 0.6
if x_location > 140:
x_location = 140
self.auto_drive(x_location, self.speed)
if self.lPos == -1:
x_location = (self.rPos - 320 - offset) * 0.6
if x_location < 40:
x_location = 40
self.auto_drive(x_location, self.speed)
elif self.rPos == -1:
x_location = (320 - self.lPos - offset) * 0.6
if x_location < 40:
x_location = 40
self.auto_drive(180 - x_location, self.speed)
elif self.lPos != -1 and self.rPos != -1:
midPos = (self.lPos + self.rPos) // 2
if (midPos >= 320):
x_location = 90 + (midPos - 320) * 0.3
elif (midPos < 320):
x_location = 90 - (320 - midPos) * 0.3
if x_location < 40:
x_location = 40
elif x_location > 140:
x_location = 140
self.auto_drive(x_location, self.speed)
elif self.lPos == -1 and self.rPos == -1:
self.auto_drive(90, 90)
def pitch(self):
r, p, y = self.imu.get_data()
print('R (%.1f) P (%.1f), Y (%.1f)' % (r, p, y))
if abs(p) > 4:
return True
def colorDetect(self):
if self.color.color == 'red': # stop
self.auto_drive(90, 90)
time.sleep(3)
self.auto_drive(90, 115)
time.sleep(2)
self.speed = 125
elif self.color.color == 'yellow': # slow drive
for i in range(30):
self.speed = 115
self.trace()
self.speed = 128
#elif self.color.color == 'pink': # load
#pass
elif self.color.color == 'green': # unload
for i in range(40):
self.auto_drive(120, 115)
time.sleep(0.1)
self.auto_drive(90, 90)
time.sleep(5)
elif self.color.color == 'blue': # oil
for i in range(35):
self.auto_drive(125, 115)
time.sleep(0.1)
for i in range(15):
self.auto_drive(60, 115)
time.sleep(0.1)
time.sleep(10)
for i in range(35):
self.auto_drive(60, 115)
time.sleep(0.1)
for i in range(20):
self.auto_drive(125, 115)
time.sleep(0.1)
self.trace()
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.imu = ImuRead('/diagnostics')
self.information = {"speed": -1, "signal": "", "=>": "", "pitch": -1, "roll": -1, "yaw": -1}
def trace(self):
line_l, line_r, red, yellow, green = self.line_detector.detect_lines()
self.r, self.y, self.g = red, yellow, green
self.line_detector.show_images(line_l, line_r)
self.information["roll"], self.information["pitch"], self.information["yaw"] = self.imu.get_data()
angle = self.steer(line_l, line_r)
speed = self.accelerate(angle, red, yellow, green)
self.information["speed"] = speed + 90
self.driver.drive(angle + 90, speed + 90)
print(speed + 90)
print(r, p, y)
def steer(self, left, right):
if left == -1:
if 320 < right < 390:
angle = -50
else:
angle = (550 - right) / (-3)
elif right == -1:
if 250 < left < 320:
angle = 50
else:
angle = (left - 90) / 3
else:
angle = 0
return angle
def accelerate(self, angle, red, yellow, green):
# if red is detected, stop
if red and (not yellow) and (not green):
speed = 0
# if yellow is detected, go low speed
elif (not red) and yellow and (not green):
speed = 20
# if green is detected, go origin speed
elif (not red) and (not yellow) and green:
speed = 50
# This is origin code
else: # (not red) and (not yellow) and (not green):
if angle <= -40 or angle >= 40:
speed = 30
elif angle <= -25 or angle >= 25:
speed = 40
else:
speed = 50
print(red, yellow, green)
return speed
def printInformation(self):
if self.r:
self.information["signal"] = "red"
self.information["=>"] = "stop!"
if self.y:
self.information["signal"] = "yellow"
self.information["=>"] = "slow"
if self.g:
self.information["signal"] = "green"
self.information["=>"] = "just go"
print("speed : " + str(self.information["speed"]) + " ", "signal : " + str(self.information["signal"]) + " ",
"=> " + str(self.information["=>"]))
print("pitch : " + str(self.information["pitch"]) + " ", "roll : " + str(self.information["roll"]) + " ",
"yaw : " + str(self.information["yaw"]))
print("")
self.r, self.y, self.g = False, False, False
def exit(self):
print('finished')