import cv2
import numpy as np
import math
import time
#import serial
uart_baud = 115200
#ser = serial.Serial("/dev/ttyUSB0", uart_baud)
black_min=(0,43,46)
black_max=(10,255,255)
angle_exp=0
distance_exp=0
#get the mid line of the track
def liner(img,result):
# print(img)
n=[len(img[:,0]),len(img[0,:])]
# print(n,'n')
(x_point,y_point)=np.nonzero(img)
if len(x_point)<2:
return 0
f1 = np.polyfit(x_point, y_point,1)
p1 = np.poly1d(f1)
point1=(int(p1(0)),0)
point2=(int(p1(n[0])),n[0])
#print(point2,point1)
#print((int(n[1]/2),0) , (int(n[1]/2),n[0]))
cv2.line(result, (int(n[1]/2),0) , (int(n[1]/2),n[0]), (0, 0, 255),2)
cv2.line(result,point1,point2,(0,255,0),2)
cv2.imshow('inside',result)
#print(math.atan(f1[0])*180/math.pi)
#print([n[1]/2-p1(n[0]),f1[1]],'ret')
angle_now=math.atan(f1[0])*180/math.pi # i'm not sure, maybe i should add a -...
distance_now = n[1]/2-p1(n[0]) # the same as upper
return [distance_now,angle_now]
#PID control
class Pid():
def __init__(self, exp_val, now_val,kp,kd):
self.KP = kp
self.KD = kd
self.exp_val = exp_val
self.now_val = now_val
#self.sum_err = 0
self.now_err = 0
self.last_err = 0
def cmd_pid(self):
self.last_err = self.now_err
self.now_err = self.exp_val - self.now_val
#self.sum_err += self.now_err
self.now_val = self.KP * (self.exp_val - self.now_val) + self.KD * (self.now_err - self.last_err)
return self.now_val
def int_transform_4hex(str_list_16nums):
str_list_16nums=list(str_list_16nums)
if len(str_list_16nums) == 4:
str_list_16nums.insert(2, ' ')
elif len(str_list_16nums) == 3:
str_list_16nums.insert(0, '0')
str_list_16nums.insert(2, ' ')
elif len(str_list_16nums) == 2:
str_list_16nums.insert(0, '0')
str_list_16nums.insert(0, '0')
str_list_16nums.insert(2, ' ')
elif len(str_list_16nums) == 1:
str_list_16nums.insert(0, '0')
str_list_16nums.insert(0, '0')
str_list_16nums.insert(0, '0')
str_list_16nums.insert(2, ' ')
else:
pass
str_list_16nums = ''.join(str_list_16nums)
return str_list_16nums.upper()
def trans(adjust_speed,rotate_speed):
upper=65535
direction_adjust='00'
direction_rotate='00'
adjust_speed=int(adjust_speed/0.001)
rotate_speed=int(rotate_speed/0.01)
if adjust_speed>0:
direction_adjust = '01' # to judge the direction , if not right please add a -
if rotate_speed>0:
direction_rotate ='01'
adjust_speed=abs(adjust_speed)
rotate_speed=abs(rotate_speed)
if adjust_speed>upper:
adjust_speed=upper
if rotate_speed>upper:
rotate_speed=upper
adjust_trans=hex(adjust_speed).replace('0x','')
rotate_trans=hex(rotate_speed).replace('0x','')
adjust_trans=int_transform_4hex(adjust_trans)
rotate_trans=int_transform_4hex(rotate_trans)
#print(adjust_trans,rotate_trans)
control_cmd='CD 0A 01 00 03 20'+' '+direction_adjust+' '+adjust_trans+' '+direction_rotate+' '+rotate_trans+' 3A'
#print(control_cmd)
return control_cmd
#main func
if __name__ == '__main__':
cap = cv2.VideoCapture(0)
while True:
ret, frame = cap.read()
grid_RGB = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
grid_HSV = cv2.cvtColor(grid_RGB, cv2.COLOR_RGB2HSV)
lower1 = np.array([0,43,46])
upper1 = np.array([10,255,255])
mask1 = cv2.inRange(grid_HSV, lower1, upper1)
res1 = cv2.bitwise_and(grid_RGB, grid_RGB, mask=mask1)
lower2 = np.array([156,43,46])
upper2 = np.array([180,255,255])
mask2 = cv2.inRange(grid_HSV, lower2, upper2)
res2 = cv2.bitwise_and(grid_RGB,grid_RGB, mask=mask2)
mask = mask1 + mask2
#mask=cv2.inRange(frame,black_min,black_max)
kernel=np.ones((3,3),np.uint8)
erode=cv2.morphologyEx(mask,cv2.MORPH_ERODE,kernel)
#cv2.imshow('mask', mask)
#cv2.imshow('erode', erode)
cv2.imshow('frame', frame)
c = cv2.waitKey(1)
control_val=liner(mask,frame)#calculate angle and distance
#no.1 is dis while no.2 is angle
adjust_speed=Pid(distance_exp,control_val[0],0.1,0.15)# expected transverse v to adjust distance , here to adjust
rotate_speed=Pid(angle_exp,control_val[1],0.1,0.15)# expected rotate to adjust angle , here to adjust
control_cmd=trans(adjust_speed.now_val,rotate_speed.now_val)
d=bytes.fromhex(control_cmd)
print(d)
ser.flushInput()
############serial has not sent control_cmd yet
time.sleep(0.05)#control period 50ms
if c == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
