1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119
| import pigpio import time import os import cv2 import numpy as np
show = True
lower = np.array([0, 0, 120]) upper = np.array([105, 85, 255])
y_scan = 150 x_middle = 140 wide_scan = 5 wide_need = 2
sidewalk = True sidewalk_len = 50 motor_before_speed = 12000 motor_stop_pwm = 8000
motor_pin = 20 motor_speed = 13300
servo_pin = 21 servo_middle = 68 servo_min = 60 servo_max = 72
pid_left = 0.05 pid_right = 0.10
pi = pigpio.pi() pi.set_mode(motor_pin, pigpio.OUTPUT) pi.set_PWM_frequency(motor_pin, 200) pi.set_PWM_range(motor_pin, 40000) pi.set_PWM_dutycycle(motor_pin, 10000) time.sleep(1)
pi2 = pigpio.pi() pi2.set_mode(servo_pin, pigpio.OUTPUT) pi2.set_PWM_frequency(servo_pin, 50) pi2.set_PWM_range(servo_pin, 1000) pi2.set_PWM_dutycycle(servo_pin, servo_middle) time.sleep(2)
cap = cv2.VideoCapture(-1) while True: try: ret, frame = cap.read() if frame is None: cap.release() pi.set_PWM_dutycycle(motor_pin, 10000) cap = cv2.VideoCapture(-1) ret, frame = cap.read() continue pi.set_PWM_dutycycle(motor_pin, motor_speed) frame = cv2.resize(frame, (320, 180)) frame_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) frame_hsv = cv2.inRange(frame_hsv, lower, upper) y_line = frame_hsv[y_scan] left_line_x = x_middle right_line_x = x_middle while left_line_x > wide_scan + 1: wide_range = y_line[left_line_x - wide_scan:left_line_x] if list(wide_range).count(255) > wide_need: break left_line_x = left_line_x - 1 while right_line_x < 320 - wide_scan - 1: wide_range = y_line[right_line_x:right_line_x + wide_scan] if list(wide_range).count(255) > wide_need: break right_line_x = right_line_x + 1 loss = (right_line_x + left_line_x) / 2 - x_middle print(loss)
if loss > 0: pi2.set_PWM_dutycycle(servo_pin, servo_middle - pid_right * loss) else: pi2.set_PWM_dutycycle(servo_pin, servo_middle - pid_left * loss) if sidewalk: pi.set_PWM_dutycycle(motor_pin, motor_before_speed) else: pi.set_PWM_dutycycle(motor_pin, motor_speed) print(sidewalk) if sidewalk: white_count = right_line_x - left_line_x print(white_count) if white_count<sidewalk_len: sidewalk = False pi.set_PWM_dutycycle(motor_pin, motor_stop_pwm) time.sleep(3)
if show: cv2.line(frame, (left_line_x, y_scan), (right_line_x, y_scan), (0, 255, 0), 5) cv2.imshow("frame", frame) cv2.imshow("frame_hsv", frame_hsv) if cv2.waitKey(1) == 27: break except KeyboardInterrupt: break
pi.set_PWM_dutycycle(motor_pin, 10000)
|