from picamera2 import Picamera2 
import cv2 
import numpy as np 
import time
import math
import argparse
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BOARD) 
PWM_pin   = 32   #Define pin, frequency and duty cycle 
freq      = 50 
dutyCycle = 7.5 #90 degrees at first#
				#Values 0 - 100 (represents 4%, ~27 deg) 
minv=20
maxv=48
angles=[90]
width=1000
height=1333
rect1=(0,654)  
rect2=(1000,1005) 
x1s=[]
x2s=[]
y1s=[]
y2s=[]
x3s=[]
y3s=[]
GPIO.setup(PWM_pin, GPIO.OUT) # Create PWM instance for pin w freqency 
pwm = GPIO.PWM(PWM_pin, freq) 
# picam2 = Picamera2()    
parser=argparse.ArgumentParser(description="lower and upper bound for trying")
parser.add_argument('--tim',default=2,help="length of time to run")
parser.add_argument('--delay',default=1,help="time between image captures")
parser.add_argument('--debug',action='store_true',default=True,help="debug mode")
args=parser.parse_args()

def get_duty(angle1,angle2):
	duty1=(1/18)*angle1 +2.2
	duty2=duty1+0.4*(angle2-angle1)*(1/18)
	return duty2

def get_points(img,debug):
	
	if img is None:
	    print("Error: Unable to load image.")
	    exit()

	height, width = (img.shape[:2])
	
	def blank(x):  # Null function for trackbar
		pass
	
	
	mask = np.zeros(img.shape[:2], dtype=np.uint8)
	mask = cv2.rectangle(mask,rect1,rect2, 255, -1)
	masked_img = cv2.bitwise_and(img, img, mask=mask)
	
	img=masked_img
	img =cv2.GaussianBlur(img, (3, 3), 0)
	edge = cv2.Canny(img, minv, maxv)
	lines = cv2.HoughLinesP(edge, 1, np.pi / 180, threshold=100, minLineLength=20, maxLineGap=10)

	output = img.copy()  # Avoid modifying the original image
	if lines is not None:  # Check if lines were detected
		for i in range(len(lines)):
			for x1, y1, x2, y2 in lines[i]:
				cv2.line(output, (x1, y1), (x2, y2), (0, 255, 0), 2)
				print(f'x1:{x1:0.2f}\ty1:{y1:0.2f}\tx2:{x2:0.2f}\ty2:{y2:0.2f}')
				x1s.append(x1)
				x2s.append(x1)
				y1s.append(y1)
				y2s.append(y2)
				with open('data.txt', 'a') as f:
					f.write(f'{x1:0.2f}\t{y1:0.2f}\t{x2:0.2f}\t{y2:0.2f}')
					f.write('\n')

	if debug== True:
		# cv2.imshow('image', img)
		# cv2.imshow('mask', mask)
		# cv2.imshow('masked_image', masked_img)
		cv2.imshow("houghline", output)
		cv2.waitKey(5000)
		cv2.destroyAllWindows()  
		
		
	# picam2.stop() 
	return x1s,x2s,y1s,y2s
	
def get_angle(x1s,x2s,y1s,y2s):
	pass
	
	
	
	
	
start_time = time.time() 
cur_time   = start_time 
mesg_time  = start_time  
i=0
while (start_time + args.tim > cur_time):     
	time.sleep(0.001)             
	cur_time = time.time() # some short delay to avoid busy waits
	if (mesg_time + args.delay < cur_time):
		i+=1
		time_1=time.time()
		mesg_time = cur_time
		# picam2.start()
		img = cv2.imread("hallway.jpg") #take picture into memory
		height, width = img.shape[:2]
		scale = 0.5
		new_size  = (int(height*scale), int(width*scale))
		small_img = cv2.resize(img, new_size, interpolation=cv2.INTER_LINEAR) 
		#先不缩小
		# img_RAM_BGR= cv2.cvtColor(img_RAM,cv2.COLOR_RGB2BGR) #change color 
		#from RGB to BGR
		debug=args.debug
		# if debug == True:
			# cv2.imshow("picture",img_RAM_BGR)
			# cv2.waitKey(1000)
			# cv2.destroyAllWindows()
		# else:
			# pass
		# img_test = cv2.imread("bluetape.jpg")
		x1s,x2s,y1s,y2s=get_points(small_img,debug) #call function
		if debug == True:
			time_2=time.time() 
			print(f'{time_2-time_1:0.2f}') #print the time it taken to complete the finding angle process
		angle=get_angle(x1s,x2s,y1s,y2s) #
		# dutyCycle=get_duty(angle1,angle2) #
		# pwm.ChangeDutyCycle(dutyCycle) #
		# if debug ==True:
			# print(f'PWM:{dutyCycle}')
		# else:
			# pass

Quote:/home/ricky/Desktop/lab_deliverable/lab6/hallway.py:26: RuntimeWarning: This channel is already in use, continuing anyway. Use GPIO.setwarnings(False) to disable warnings.
GPIO.setup(PWM_pin, GPIO.OUT) # Create PWM instance for pin w freqency
QStandardPaths: wrong permissions on runtime directory /run/user/1000, 0770 instead of 0700
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
6.66
0.26

scale = 0.5
new_size  = (int(height*scale), int(width*scale))
small_img = cv2.resize(img, new_size, interpolation=cv2.INTER_LINEAR)

new_size  = (int(width*scale), int(height*scale))
	img = cv2.resize(img, new_size, interpolation=cv2.INTER_LINEAR) 

mask = cv2.rectangle(mask,rect1,rect2, 255, -1)